mirror of https://github.com/ArduPilot/ardupilot
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:
parent
a1a87bd1f5
commit
ea91d6e171
|
@ -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] = {
|
const uint16_t AP_RCProtocol_CRSF::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = {
|
||||||
4, 50, 150, 250, // CRSF
|
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;
|
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
|
// return the link rate as defined by the LinkStatistics
|
||||||
uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const {
|
uint16_t AP_RCProtocol_CRSF::get_link_rate(ProtocolType protocol) const {
|
||||||
if (protocol == ProtocolType::PROTOCOL_ELRS) {
|
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) {
|
} else if (protocol == ProtocolType::PROTOCOL_TRACER) {
|
||||||
return 250;
|
return 250;
|
||||||
} else {
|
} else {
|
||||||
|
@ -573,6 +573,11 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
|
||||||
return true;
|
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
|
// process link statistics to get RSSI
|
||||||
void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
|
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;
|
rssi_dbm = link->uplink_rssi_ant2;
|
||||||
}
|
}
|
||||||
_link_status.link_quality = link->uplink_status;
|
_link_status.link_quality = link->uplink_status;
|
||||||
|
|
||||||
if (_use_lq_for_rssi) {
|
if (_use_lq_for_rssi) {
|
||||||
_link_status.rssi = derive_scaled_lq_value(link->uplink_status);
|
_link_status.rssi = derive_scaled_lq_value(link->uplink_status);
|
||||||
} else{
|
} 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
|
// process link statistics to get RX RSSI
|
||||||
|
|
|
@ -27,6 +27,7 @@
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <RC_Channel/RC_Channel.h>
|
#include <RC_Channel/RC_Channel.h>
|
||||||
#include "SoftSerial.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_MAX_CHANNELS 24U // Maximum number of channels from crsf datastream
|
||||||
#define CRSF_FRAMELEN_MAX 64U // maximum possible framelength
|
#define CRSF_FRAMELEN_MAX 64U // maximum possible framelength
|
||||||
|
@ -258,29 +259,51 @@ public:
|
||||||
PROTOCOL_ELRS
|
PROTOCOL_ELRS
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// Source for ELRS RF modes: https://www.expresslrs.org/info/signal-health/#rf-mode-indexes-rfmd
|
||||||
enum RFMode {
|
enum RFMode {
|
||||||
CRSF_RF_MODE_4HZ = 0,
|
CRSF_RF_MODE_4HZ = 0,
|
||||||
CRSF_RF_MODE_50HZ,
|
CRSF_RF_MODE_50HZ,
|
||||||
CRSF_RF_MODE_150HZ,
|
CRSF_RF_MODE_150HZ,
|
||||||
CRSF_RF_MODE_250HZ,
|
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_25HZ,
|
||||||
ELRS_RF_MODE_50HZ,
|
ELRS_RF_MODE_50HZ,
|
||||||
ELRS_RF_MODE_100HZ,
|
ELRS_RF_MODE_100HZ,
|
||||||
|
ELRS_RF_MODE_100HZ_FULL,
|
||||||
ELRS_RF_MODE_150HZ,
|
ELRS_RF_MODE_150HZ,
|
||||||
ELRS_RF_MODE_200HZ,
|
ELRS_RF_MODE_200HZ,
|
||||||
ELRS_RF_MODE_250HZ,
|
ELRS_RF_MODE_250HZ,
|
||||||
|
ELRS_RF_MODE_333HZ_FULL,
|
||||||
ELRS_RF_MODE_500HZ,
|
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_MAX_MODES,
|
||||||
RF_MODE_UNKNOWN,
|
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 {
|
struct LinkStatus {
|
||||||
int16_t rssi = -1;
|
int16_t rssi = -1;
|
||||||
int16_t link_quality = -1;
|
int16_t link_quality = -1;
|
||||||
uint8_t rf_mode;
|
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
|
// 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
|
// from within AP_RCProtocol_CRSF thread so no need for cross-thread synch
|
||||||
const volatile LinkStatus& get_link_status() const {
|
const volatile LinkStatus& get_link_status() const {
|
||||||
|
|
Loading…
Reference in New Issue