Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: add RC option to report CRSF LQ as RSSI

Browse Source 
Co-author: Jules Gilson <julesgilson@yahoo.co.uk>
This commit is contained in:
Hwurzburg 2021-12-19 08:35:22 -06:00 committed by Andrew Tridgell
parent d95c997606
commit eaa557106a
2 changed files with 34 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View File

@ -272,6 +272,9 @@ void AP_RCProtocol_CRSF::update(void)
process_telemetry(false);
_last_frame_time_us = now;
}
//Check if LQ is to be reported in place of RSSI
_use_lq_for_rssi = bool(rc().use_crsf_lq_as_rssi());
}
// write out a frame of any type
@ -464,6 +467,9 @@ 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{
// AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
if (rssi_dbm < 50) {
_link_status.rssi = 255;
@ -473,6 +479,7 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
// this is an approximation recommended by Remo from TBS
_link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
}
}
_link_status.rf_mode = static_cast<RFMode>(MIN(link->rf_mode, 3U));
}
@ -482,7 +489,11 @@ void AP_RCProtocol_CRSF::process_link_stats_rx_frame(const void* data)
{
const LinkStatisticsRXFrame* link = (const LinkStatisticsRXFrame*)data;
if (_use_lq_for_rssi) {
_link_status.rssi = derive_scaled_lq_value(link->link_quality);
} else {
_link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
}
}
// process link statistics to get TX RSSI
@ -490,7 +501,11 @@ void AP_RCProtocol_CRSF::process_link_stats_tx_frame(const void* data)
{
const LinkStatisticsTXFrame* link = (const LinkStatisticsTXFrame*)data;
if (_use_lq_for_rssi) {
_link_status.rssi = derive_scaled_lq_value(link->link_quality);
} else {
_link_status.rssi = link->rssi_percent * 255.0f * 0.01f;
}
}
// process a byte provided by a uart
@ -535,6 +550,12 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate)
return true;
}
//returns uplink link quality on 0-255 scale
int16_t AP_RCProtocol_CRSF::derive_scaled_lq_value(uint8_t uplink_lq)
{
return int16_t(roundf(constrain_float(uplink_lq*2.5f,0,255)));
}
namespace AP {
AP_RCProtocol_CRSF* crsf() {
return AP_RCProtocol_CRSF::get_singleton();

3
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View File

@ -270,6 +270,9 @@ private:
uint32_t _new_baud_rate;
bool _crsf_v3_active;
bool _use_lq_for_rssi;
int16_t derive_scaled_lq_value(uint8_t uplink_lq);
volatile struct LinkStatus _link_status;
AP_HAL::UARTDriver *_uart;