Modify GCS_Common to recieve messages from rx-er

uci-overRID · Jan 19, 2024 · 0309270 · 0309270
1 parent 9af642e
commit 0309270
Show file tree

Hide file tree

Showing 5 changed files with 67 additions and 3 deletions.
diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp b/libraries/AP_ODIDScanner/AP_ODIDScanner.cpp
@@ -1,15 +1,33 @@
 #include "AP_ODIDScanner.h"
 #include "GCS_MAVLink/GCS_MAVLink.h"
 #include "GCS_MAVLink/GCS.h"
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_GPS/AP_GPS.h>
+#include <AP_Baro/AP_Baro.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_Parachute/AP_Parachute.h>
+#include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_DroneCAN/AP_DroneCAN.h>
+#include <stdio.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Param/AP_Param.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_Common/Location.h>
 
 // TODO: Random default for mav_port needs fix
-AP_ODIDScanner::AP_ODIDScanner() : _mav_port(7){
+AP_ODIDScanner::AP_ODIDScanner() : _mav_port(1){
 
 }
 void AP_ODIDScanner::init() {
-    _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
+   _chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
     _initialised = true;
     _port = AP::serialmanager().get_serial_by_id(_mav_port);
+    if (_port != nullptr) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Found RID Device");
+        _port->begin(57600, 512, 512);
+    }
 }
 
 void AP_ODIDScanner::update_recv() {
@@ -18,15 +36,24 @@ void AP_ODIDScanner::update_recv() {
     uint32_t now_ms = AP_HAL::millis();
 
     status.packet_rx_drop_count = 0;
+    if(_port == nullptr) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"_port is null");
+        return;
+    } 
 
     const uint16_t nbytes = _port->available();
+    if (nbytes > 0) {
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"available Bytes: %d", nbytes);
+    }
     for (uint16_t i=0; i<nbytes; i++)
     {
         const uint8_t c = (uint8_t)_port->read();
         if (mavlink_frame_char_buffer(channel_buffer(), channel_status(), c, &msg, &status) == MAVLINK_FRAMING_OK) {
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Found message");
             switch(msg.msgid) {
                 case MAVLINK_MSG_ID_HEARTBEAT: 
                 {
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Recv'd heartbeat");
                     mavlink_heartbeat_t packet;
                     mavlink_msg_heartbeat_decode(&msg, &packet);
                     last_dev_hb_ms = now_ms;
@@ -35,10 +62,38 @@ void AP_ODIDScanner::update_recv() {
         }
     }
 }
+
+void AP_ODIDScanner::handle_msg(mavlink_message_t msg) {
+    static int i = 0;
+    switch(msg.msgid) {
+        case MAVLINK_MSG_ID_UAV_FOUND: 
+        {
+            // mavlink_msg_uav_found_t uav;
+            // mavlink_msg_uav_found_decode(&msg, &uav);
+            i+=1;
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ODIDScanner: uav's here %d", i);
+            break;
+        }
+    }
+}
 void AP_ODIDScanner::update() {
     const uint32_t now_ms = AP_HAL::millis();
     if (now_ms - last_dev_hb_ms > 5000 && now_ms - last_dev_hb_msg_ms > 5000) {
         last_dev_hb_msg_ms = now_ms;
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Scanner: Device Not Found");
+        _port->printf("Scanner: Where is this printing?");
     }
+    if (now_ms - last_hb_send_ms > 1000) {
+        _port->printf("Scanner: HB Where is this printing?");
+
+        last_hb_send_ms = now_ms;
+    mavlink_msg_heartbeat_send(
+        _chan,
+        gcs().frame_type(),
+        MAV_AUTOPILOT_ARDUPILOTMEGA,
+        0,
+        gcs().custom_mode(),
+        0);
+        } 
 }
+
diff --git a/libraries/AP_ODIDScanner/AP_ODIDScanner.h b/libraries/AP_ODIDScanner/AP_ODIDScanner.h
@@ -22,14 +22,17 @@ class AP_ODIDScanner
     void init();
     void update();
     void update_recv();
-    mavlink_channel_t _chan; // mavlink channel that communicates with the remote id transceiver
+    void handle_msg(mavlink_message_t);
+    // mavlink_channel_t _chan; // mavlink channel that communicates with the remote id transceiver
     uint8_t _mav_port;
     mavlink_uav_found_t found_msg;
+    mavlink_channel_t _chan;
     AP_HAL::UARTDriver* _port;
     bool _initialised;
     uint32_t last_send_ms;
     uint32_t last_dev_hb_ms;
     uint32_t last_dev_hb_msg_ms;
+    uint32_t last_hb_send_ms;
 
     mavlink_message_t _channel_buffer;
     mavlink_status_t _channel_status;

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -396,6 +396,7 @@ void AP_Vehicle::setup()
 #if AP_OPENDRONEID_ENABLED
     opendroneid.init();
 #endif
+    odidscanner.init();
 
 // init EFI monitoring
 #if HAL_EFI_ENABLED

diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -347,6 +347,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     AP_OpenDroneID opendroneid;
 #endif
 
+public:
     AP_ODIDScanner odidscanner;
 
 #if HAL_MSP_ENABLED

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -4171,6 +4171,10 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
         AP::opendroneid().handle_msg(chan, msg);
         break;
 #endif
+    case MAVLINK_MSG_ID_UAV_FOUND:
+        AP::vehicle()->odidscanner.handle_msg(msg);
+        break;
+
 
 #if AP_SIGNED_FIRMWARE
     case MAVLINK_MSG_ID_SECURE_COMMAND:
-Original file line number
+Diff line change
@@ Expand Up / @@ -347,6 +347,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@
         AP_OpenDroneID opendroneid;
     #endif
+    public:
         AP_ODIDScanner odidscanner;
     #if HAL_MSP_ENABLED
@@ Expand Down @@