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_RCTelemetry: don't enter parameter mode while armed.

Browse Source 
exit parameter mode quickly on ping
construct and respond to ping frames correctly
This commit is contained in:
Andy Piper 2021-09-15 08:55:12 +01:00 committed by Andrew Tridgell
parent f541653502
commit e1fdab8423
1 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -282,7 +282,10 @@ void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
We start a "fast parameter window" that we close after 5sec We start a "fast parameter window" that we close after 5sec
*/ */
bool expired = (now_ms - _custom_telem.params_mode_start_ms) > 5000; bool expired = (now_ms - _custom_telem.params_mode_start_ms) > 5000;
if (!_custom_telem.params_mode_active && _pending_request.frame_type > 0) { if (!_custom_telem.params_mode_active
&& _pending_request.frame_type > 0
&& _pending_request.frame_type != AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO
&& !hal.util->get_soft_armed()) {
// fast window start // fast window start
_custom_telem.params_mode_start_ms = now_ms; _custom_telem.params_mode_start_ms = now_ms;
_custom_telem.params_mode_active = true; _custom_telem.params_mode_active = true;
@ -311,7 +314,7 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: RX device ping failed"); gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: RX device ping failed");
} else { } else {
_pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER; _pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO; _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info"); gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info");
} }
} }
@ -515,7 +518,7 @@ void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping)
} }
_param_request.origin = ping->origin; _param_request.origin = ping->origin;
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING; _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
} }
// request for device info // request for device info
@ -606,21 +609,22 @@ void AP_CRSF_Telem::update()
void AP_CRSF_Telem::update_params() void AP_CRSF_Telem::update_params()
{ {
uint32_t now = AP_HAL::millis();
// reset parameter passthrough timeout
_custom_telem.params_mode_start_ms = now;
// handle general parameter requests // handle general parameter requests
switch (_pending_request.frame_type) { switch (_pending_request.frame_type) {
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING: // construct a response to a ping frame
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
_custom_telem.params_mode_start_ms = AP_HAL::millis();
calc_device_info(); calc_device_info();
break; break;
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ: // construct a ping frame originating here
calc_parameter(); case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
break;
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
calc_device_ping(); calc_device_ping();
break; break;
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
// reset parameter passthrough timeout
_custom_telem.params_mode_start_ms = AP_HAL::millis();
calc_parameter();
break;
default: default:
break; break;
} }