AP_RCProtocol: add support for some extra stats to DroneCAN RC

ArduPilot · Oct 13, 2024 · c6e526a · c6e526a
1 parent c3affa4
commit c6e526a
Show file tree

Hide file tree

Showing 4 changed files with 67 additions and 2 deletions.
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -250,6 +250,17 @@ class AP_RCProtocol {
     void handle_radio_rc_channels(const mavlink_radio_rc_channels_t* packet);
 #endif
 
+    // some backends have a struct LinkStatus and a field _link_status, avoid name clash
+    struct RcLinkStatus {
+        int16_t link_quality = -1;
+        int8_t rssi_dbm = -1;
+        int8_t snr = INT8_MIN;
+        int8_t tx_power = -1;
+    };
+    volatile struct RcLinkStatus _rc_link_status;
+
+    const volatile RcLinkStatus& get_link_status() const { return _rc_link_status; }
+
 private:
     void check_added_uart(void);
 

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
@@ -632,6 +632,14 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
         _link_status.active_antenna = -1;
     }
 #endif
+
+    // Report to frontend
+    frontend._rc_link_status.link_quality = _link_status.link_quality;
+#if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED
+    frontend._rc_link_status.rssi_dbm = _link_status.rssi_dbm;
+    frontend._rc_link_status.snr = _link_status.snr;
+    frontend._rc_link_status.tx_power = _link_status.tx_power;
+#endif
 }
 
 // process link statistics to get RX RSSI

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp
@@ -90,12 +90,47 @@ void AP_RCProtocol_DroneCAN::update()
         }
         last_receive_ms = rcin.last_sample_time_ms;
 
+        if (rcin.bits.QUALITY_VALID) {
+            switch (rcin.bits.QUALITY_TYPE) {
+                case QualityType::RSSI:
+                    rssi = rcin.quality;
+                    break;
+                case QualityType::LQ:
+                    frontend._rc_link_status.link_quality = rcin.quality;
+                    break;
+                case QualityType::RSSI_DBM:
+                    frontend._rc_link_status.rssi_dbm = rcin.quality;
+                    // also set the rssi field, this avoids having to waste a slot for sending both
+                    // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
+                    if (rcin.quality < 50) {
+                        rssi = 255;
+                    } else if (rcin.quality > 120) {
+                        rssi = 0;
+                    } else {
+                        rssi = int16_t(roundf((120.0f - rcin.quality) * (255.0f / 70.0f)));
+                    }
+                    break;
+                case QualityType::SNR:
+                    frontend._rc_link_status.snr = (int8_t)rcin.quality - 128;
+                    break;
+                case QualityType::TX_POWER:
+                    frontend._rc_link_status.tx_power = rcin.quality;
+                    break;
+            }
+        } else {
+            rssi = -1;
+            frontend._rc_link_status.link_quality = -1;
+            frontend._rc_link_status.rssi_dbm = -1;
+            frontend._rc_link_status.snr = INT8_MIN;
+            frontend._rc_link_status.tx_power = -1;
+        }
+
         add_input(
             rcin.num_channels,
             rcin.channels,
             rcin.bits.FAILSAFE,
-            rcin.bits.QUALITY_VALID ? rcin.quality : 0,  // CHECK ME
-            0  // link quality
+            rssi,
+            frontend._rc_link_status.link_quality
             );
     }
 }

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h
@@ -30,13 +30,22 @@ class AP_RCProtocol_DroneCAN : public AP_RCProtocol_Backend {
 
     static AP_RCProtocol_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
 
+    enum QualityType {
+        RSSI = 0,
+        LQ = 1,
+        RSSI_DBM = 2,
+        SNR = 3,
+        TX_POWER = 4
+    };
+
     struct {
         uint8_t quality;
         union {
             uint16_t status;
             struct {
                 uint8_t QUALITY_VALID : 1;
                 uint8_t FAILSAFE : 1;
+                uint8_t QUALITY_TYPE : 3;
             } bits;
         };
         uint8_t num_channels;
@@ -46,6 +55,8 @@ class AP_RCProtocol_DroneCAN : public AP_RCProtocol_Backend {
         HAL_Semaphore sem;
     } rcin;
 
+    int16_t rssi = -1;
+
     // Module Detection Registry
     static struct Registry {
         struct DetectedDevice {