AP_RCProtocol: add support for some extra stats to DroneCAN RC

ArduPilot · Oct 12, 2024 · a8a1851 · a8a1851
1 parent c3affa4
commit a8a1851
Show file tree

Hide file tree

Showing 4 changed files with 50 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 rssi = -1;
+        int16_t link_quality = -1;
+        int8_t rssi_dbm = -1;
+        int8_t snr = INT8_MIN;
+    };
+    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.rssi = _link_status.rssi;
+    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;
+#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,38 @@ void AP_RCProtocol_DroneCAN::update()
         }
         last_receive_ms = rcin.last_sample_time_ms;
 
+        if (rcin.bits.QUALITY_VALID) {
+            frontend._rc_link_status.rssi = rcin.quality;
+        } else
+        if (rcin.bits.QUALITY_LQ) {
+            frontend._rc_link_status.link_quality = rcin.quality;
+        } else
+        if (rcin.bits.QUALITY_RSSI_DBM) {
+            frontend._rc_link_status.rssi_dbm = rcin.quality;
+            // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
+            if ( rcin.quality < 50) {
+                frontend._rc_link_status.rssi = 255;
+            } else if ( rcin.quality > 120) {
+                frontend._rc_link_status.rssi = 0;
+            } else {
+                frontend._rc_link_status.rssi = int16_t(roundf((120.0f - rcin.quality) * (255.0f / 70.0f) ));
+            }
+        } else
+        if (rcin.bits.QUALITY_SNR) {
+            frontend._rc_link_status.snr = (int8_t)rcin.quality - 128;
+        } else {
+            frontend._rc_link_status.rssi = -1;
+            frontend._rc_link_status.link_quality = -1;
+            frontend._rc_link_status.rssi_dbm = -1;
+            frontend._rc_link_status.snr = INT8_MIN;
+        }
+
         add_input(
             rcin.num_channels,
             rcin.channels,
             rcin.bits.FAILSAFE,
-            rcin.bits.QUALITY_VALID ? rcin.quality : 0,  // CHECK ME
-            0  // link quality
+            frontend._rc_link_status.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
@@ -37,6 +37,9 @@ class AP_RCProtocol_DroneCAN : public AP_RCProtocol_Backend {
             struct {
                 uint8_t QUALITY_VALID : 1;
                 uint8_t FAILSAFE : 1;
+                uint8_t QUALITY_LQ : 1;
+                uint8_t QUALITY_RSSI_DBM : 1;
+                uint8_t QUALITY_SNR : 1;
             } bits;
         };
         uint8_t num_channels;