diff --git a/libraries/AP_HAL/RCInput.h b/libraries/AP_HAL/RCInput.h index 4017e4a73473d..80870d3d1cd1b 100644 --- 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 index 086dedc0aaaa2..f800ecf199de2 100644 --- 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 index 3bcabd50b0f86..88ba3e8bd02d4 100644 --- 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 index a3b88664d4737..be7efda7d4870 100644 --- 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 index 6ff251d845f55..5bd8033eef431 100644 --- 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 index 48763995933a0..540e2eb5820ae 100644 --- 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 index 4421fe945a23c..983b17955fb5b 100644 --- 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 index f18f9790a951d..855a4e461b182 100644 --- 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 index 56535b8b27fd9..e99d29fa4159f 100644 --- 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 index bfb9ddc6b96cd..c69eb44caf101 100644 --- 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 index 8d0b959776f3e..acb38562bb537 100644 --- 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 index 355ff3ec11b2a..93d61b8114bcd 100644 --- 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); };