From 8d8f3acc89fbe7d813b418f806e58cd94fa933bc Mon Sep 17 00:00:00 2001 From: rmaia <9812730+rmaia3d@users.noreply.github.com> Date: Wed, 27 Mar 2024 17:40:03 -0300 Subject: [PATCH] AP_RCProtocol: add extended RC link stats OSD fields Adds RSSI dBm, SNR, LQ, Tx power and active antenna OSD fields --- .../AP_RCProtocol/AP_RCProtocol_CRSF.cpp | 33 +++++++++++++++++-- libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h | 25 +++++++++++++- .../AP_RCProtocol/AP_RCProtocol_config.h | 2 +- 3 files changed, 55 insertions(+), 5 deletions(-) diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 745b55a51b..b310753557 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -160,7 +160,7 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0) const uint16_t AP_RCProtocol_CRSF::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = { 4, 50, 150, 250, // CRSF - 4, 25, 50, 100, 150, 200, 250, 500 // ELRS + 4, 25, 50, 100, 100, 150, 200, 250, 333, 500, 250, 500, 500, 1000, 50 // ELRS }; AP_RCProtocol_CRSF* AP_RCProtocol_CRSF::_singleton; @@ -204,7 +204,7 @@ const char* AP_RCProtocol_CRSF::get_protocol_string(ProtocolType protocol) const // return the link rate as defined by the LinkStatistics uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const { if (protocol == ProtocolType::PROTOCOL_ELRS) { - return RF_MODE_RATES[_link_status.rf_mode + RFMode::ELRS_RF_MODE_4HZ]; + return RF_MODE_RATES[_link_status.rf_mode + RFMode::CRSF_RF_MAX_MODES]; } else if (protocol == ProtocolType::PROTOCOL_TRACER) { return 250; } else { @@ -578,6 +578,11 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint) return true; } +#if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED + // Define the static tx powers array + constexpr uint16_t AP_RCProtocol_CRSF::tx_powers[]; +#endif + // process link statistics to get RSSI void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data) { @@ -590,6 +595,7 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data) rssi_dbm = link->uplink_rssi_ant2; } _link_status.link_quality = link->uplink_status; + if (_use_lq_for_rssi) { _link_status.rssi = derive_scaled_lq_value(link->uplink_status); } else{ @@ -604,7 +610,28 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data) } } - _link_status.rf_mode = MIN(link->rf_mode, 7U); + // Define the max number of RFModes based on ELRS modes, which is larger than Crossfire + const uint8_t max_modes = (RFMode::RF_MODE_MAX_MODES - RFMode::CRSF_RF_MAX_MODES) - 1U; // Subtract 1 due to zero-indexing + _link_status.rf_mode = MIN(link->rf_mode, max_modes); // Cap to avoid memory spills in the conversion tables + +#if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED + // Populate the extra data items + if (link->uplink_status > 0) { + _link_status.rssi_dbm = rssi_dbm; + _link_status.tx_power = -1; + if (link->uplink_tx_power < ARRAY_SIZE(AP_RCProtocol_CRSF::tx_powers)) { + _link_status.tx_power = AP_RCProtocol_CRSF::tx_powers[link->uplink_tx_power]; + } + _link_status.snr = link->uplink_snr; + _link_status.active_antenna = link->active_antenna; + } else { + // This means LQ is zero, so set all values to "no signal" state + _link_status.rssi_dbm = -1; + _link_status.tx_power = -1; + _link_status.snr = INT8_MIN; + _link_status.active_antenna = -1; + } +#endif } // process link statistics to get RX RSSI diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index ac2af47123..3766848430 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -26,6 +26,7 @@ #include #include #include "SoftSerial.h" +#include #define CRSF_MAX_CHANNELS 24U // Maximum number of channels from crsf datastream #define CRSF_FRAMELEN_MAX 64U // maximum possible framelength @@ -253,29 +254,51 @@ public: PROTOCOL_ELRS }; + // Source for ELRS RF modes: https://www.expresslrs.org/info/signal-health/#rf-mode-indexes-rfmd enum RFMode { CRSF_RF_MODE_4HZ = 0, CRSF_RF_MODE_50HZ, CRSF_RF_MODE_150HZ, CRSF_RF_MODE_250HZ, - ELRS_RF_MODE_4HZ, + CRSF_RF_MAX_MODES = 4, + ELRS_RF_MODE_4HZ = 4, ELRS_RF_MODE_25HZ, ELRS_RF_MODE_50HZ, ELRS_RF_MODE_100HZ, + ELRS_RF_MODE_100HZ_FULL, ELRS_RF_MODE_150HZ, ELRS_RF_MODE_200HZ, ELRS_RF_MODE_250HZ, + ELRS_RF_MODE_333HZ_FULL, ELRS_RF_MODE_500HZ, + ELRS_RF_MODE_D250HZ, + ELRS_RF_MODE_D500HZ, + ELRS_RF_MODE_F500HZ, + ELRS_RF_MODE_F1000HZ, + ELRS_RF_MODE_D50HZ, RF_MODE_MAX_MODES, RF_MODE_UNKNOWN, }; +#if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED + // These power levels are valid for both Crossfire and ELRS systems + static constexpr uint16_t tx_powers[] = { 0, 10, 25, 100, 500, 1000, 2000, 250, 50 }; +#endif + struct LinkStatus { int16_t rssi = -1; int16_t link_quality = -1; uint8_t rf_mode; +#if AP_OSD_LINK_STATS_EXTENSIONS_ENABLED + // Add the extra data fields to be used by the OSD panels + int16_t tx_power = -1; + int8_t rssi_dbm = -1; + int8_t snr = INT8_MIN; + int8_t active_antenna = -1; +#endif }; + // this will be used by AP_CRSF_Telem to access link status data // from within AP_RCProtocol_CRSF thread so no need for cross-thread synch const volatile LinkStatus& get_link_status() const { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index d6bef7d61b..5d23228c36 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -64,4 +64,4 @@ #ifndef AP_RCPROTOCOL_GHST_ENABLED #define AP_RCPROTOCOL_GHST_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED -#endif +#endif \ No newline at end of file