Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCTelemetry: correct VTX power settings and pass parameter requests more quickly

Browse Source 
return parameter ids when CRSF folder request is made
turn off telemetry while processing parameters
This commit is contained in:
Andy Piper 2021-05-30 21:17:48 +01:00 committed by Andrew Tridgell
parent 4a05e65367
commit 96acb46fbf
1 changed files with 51 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -32,7 +32,7 @@
#if HAL_CRSF_TELEM_ENABLED
// #define CRSF_DEBUG
//#define CRSF_DEBUG
#ifdef CRSF_DEBUG
# define debug(fmt, args...) hal.console->printf("CRSF: " fmt "\n", ##args)
#else
@ -74,7 +74,7 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
// CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit
add_scheduler_entry(50, 100); // heartbeat 10Hz
add_scheduler_entry(50, 50); // parameters 20Hz (generally not active unless requested by the TX)
add_scheduler_entry(5, 20); // parameters 50Hz (generally not active unless requested by the TX)
add_scheduler_entry(50, 120); // Attitude and compass 8Hz
add_scheduler_entry(200, 1000); // VTX parameters 1Hz
add_scheduler_entry(1300, 500); // battery 2Hz
@ -232,24 +232,27 @@ void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
void AP_CRSF_Telem::enter_scheduler_params_mode()
{
set_scheduler_entry(HEARTBEAT, 50, 100); // heartbeat 10Hz
set_scheduler_entry(ATTITUDE, 50, 120); // Attitude and compass 8Hz
set_scheduler_entry(BATTERY, 1300, 500); // battery 2Hz
set_scheduler_entry(GPS, 550, 280); // GPS 3Hz
set_scheduler_entry(FLIGHT_MODE, 550, 500); // flight mode 2Hz
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);
}
void AP_CRSF_Telem::exit_scheduler_params_mode()
{
debug("parameter passthrough disabled");
// setup the crossfire scheduler for custom telemetry
set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz
set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz
set_scheduler_entry(FLIGHT_MODE, 1200, 2000); // 0.5Hz
set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz
enable_scheduler_entry(ATTITUDE);
enable_scheduler_entry(BATTERY);
enable_scheduler_entry(GPS);
enable_scheduler_entry(FLIGHT_MODE);
enable_scheduler_entry(PASSTHROUGH);
enable_scheduler_entry(STATUS_TEXT);
@ -425,10 +428,10 @@ void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) {
apvtx.set_power_mw(25);
break;
case 1:
apvtx.set_power_mw(200);
apvtx.set_power_mw(100);
break;
case 2:
apvtx.set_power_mw(500);
apvtx.set_power_mw(400);
break;
case 3:
apvtx.set_power_mw(800);
@ -537,8 +540,8 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame)
{
//debug("process_param_read_frame: %d -> %d for %d[%d]", read_frame->origin, read_frame->destination,
// read_frame->param_number, read_frame->param_chunk);
debug("process_param_read_frame: %d -> %d for %d[%d]", read_frame->origin, read_frame->destination,
read_frame->param_num, read_frame->param_chunk);
if (read_frame->destination != 0 && read_frame->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
return; // request was not for us
}
@ -554,6 +557,10 @@ void AP_CRSF_Telem::update()
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
switch (_pending_request.frame_type) {
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
@ -771,7 +778,7 @@ void AP_CRSF_Telem::calc_device_info() {
const AP_FWVersion &fwver = AP::fwversion();
// write out the name with version, max width is 60 - 18 = the meaning of life
int32_t n = strlen(fwver.fw_string);
strncpy((char*)_telem.ext.info.payload, fwver.fw_string, 41);
strncpy((char*)_telem.ext.info.payload, fwver.fw_short_string, 41);
n = MIN(n + 1, 42);
put_be32_ptr(&_telem.ext.info.payload[n], // serial number
@ -813,15 +820,41 @@ void AP_CRSF_Telem::calc_device_ping() {
// return parameter information
void AP_CRSF_Telem::calc_parameter() {
#if OSD_PARAM_ENABLED
_telem.ext.param_entry.header.destination = _param_request.origin;
_telem.ext.param_entry.header.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
size_t idx = 0;
// root folder request
if (_param_request.param_num == 0) {
_telem.ext.param_entry.header.param_num = 0;
_telem.ext.param_entry.header.chunks_left = 0;
_telem.ext.param_entry.payload[idx++] = 0; // parent folder
_telem.ext.param_entry.payload[idx++] = ParameterType::FOLDER; // type
_telem.ext.param_entry.payload[idx++] = 'r'; // "root" name
_telem.ext.param_entry.payload[idx++] = 'o';
_telem.ext.param_entry.payload[idx++] = 'o';
_telem.ext.param_entry.payload[idx++] = 't';
_telem.ext.param_entry.payload[idx++] = 0; // null terminator
// write out all of the ids we are going to send
for (uint8_t i = 0; i < AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; i++) {
_telem.ext.param_entry.payload[idx++] = i + 1;
}
_telem.ext.param_entry.payload[idx] = 0xFF; // terminator
_telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
_pending_request.frame_type = 0;
_telem_pending = true;
return;
}
AP_OSD* osd = AP::osd();
if (osd == nullptr) {
return;
}
_telem.ext.param_entry.header.destination = _param_request.origin;
_telem.ext.param_entry.header.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
AP_OSD_ParamSetting* param = osd->get_setting((_param_request.param_num - 1) / AP_OSD_ParamScreen::NUM_PARAMS,
(_param_request.param_num - 1) % AP_OSD_ParamScreen::NUM_PARAMS);
@ -836,7 +869,6 @@ void AP_CRSF_Telem::calc_parameter() {
return;
}
#endif
size_t idx = 0;
_telem.ext.param_entry.header.chunks_left = 0;
_telem.ext.param_entry.payload[idx++] = 0; // parent folder
idx++; // leave a gap for the type