added option to print msg debug on mavlink

ArduPilot · Oct 16, 2023 · 08e54ca · 08e54ca
1 parent bd17dcf
commit 08e54ca
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 8 deletions.
diff --git a/RemoteIDModule/DroneCAN.cpp b/RemoteIDModule/DroneCAN.cpp
@@ -153,28 +153,28 @@ void DroneCAN::onTransferReceived(CanardInstance* ins,
         handle_get_node_info(ins, transfer);
         break;
     case UAVCAN_PROTOCOL_RESTARTNODE_ID:
-        Serial.printf("RestartNode\n");
+        Serial.printf("DroneCAN: restartNode\n");
         delay(20);
         esp_restart();
         break;
     case DRONECAN_REMOTEID_BASICID_ID:
-        Serial.printf("Got BasicID\n");
+        Serial.printf("DroneCAN: got BasicID\n");
         handle_BasicID(transfer);
         break;
     case DRONECAN_REMOTEID_LOCATION_ID:
-        Serial.printf("Got Location\n");
+        Serial.printf("DroneCAN: got Location\n");
         handle_Location(transfer);
         break;
     case DRONECAN_REMOTEID_SELFID_ID:
-        Serial.printf("Got SelfID\n");
+        Serial.printf("DroneCAN: got SelfID\n");
         handle_SelfID(transfer);
         break;
     case DRONECAN_REMOTEID_SYSTEM_ID:
-        Serial.printf("Got System\n");
+        Serial.printf("DroneCAN: got System\n");
         handle_System(transfer);
         break;
     case DRONECAN_REMOTEID_OPERATORID_ID:
-        Serial.printf("Got OperatorID\n");
+        Serial.printf("DroneCAN: got OperatorID\n");
         handle_OperatorID(transfer);
         break;
     case UAVCAN_PROTOCOL_PARAM_GETSET_ID:

diff --git a/RemoteIDModule/mavlink.cpp b/RemoteIDModule/mavlink.cpp
@@ -144,6 +144,9 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION: {
         mavlink_msg_open_drone_id_location_decode(&msg, &location);
+        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
+            Serial.printf("MAVLink: got Location\n");
+        }
         if (last_location_timestamp != location.timestamp) {
             //only update the timestamp if we receive information with a different timestamp
             last_location_ms = millis();
@@ -154,6 +157,9 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID: {
         mavlink_open_drone_id_basic_id_t basic_id_tmp;
+        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
+            Serial.printf("MAVLink: got BasicID\n");
+        }
         mavlink_msg_open_drone_id_basic_id_decode(&msg, &basic_id_tmp);
         if ((strlen((const char*) basic_id_tmp.uas_id) > 0) && (basic_id_tmp.id_type > 0) && (basic_id_tmp.id_type <= MAV_ODID_ID_TYPE_SPECIFIC_SESSION_ID)) {
             //only update if we receive valid data
@@ -164,15 +170,24 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION: {
         mavlink_msg_open_drone_id_authentication_decode(&msg, &authentication);
+        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
+            Serial.printf("MAVLink: got Auth\n");
+        }
         break;
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID: {
         mavlink_msg_open_drone_id_self_id_decode(&msg, &self_id);
+        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
+            Serial.printf("MAVLink: got SelfID\n");
+        }
         last_self_id_ms = now_ms;
         break;
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM: {
         mavlink_msg_open_drone_id_system_decode(&msg, &system);
+        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
+            Serial.printf("MAVLink: got System\n");
+        }
         if ((last_system_timestamp != system.timestamp) || (system.timestamp == 0)) {
             //only update the timestamp if we receive information with a different timestamp
             last_system_ms = millis();
@@ -181,6 +196,9 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
         break;
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE: {
+        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
+            Serial.printf("MAVLink: got System update\n");
+        }
         mavlink_open_drone_id_system_update_t pkt_system_update;
         mavlink_msg_open_drone_id_system_update_decode(&msg, &pkt_system_update);
         system.operator_latitude = pkt_system_update.operator_latitude;
@@ -201,6 +219,9 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
     }
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID: {
         mavlink_msg_open_drone_id_operator_id_decode(&msg, &operator_id);
+        if (g.options & OPTIONS_PRINT_RID_MAVLINK) {
+            Serial.printf("MAVLink: got OperatorID\n");
+        }
         last_operator_id_ms = now_ms;
         break;
     }

diff --git a/RemoteIDModule/parameters.h b/RemoteIDModule/parameters.h
@@ -107,7 +107,8 @@ class Parameters {
 };
 
 // bits for OPTIONS parameter
-#define OPTIONS_FORCE_ARM_OK 1U<<0
-#define OPTIONS_DONT_SAVE_BASIC_ID_TO_PARAMETERS 1U<<1
+#define OPTIONS_FORCE_ARM_OK (1U<<0)
+#define OPTIONS_DONT_SAVE_BASIC_ID_TO_PARAMETERS (1U<<1)
+#define OPTIONS_PRINT_RID_MAVLINK (1U<<2)
 
 extern Parameters g;