From ce970dcf75aa79332869ef85580b796c1b0ba1f3 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 15 Jul 2020 20:26:28 +0100
Subject: [PATCH] AP_RCProtocol: process CRSF link statistics frames to get
 RSSI

---
 .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp      | 27 ++++++++++++++++++-
 libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h  | 15 +++++++++++
 2 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
index 9bad69933c..737f077dd4 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
@@ -206,7 +206,7 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
         _last_frame_time_us = timestamp_us;
         // decode here
         if (decode_csrf_packet()) {
-            add_input(MAX_CHANNELS, _channels, false, -1);
+            add_input(MAX_CHANNELS, _channels, false, _current_rssi);
         }
     }    
 }
@@ -284,6 +284,9 @@ bool AP_RCProtocol_CRSF::decode_csrf_packet()
             decode_11bit_channels((const uint8_t*)(&_frame.payload), CRSF_MAX_CHANNELS, _channels, 5U, 8U, 880U);
             rc_active = !_uart; // only accept RC data if we are not in standalone mode
             break;
+        case CRSF_FRAMETYPE_LINK_STATISTICS:
+            process_link_stats_frame((uint8_t*)&_frame.payload);
+            break;
         default:
             break;
     }
@@ -337,6 +340,28 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
     return true;
 }
 
+// process link statistics to get RSSI
+void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
+{
+    const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data;
+
+    uint8_t rssi_dbm;
+    if (link->active_antenna == 0) {
+        rssi_dbm = link->uplink_rssi_ant1;
+    } else {
+        rssi_dbm = link->uplink_rssi_ant2;
+    }
+     // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
+    if (rssi_dbm < 50) {
+        _current_rssi = 255;
+    } else if (rssi_dbm > 120) {
+        _current_rssi = 0;
+    } else {
+        // this is an approximation recommended by Remo from TBS
+        _current_rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
+    }
+}
+
 // process a byte provided by a uart
 void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate)
 {
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
index 327c06b16b..d2b7c32299 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
@@ -148,6 +148,19 @@ public:
         uint8_t payload[CRSF_FRAMELEN_MAX - 3]; // +1 for crc
     } PACKED;
 
+    struct LinkStatisticsFrame {
+        uint8_t uplink_rssi_ant1; // ( dBm * -1 )
+        uint8_t uplink_rssi_ant2; // ( dBm * -1 )
+        uint8_t uplink_status; // Package success rate / Link quality ( % )
+        int8_t uplink_snr; // ( db )
+        uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
+        uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
+        uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
+        uint8_t downlink_rssi; // ( dBm * -1 )
+        uint8_t downlink_status; // Downlink package success rate / Link quality ( % )
+        int8_t downlink_dnr; // ( db )
+    } PACKED;
+
 private:
     struct Frame _frame;
     struct Frame _telemetry_frame;
@@ -160,6 +173,7 @@ private:
     void _process_byte(uint32_t timestamp_us, uint8_t byte);
     bool decode_csrf_packet();
     bool process_telemetry(bool check_constraint = true);
+    void process_link_stats_frame(const void* data);
     void write_frame(Frame* frame);
     void start_uart();
     AP_HAL::UARTDriver* get_current_UART() { return (_uart ? _uart : get_available_UART()); }
@@ -174,6 +188,7 @@ private:
     uint32_t _start_frame_time_us;
     bool telem_available;
     bool _fast_telem; // is 150Hz telemetry active
+    int16_t _current_rssi = -1;
 
     AP_HAL::UARTDriver *_uart;