Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: add extended RC link stats OSD fields 

Adds RSSI dBm, SNR, LQ, Tx power and active antenna OSD fields
This commit is contained in:
rmaia 2024-03-27 17:40:03 -03:00 committed by Andrew Tridgell
parent a1a87bd1f5
commit ea91d6e171
3 changed files with 55 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View File

@ -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 {
@ -573,6 +573,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)
{
@ -585,6 +590,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{
@ -599,7 +605,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

25
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View File

@ -27,6 +27,7 @@
#include <AP_Math/AP_Math.h>
#include <RC_Channel/RC_Channel.h>
#include "SoftSerial.h"
#include <AP_OSD/AP_OSD_config.h>
#define CRSF_MAX_CHANNELS 24U // Maximum number of channels from crsf datastream
#define CRSF_FRAMELEN_MAX 64U // maximum possible framelength
@ -258,29 +259,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 {

2
libraries/AP_RCProtocol/AP_RCProtocol_config.h
View File

@ -85,4 +85,4 @@
#ifndef AP_RCPROTOCOL_FDM_ENABLED
#define AP_RCPROTOCOL_FDM_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#endif