From c5f53fc0b6e4be440b9ca0c8d89bccc34ab1d544 Mon Sep 17 00:00:00 2001
From: Hwurzburg <hwurzburg@yahoo.com>
Date: Tue, 13 Jul 2021 11:45:08 -0500
Subject: [PATCH] AP_RCProtocol: add Link Quality reporting to RC protocols

---
 libraries/AP_RCProtocol/AP_RCProtocol.cpp         | 8 +++++++-
 libraries/AP_RCProtocol/AP_RCProtocol.h           | 1 +
 libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp | 3 ++-
 libraries/AP_RCProtocol/AP_RCProtocol_Backend.h   | 7 +++++--
 libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp    | 3 ++-
 libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h      | 1 +
 6 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
index a722b0f59d..1744146201 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp
@@ -351,7 +351,13 @@ int16_t AP_RCProtocol::get_RSSI(void) const
     }
     return -1;
 }
-
+int16_t AP_RCProtocol::get_rx_link_quality(void) const
+{
+    if (_detected_protocol != AP_RCProtocol::NONE) {
+        return backend[_detected_protocol]->get_rx_link_quality();
+    }
+    return -1;
+}
 /*
   ask for bind start on supported receivers (eg spektrum satellite)
  */
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h
index 2a1575ffa5..ed084dcb70 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol.h
@@ -70,6 +70,7 @@ public:
     bool new_input();
     void start_bind(void);
     int16_t get_RSSI(void) const;
+    int16_t get_rx_link_quality(void) const;
 
     // return protocol name as a string
     static const char *protocol_name_from_protocol(rcprotocol_t protocol);
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
index 85e5a44c0c..abfe7501ad 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp
@@ -58,7 +58,7 @@ void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
 /*
   provide input from a backend
  */
-void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi)
+void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi, int16_t _rx_link_quality)
 {
     num_values = MIN(num_values, MAX_RCIN_CHANNELS);
     memcpy(_pwm_values, values, num_values*sizeof(uint16_t));
@@ -73,6 +73,7 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool
         rc_input_count++;
     }
     rssi = _rssi;
+    rx_link_quality = _rx_link_quality;
 }
 
 
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
index 19f3e503ea..1dc03a6c72 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h
@@ -67,7 +67,9 @@ public:
     int16_t get_RSSI(void) const {
         return rssi;
     }
-
+    int16_t get_rx_link_quality(void) const {
+        return rx_link_quality;
+    }
     // get UART for RCIN, if available. This will return false if we
     // aren't getting the active RC input protocol via the uart
     AP_HAL::UARTDriver *get_UART(void) const {
@@ -99,7 +101,7 @@ protected:
         uint32_t ch7 : 11;
     } PACKED;
 
-    void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1);
+    void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1, int16_t rx_link_quality=-1);
     AP_RCProtocol &frontend;
 
     void log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const;
@@ -115,4 +117,5 @@ private:
     uint16_t _pwm_values[MAX_RCIN_CHANNELS];
     uint8_t  _num_channels;
     int16_t rssi = -1;
+    int16_t rx_link_quality = -1;
 };
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
index 71e8847e75..6e069aa091 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
@@ -204,7 +204,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, _link_status.rssi);
+            add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality);
         }
     }
 }
@@ -341,6 +341,7 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
     } else {
         rssi_dbm = link->uplink_rssi_ant2;
     }
+    _link_status.link_quality = link->uplink_status;
      // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
     if (rssi_dbm < 50) {
         _link_status.rssi = 255;
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
index 3e217b9314..03736b458b 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
@@ -181,6 +181,7 @@ public:
 
     struct LinkStatus {
         int16_t rssi = -1;
+        int16_t link_quality = -1;
         RFMode rf_mode;
     };