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

AP_RCTelemetry: don't spam mode changes on CRSF startup 

rewrite device ping bootstrap to avoid ping flood
enable device pings on CRSF in the event of TX loss.
only send pings if not negotiating the version
This commit is contained in:
Andy Piper 2022-01-15 15:27:36 +00:00 committed by Randy Mackay
parent c198cbdd52
commit ccbc24a480
2 changed files with 108 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -28,6 +28,7 @@
#include <AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h>
#include <math.h>
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#if HAL_CRSF_TELEM_ENABLED
@ -62,6 +63,7 @@ bool AP_CRSF_Telem::init(void)
&& !AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_CRSF, 0)) {
return false;
}
return AP_RCTelemetry::init();
}
@ -85,6 +87,9 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
add_scheduler_entry(5000, 100); // passthrough max 10Hz
add_scheduler_entry(5000, 500); // status text max 2Hz
add_scheduler_entry(5, 20); // command 50Hz (generally not active unless requested by the TX)
add_scheduler_entry(5, 500); // version ping 2Hz (only active at startup)
add_scheduler_entry(5, 100); // device ping 10Hz (only active during TX loss, also see CRSF_RX_TIMEOUT)
disable_scheduler_entry(DEVICE_PING);
}
void AP_CRSF_Telem::setup_custom_telemetry()
@ -167,7 +172,7 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_
}
}
void AP_CRSF_Telem::process_rf_mode_changes()
bool AP_CRSF_Telem::process_rf_mode_changes()
{
const AP_RCProtocol_CRSF::RFMode current_rf_mode = get_rf_mode();
uint32_t now = AP_HAL::millis();
@ -179,18 +184,25 @@ void AP_CRSF_Telem::process_rf_mode_changes()
uart = crsf->get_UART();
}
if (uart == nullptr) {
return;
return true;
}
// not ready yet
if (!uart->is_initialized()) {
return false;
}
// warn the user if their setup is sub-optimal
if (_telem_last_report_ms == 0 && !uart->is_dma_enabled()) {
if (_telem_bootstrap_msg_pending && !uart->is_dma_enabled()) {
gcs().send_text(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string());
}
// note if option was set to show LQ in place of RSSI
bool current_lq_as_rssi_active = bool(rc().use_crsf_lq_as_rssi());
if(_telem_last_report_ms == 0 || _noted_lq_as_rssi_active != current_lq_as_rssi_active){
if(_telem_bootstrap_msg_pending || _noted_lq_as_rssi_active != current_lq_as_rssi_active){
_noted_lq_as_rssi_active = current_lq_as_rssi_active;
gcs().send_text(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally");
}
_telem_bootstrap_msg_pending = false;
const bool is_high_speed = is_high_speed_telemetry(current_rf_mode);
if ((now - _telem_last_report_ms > 5000)) {
// report an RF mode change or a change in telemetry rate if we haven't done so in the last 5s
@ -204,8 +216,12 @@ void AP_CRSF_Telem::process_rf_mode_changes()
_telem_is_high_speed = is_high_speed;
_telem_rf_mode = current_rf_mode;
_telem_last_avg_rate = _scheduler.avg_packet_rate;
if (_telem_last_report_ms == 0) { // only want to show bootstrap messages once
_telem_bootstrap_msg_pending = true;
}
_telem_last_report_ms = now;
}
return true;
}
// return custom frame id based on fw version
@ -288,25 +304,25 @@ void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
AP_RCTelemetry::queue_message(severity, text);
}
void AP_CRSF_Telem::enter_scheduler_params_mode()
/*
disable telemetry entries that require a transmitter to be present
*/
void AP_CRSF_Telem::disable_tx_entries()
{
debug("parameter passthrough enabled");
set_scheduler_entry(HEARTBEAT, 50, 200); // heartbeat 5Hz
disable_scheduler_entry(ATTITUDE);
disable_scheduler_entry(BATTERY);
disable_scheduler_entry(GPS);
disable_scheduler_entry(FLIGHT_MODE);
disable_scheduler_entry(PASSTHROUGH);
disable_scheduler_entry(STATUS_TEXT);
// GENERAL_COMMAND and PARAMETERS will only be sent under very specific circumstances
}
void AP_CRSF_Telem::exit_scheduler_params_mode()
/*
enable telemetry entries that require a transmitter to be present
*/
void AP_CRSF_Telem::enable_tx_entries()
{
debug("parameter passthrough disabled");
// setup the crossfire scheduler for custom telemetry
set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz
enable_scheduler_entry(ATTITUDE);
enable_scheduler_entry(BATTERY);
enable_scheduler_entry(GPS);
@ -317,6 +333,21 @@ void AP_CRSF_Telem::exit_scheduler_params_mode()
update_custom_telemetry_rates(_telem_rf_mode);
}
void AP_CRSF_Telem::enter_scheduler_params_mode()
{
debug("parameter passthrough enabled");
set_scheduler_entry(HEARTBEAT, 50, 200); // heartbeat 5Hz
disable_tx_entries();
}
void AP_CRSF_Telem::exit_scheduler_params_mode()
{
debug("parameter passthrough disabled");
// setup the crossfire scheduler for custom telemetry
set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz
enable_tx_entries();
}
void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
{
uint32_t now_ms = AP_HAL::millis();
@ -346,24 +377,12 @@ void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
// WFQ scheduler
bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
{
process_rf_mode_changes();
if (!process_rf_mode_changes()) {
return false;
}
switch (idx) {
case PARAMETERS:
// to get crossfire firmware version we send an RX device ping until we get a response
// but only if there are no other requests pending
if (_crsf_version.pending && _pending_request.frame_type == 0) {
if (_crsf_version.retry_count++ > CRSF_RX_DEVICE_PING_MAX_RETRY) {
_crsf_version.pending = false;
_crsf_version.minor = 0;
_crsf_version.major = 0;
gcs().send_text(MAV_SEVERITY_DEBUG,"%s: RX device ping failed", get_protocol_string());
} else {
_pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
gcs().send_text(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string());
}
}
return _pending_request.frame_type > 0;
case VTX_PARAMETERS:
return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending;
@ -373,6 +392,12 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
return rc().crsf_custom_telemetry() && !queue_empty;
case GENERAL_COMMAND:
return _baud_rate_request.pending;
case VERSION_PING:
return _crsf_version.pending;
case HEARTBEAT:
return true; // always send heartbeat if enabled
case DEVICE_PING:
return !_crsf_version.pending; // only send pings if version has been negotiated
default:
return _enable_telemetry;
}
@ -387,7 +412,7 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
calc_heartbeat();
break;
case PARAMETERS: // update parameter settings
update_params();
process_pending_requests();
break;
case ATTITUDE:
calc_attitude();
@ -422,6 +447,22 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
case GENERAL_COMMAND:
calc_command_response();
break;
case VERSION_PING:
// to get crossfire firmware version we send an RX device ping
if (_crsf_version.retry_count++ > CRSF_RX_DEVICE_PING_MAX_RETRY) {
_crsf_version.pending = false;
_crsf_version.minor = 0;
_crsf_version.major = 0;
disable_scheduler_entry(VERSION_PING);
gcs().send_text(MAV_SEVERITY_DEBUG,"%s: RX device ping failed", get_protocol_string());
} else {
calc_device_ping(AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER);
gcs().send_text(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string());
}
break;
case DEVICE_PING:
calc_device_ping(AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER);
break;
default:
break;
}
@ -566,6 +607,7 @@ void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping)
_param_request.origin = ping->origin;
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
_pending_request.destination = ping->origin;
}
// request for device info
@ -611,6 +653,7 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
}
_crsf_version.pending = false;
disable_scheduler_entry(VERSION_PING);
}
// request for a general command
@ -658,7 +701,7 @@ void AP_CRSF_Telem::update()
{
}
void AP_CRSF_Telem::update_params()
void AP_CRSF_Telem::process_pending_requests()
{
// handle general parameter requests
switch (_pending_request.frame_type) {
@ -669,7 +712,7 @@ void AP_CRSF_Telem::update_params()
break;
// construct a ping frame originating here
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
calc_device_ping();
calc_device_ping(_pending_request.destination);
break;
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
// reset parameter passthrough timeout
@ -679,6 +722,8 @@ void AP_CRSF_Telem::update_params()
default:
break;
}
_pending_request.frame_type = 0;
}
void AP_CRSF_Telem::update_vtx_params()
@ -913,13 +958,11 @@ void AP_CRSF_Telem::calc_device_info() {
}
// send a device ping
void AP_CRSF_Telem::calc_device_ping() {
_telem.ext.ping.destination = _pending_request.destination;
void AP_CRSF_Telem::calc_device_ping(uint8_t destination) {
_telem.ext.ping.destination = destination;
_telem.ext.ping.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
_telem_size = 2;
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
_pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST;
_pending_request.frame_type = 0;
_telem_pending = true;
@ -1387,10 +1430,23 @@ void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data(uint8_t size)
/*
fetch CRSF frame data
if is_tx_active is true then this will be a request for telemetry after receiving an RC frame
*/
bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data)
bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active)
{
memset(&_telem, 0, sizeof(TelemetryPayload));
// update telemetry tasks if we either lost or regained the transmitter
if (_is_tx_active != is_tx_active) {
if (is_tx_active) {
disable_scheduler_entry(DEVICE_PING);
enable_tx_entries();
} else {
disable_tx_entries();
enable_scheduler_entry(DEVICE_PING);
}
_is_tx_active = is_tx_active;
}
run_wfq_scheduler();
if (!_telem_pending) {
return false;
@ -1418,12 +1474,12 @@ bool AP_CRSF_Telem::process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void
/*
fetch data for an external transport, such as CRSF
*/
bool AP_CRSF_Telem::get_telem_data(AP_RCProtocol_CRSF::Frame* data)
bool AP_CRSF_Telem::get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active)
{
if (!get_singleton()) {
return false;
}
return singleton->_get_telem_data(data);
return singleton->_get_telem_data(data, is_tx_active);
}
AP_CRSF_Telem *AP_CRSF_Telem::get_singleton(void) {

20
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -67,7 +67,7 @@ public:
};
struct HeartbeatFrame {
uint8_t origin; // Device addres
uint8_t origin; // Device address
};
struct PACKED BatteryFrame {
@ -252,7 +252,7 @@ public:
// process any changed settings and schedule for transmission
void update();
// get next telemetry data for external consumers of SPort data
static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame);
static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame, bool is_tx_active);
private:
@ -267,6 +267,8 @@ private:
PASSTHROUGH,
STATUS_TEXT,
GENERAL_COMMAND,
VERSION_PING,
DEVICE_PING,
NUM_SENSORS
};
@ -284,18 +286,18 @@ private:
void calc_attitude();
void calc_flight_mode();
void calc_device_info();
void calc_device_ping();
void calc_device_ping(uint8_t destination);
void calc_command_response();
void calc_parameter();
#if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk);
#endif
void update_params();
void process_pending_requests();
void update_vtx_params();
void get_single_packet_passthrough_telem_data();
void get_multi_packet_passthrough_telem_data(uint8_t size = PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE);
void calc_status_text();
void process_rf_mode_changes();
bool process_rf_mode_changes();
uint8_t get_custom_telem_frame_id() const;
AP_RCProtocol_CRSF::RFMode get_rf_mode() const;
uint16_t get_telemetry_rate() const;
@ -315,9 +317,11 @@ private:
// setup the scheduler for parameters download
void enter_scheduler_params_mode();
void exit_scheduler_params_mode();
void disable_tx_entries();
void enable_tx_entries();
// get next telemetry data for external consumers
bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data);
bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active);
bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
TelemetryPayload _telem;
@ -327,10 +331,14 @@ private:
// reporting telemetry rate
uint32_t _telem_last_report_ms;
uint16_t _telem_last_avg_rate;
// do we need to report the initial state
bool _telem_bootstrap_msg_pending;
bool _telem_is_high_speed;
bool _telem_pending;
bool _enable_telemetry;
// used to limit telemetry when in a failsafe condition
bool _is_tx_active;
struct {
uint8_t destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST;