Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: process CRSF link statistics frames to get RSSI

Browse Source
This commit is contained in:
Andy Piper 2020-07-15 20:26:28 +01:00 committed by Peter Barker
parent 53553c7c71
commit ce970dcf75
2 changed files with 41 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View File

@ -206,7 +206,7 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
_last_frame_time_us = timestamp_us;
// decode here
if (decode_csrf_packet()) {
add_input(MAX_CHANNELS, _channels, false, -1);
add_input(MAX_CHANNELS, _channels, false, _current_rssi);
}
}
}
@ -284,6 +284,9 @@ bool AP_RCProtocol_CRSF::decode_csrf_packet()
decode_11bit_channels((const uint8_t*)(&_frame.payload), CRSF_MAX_CHANNELS, _channels, 5U, 8U, 880U);
rc_active = !_uart; // only accept RC data if we are not in standalone mode
break;
case CRSF_FRAMETYPE_LINK_STATISTICS:
process_link_stats_frame((uint8_t*)&_frame.payload);
break;
default:
break;
}
@ -337,6 +340,28 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
return true;
}
// process link statistics to get RSSI
void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
{
const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data;
uint8_t rssi_dbm;
if (link->active_antenna == 0) {
rssi_dbm = link->uplink_rssi_ant1;
} else {
rssi_dbm = link->uplink_rssi_ant2;
}
// AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
if (rssi_dbm < 50) {
_current_rssi = 255;
} else if (rssi_dbm > 120) {
_current_rssi = 0;
} else {
// this is an approximation recommended by Remo from TBS
_current_rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
}
}
// process a byte provided by a uart
void AP_RCProtocol_CRSF::process_byte(uint8_t byte, uint32_t baudrate)
{

15
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View File

@ -148,6 +148,19 @@ public:
uint8_t payload[CRSF_FRAMELEN_MAX - 3]; // +1 for crc
} PACKED;
struct LinkStatisticsFrame {
uint8_t uplink_rssi_ant1; // ( dBm * -1 )
uint8_t uplink_rssi_ant2; // ( dBm * -1 )
uint8_t uplink_status; // Package success rate / Link quality ( % )
int8_t uplink_snr; // ( db )
uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
uint8_t downlink_rssi; // ( dBm * -1 )
uint8_t downlink_status; // Downlink package success rate / Link quality ( % )
int8_t downlink_dnr; // ( db )
} PACKED;
private:
struct Frame _frame;
struct Frame _telemetry_frame;
@ -160,6 +173,7 @@ private:
void _process_byte(uint32_t timestamp_us, uint8_t byte);
bool decode_csrf_packet();
bool process_telemetry(bool check_constraint = true);
void process_link_stats_frame(const void* data);
void write_frame(Frame* frame);
void start_uart();
AP_HAL::UARTDriver* get_current_UART() { return (_uart ? _uart : get_available_UART()); }
@ -174,6 +188,7 @@ private:
uint32_t _start_frame_time_us;
bool telem_available;
bool _fast_telem; // is 150Hz telemetry active
int16_t _current_rssi = -1;
AP_HAL::UARTDriver *_uart;