Handle rx with mavlink structs

uci-overRID · Feb 11, 2024 · 3cdde17 · 3cdde17
1 parent 75cac10
commit 3cdde17
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 1 deletion.
diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp b/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp
@@ -78,6 +78,13 @@ void AP_ODIDScanner::handle_msg(mavlink_message_t msg) {
             last_dev_hb_ms = now_ms;
             break;
         }
+        case MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION:
+        {
+            mavlink_open_drone_id_location_t loc;
+            mavlink_msg_open_drone_id_location_decode(&msg, &loc);
+            // Handle the location message.
+            break;
+        }
     }
 }
 void AP_ODIDScanner::update() {
@@ -100,3 +107,6 @@ void AP_ODIDScanner::update() {
         } 
 }
 
+bool AP_ODIDScanner::message_from_rx(mavlink_channel_t& chan) {
+    return chan == _chan;
+}
diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner.h b/libraries/AP_ODIDScanner/AP_ODIDScanner.h
@@ -25,6 +25,8 @@ class AP_ODIDScanner
     void handle_msg(mavlink_message_t);
     // mavlink_channel_t _chan; // mavlink channel that communicates with the remote id transceiver
     uint8_t _mav_port;
+    bool message_from_rx(mavlink_channel_t& chan);
+
     mavlink_uav_found_t found_msg;
     mavlink_channel_t _chan;
     AP_HAL::UARTDriver* _port;

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -4168,7 +4168,12 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID:
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM:
     case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE:
-        AP::opendroneid().handle_msg(chan, msg);
+        if(AP::vehicle()->odidscanner.message_from_rx(chan)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Detected msg from odid-rx");
+            AP::vehicle()->odidscanner.handle_msg(msg);
+        } else {
+            AP::opendroneid().handle_msg(chan, msg);
+        }
         break;
 #endif
     case MAVLINK_MSG_ID_UAV_FOUND: