From 7b429fdb179ac935536f6c2fb64cfd1d42f9f629 Mon Sep 17 00:00:00 2001
From: olliw42 <waldmanns@gmx.de>
Date: Tue, 15 Oct 2024 18:33:29 +0200
Subject: [PATCH] AP_RCProtocol: add support for some extra stats to DroneCAN
 RC

---
 libraries/AP_RCProtocol/AP_RCProtocol.h       | 12 +++++
 .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp      |  9 ++++
 .../AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp  | 44 ++++++++++++++++++-
 .../AP_RCProtocol/AP_RCProtocol_DroneCAN.h    | 11 +++++
 4 files changed, 74 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
index 37f46a28da6d1..44bf2fd1ec5ec 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -250,6 +250,18 @@ 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;
+        int16_t tx_power = -1;
+        int8_t rssi_dbm = -1;
+        int8_t snr = INT8_MIN;
+        int8_t active_antenna = -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
index 9372946a6a71c..790349f549e9d 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
@@ -632,6 +632,15 @@ 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;
+    frontend._rc_link_status.active_antenna = _link_status.active_antenna;
+#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
index 879344e3236ce..455bac537680c 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.cpp
@@ -90,12 +90,52 @@ 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_ACTIVE_ANTENNA:
+                    // highest bit carries active antenna data
+                    frontend._rc_link_status.link_quality = (rcin.quality & 0x7F);
+                    frontend._rc_link_status.active_antenna = (rcin.quality & 0x80) ? 1 : 0;
+                    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:
+                    // SNR is shifted by 128 to support negative values
+                    frontend._rc_link_status.snr = (int8_t)rcin.quality - 128;
+                    break;
+                case QualityType::TX_POWER:
+                    // carries tx power in units of 5 mW, thus can't support higher than 255*5 = 1275 mW
+                    frontend._rc_link_status.tx_power = (int16_t)rcin.quality * 5;
+                    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;
+            frontend._rc_link_status.active_antenna = -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
index d2c81025d76c6..732a1e76875d2 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_DroneCAN.h
@@ -30,6 +30,14 @@ 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_ACTIVE_ANTENNA = 1,
+        RSSI_DBM = 2,
+        SNR = 3,
+        TX_POWER = 4
+    };
+
     struct {
         uint8_t quality;
         union {
@@ -37,6 +45,7 @@ class AP_RCProtocol_DroneCAN : public AP_RCProtocol_Backend {
             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 {