ArduPilot · StewLG · Mar 23, 2017 · Jun 19, 2017 · Jun 19, 2017
diff --git a/libraries/AP_HAL/RCInput.h b/libraries/AP_HAL/RCInput.h
@@ -26,6 +26,10 @@ class AP_HAL::RCInput {
      */
     virtual uint8_t  num_channels() = 0;
 
+    /* Return the Link Quality of the RC link. 0.0 = worst quality, 1.0 = best quality. 
+       Some platforms may not support this and will return 0.0 always. */
+    virtual float link_quality() = 0;    
+
     /* Read a single channel at a time */
     virtual uint16_t read(uint8_t ch) = 0;
 

diff --git a/libraries/AP_HAL_Empty/RCInput.cpp b/libraries/AP_HAL_Empty/RCInput.cpp
@@ -16,6 +16,10 @@ uint8_t RCInput::num_channels() {
     return 0;
 }
 
+float RCInput::link_quality() {
+    return 0.0f;
+}
+
 uint16_t RCInput::read(uint8_t ch) {
     if (ch == 2) return 900; /* throttle should be low, for safety */
     else return 1500;

diff --git a/libraries/AP_HAL_Empty/RCInput.h b/libraries/AP_HAL_Empty/RCInput.h
@@ -8,6 +8,7 @@ class Empty::RCInput : public AP_HAL::RCInput {
     void init();
     bool  new_input();
     uint8_t num_channels();
+    float link_quality();
     uint16_t read(uint8_t ch);
     uint8_t read(uint16_t* periods, uint8_t len);
 

diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp
@@ -48,6 +48,12 @@ uint8_t RCInput::num_channels()
     return _num_channels;
 }
 
+/* Return the Link Quality of the RC link. 0.0 = worst quality, 1.0 = best quality. */
+float RCInput::link_quality() {
+    // Unimplemented
+    return 0.0f;
+}
+
 uint16_t RCInput::read(uint8_t ch)
 {
     if (_override[ch]) {

diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h
@@ -18,6 +18,7 @@ class RCInput : public AP_HAL::RCInput {
 
     virtual void init();
     bool new_input();
+    float link_quality();    
     uint8_t num_channels();
     uint16_t read(uint8_t ch);
     uint8_t read(uint16_t* periods, uint8_t len);

diff --git a/libraries/AP_HAL_PX4/RCInput.cpp b/libraries/AP_HAL_PX4/RCInput.cpp
@@ -49,6 +49,14 @@ uint8_t PX4RCInput::num_channels()
     return n;
 }
 
+float PX4RCInput::link_quality() 
+{
+    pthread_mutex_lock(&rcin_mutex);
+    float current_link_quality = _rcin.link_quality;
+    pthread_mutex_unlock(&rcin_mutex);
+    return current_link_quality;
+}
+
 uint16_t PX4RCInput::read(uint8_t ch)
 {
     if (ch >= RC_INPUT_MAX_CHANNELS) {

diff --git a/libraries/AP_HAL_PX4/RCInput.h b/libraries/AP_HAL_PX4/RCInput.h
@@ -15,6 +15,7 @@ class PX4::PX4RCInput : public AP_HAL::RCInput {
     void init() override;
     bool new_input() override;
     uint8_t num_channels() override;
+    float link_quality() override;
     uint16_t read(uint8_t ch) override;
     uint8_t read(uint16_t* periods, uint8_t len) override;
 

diff --git a/libraries/AP_HAL_SITL/RCInput.h b/libraries/AP_HAL_SITL/RCInput.h
@@ -15,6 +15,10 @@ class HALSITL::RCInput : public AP_HAL::RCInput {
     uint8_t num_channels() override {
         return SITL_RC_INPUT_CHANNELS;
     }
+    float link_quality() override {
+        // Always perfect link in the simulator
+        return 1.0f;
+    }    
     uint16_t read(uint8_t ch) override;
     uint8_t read(uint16_t* periods, uint8_t len) override;
 

diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.cpp b/libraries/AP_HAL_VRBRAIN/RCInput.cpp
@@ -41,6 +41,12 @@ uint8_t VRBRAINRCInput::num_channels()
     return n;
 }
 
+float VRBRAINRCInput::link_quality() 
+{
+    // Unimplemented
+    return 0.0f;
+}
+
 uint16_t VRBRAINRCInput::read(uint8_t ch)
 {
 	if (ch >= RC_INPUT_MAX_CHANNELS) {

diff --git a/libraries/AP_HAL_VRBRAIN/RCInput.h b/libraries/AP_HAL_VRBRAIN/RCInput.h
@@ -15,6 +15,7 @@ class VRBRAIN::VRBRAINRCInput : public AP_HAL::RCInput {
     void init();
     bool new_input();
     uint8_t num_channels();
+    float link_quality();    
     uint16_t read(uint8_t ch);
     uint8_t read(uint16_t* periods, uint8_t len);
 

diff --git a/libraries/AP_RSSI/AP_RSSI.cpp b/libraries/AP_RSSI/AP_RSSI.cpp
@@ -34,7 +34,7 @@ const AP_Param::GroupInfo AP_RSSI::var_info[] = {
     // @Param: TYPE
     // @DisplayName: RSSI Type
     // @Description: Radio Receiver RSSI type. If your radio receiver supports RSSI of some kind, set it here, then set its associated RSSI_XXXXX parameters, if any.
-    // @Values: 0:Disabled,1:AnalogPin,2:RCChannelPwmValue
+    // @Values: 0:Disabled,1:AnalogPin,2:RCChannelPwmValue,3:SBUSLinkQuality (Pixhawk only)
     // @User: Standard
     AP_GROUPINFO("TYPE", 0, AP_RSSI, rssi_type,  BOARD_RSSI_DEFAULT),
 
@@ -86,6 +86,24 @@ const AP_Param::GroupInfo AP_RSSI::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("CHAN_HIGH", 6, AP_RSSI, rssi_channel_high_pwm_value,  2000),
 
+    // @Param: RSSI_SBUS_LOW
+    // @DisplayName: Receiver SBUS Link Quality low value
+    // @Description: The lowest value expected from the SBUS Link Quality metric. This is normally 0, which would happen when there is no signal.
+    // @Units: Percent
+    // @Increment: 1
+    // @Range: 0 100
+    // @User: Standard
+    AP_GROUPINFO("SBUS_LOW", 7, AP_RSSI, rssi_sbus_low_value, 0),
+
+    // @Param: RSSI_SBUS_HIGH
+    // @DisplayName: Receiver SBUS Link Quality high value
+    // @Description: The lowest value expected from the SBUS Link Quality metric. This can be as high as 100, but may be lower depending on your particular hardware, since a certain level of frames dropped may be normal even at short distances.
+    // @Units: Percent
+    // @Increment: 1
+    // @Range: 0 100
+    // @User: Standard
+    AP_GROUPINFO("SBUS_HIGH", 8, AP_RSSI, rssi_sbus_high_value, 100),
+
     AP_GROUPEND
 };
 
@@ -128,6 +146,9 @@ float AP_RSSI::read_receiver_rssi()
         case RssiType::RSSI_RC_CHANNEL_VALUE :
             receiver_rssi = read_channel_rssi();
             break;
+        case RssiType::SBUS_LINK_QUALITY :
+            receiver_rssi = read_sbus_link_quality();
+            break;              
         default :   
             receiver_rssi = 0.0f;
             break;
@@ -163,6 +184,13 @@ float AP_RSSI::read_channel_rssi()
     return channel_rssi;    
 }
 
+// read the SBUS Link Quality
+float AP_RSSI::read_sbus_link_quality()
+{
+    float current_link_quality = hal.rcin->link_quality() * 100;
+    return scale_and_constrain_float_rssi(current_link_quality, rssi_sbus_low_value, rssi_sbus_high_value);
+}
+
 // Scale and constrain a float rssi value to 0.0 to 1.0 range 
 float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range)
 {    

diff --git a/libraries/AP_RSSI/AP_RSSI.h b/libraries/AP_RSSI/AP_RSSI.h
@@ -24,7 +24,8 @@ class AP_RSSI
     enum RssiType {
         RSSI_DISABLED           = 0,
         RSSI_ANALOG_PIN         = 1,
-        RSSI_RC_CHANNEL_VALUE   = 2
+        RSSI_RC_CHANNEL_VALUE   = 2,
+        SBUS_LINK_QUALITY       = 3
     };
 
     // constructor
@@ -60,6 +61,8 @@ class AP_RSSI
     AP_Int8         rssi_channel;                           // allows rssi to be read from given channel as PWM value
     AP_Int16        rssi_channel_low_pwm_value;             // PWM value for weakest rssi signal
     AP_Int16        rssi_channel_high_pwm_value;            // PWM value for strongest rssi signal
+    AP_Int16        rssi_sbus_low_value;                    // Receiver SBUS Link Quality low value
+    AP_Int16        rssi_sbus_high_value;                   // Receiver SBUS Link Quality high     
 
     // Analog Inputs
     // a pin for reading the receiver RSSI voltage. 
@@ -71,6 +74,9 @@ class AP_RSSI
     // read the RSSI value from a PWM value on a RC channel
     float read_channel_rssi();
 
+    // read the SBUS Link Quality
+    float read_sbus_link_quality();    
+
     // Scale and constrain a float rssi value to 0.0 to 1.0 range 
     float scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range);
 };