aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RCTelemetry: add baud rate change command frame processing.

Browse Source 
report CRSF version when rate changes
This commit is contained in:
Andy Piper 2021-07-12 22:07:16 +01:00 committed by Andrew Tridgell
parent 293bb7704e
commit 6f53337b88
2 changed files with 76 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -82,6 +82,7 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
add_scheduler_entry(550, 500); // flight mode 2Hz
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)
}
void AP_CRSF_Telem::setup_custom_telemetry()
@ -174,7 +175,8 @@ void AP_CRSF_Telem::process_rf_mode_changes()
if ((now - _telem_last_report_ms > 5000) &&
(_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) {
if (!rc().suppress_crsf_message()) {
gcs().send_text(MAV_SEVERITY_INFO, "CRSF: RF mode %d, rate is %dHz", (uint8_t)current_rf_mode, _scheduler.avg_packet_rate);
gcs().send_text(MAV_SEVERITY_INFO, "CRSFv%d: RF mode %d, rate is %dHz", uint8_t(2 + AP::crsf()->is_crsf_v3_active()),
(uint8_t)current_rf_mode, _scheduler.avg_packet_rate);
}
update_custom_telemetry_rates(current_rf_mode);
_telem_rf_mode = current_rf_mode;
@ -320,6 +322,8 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
return rc().crsf_custom_telemetry();
case STATUS_TEXT:
return rc().crsf_custom_telemetry() && !queue_empty;
case GENERAL_COMMAND:
return _baud_rate_request.pending;
default:
return _enable_telemetry;
}
@ -365,6 +369,9 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
case STATUS_TEXT:
calc_status_text();
break;
case GENERAL_COMMAND:
calc_command_response();
break;
default:
break;
}
@ -404,6 +411,10 @@ bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, voi
process_device_info_frame((ParameterDeviceInfoFrame*)data);
break;
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND:
process_command_frame((CommandFrame*)data);
break;
default:
break;
}
@ -548,6 +559,34 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
_crsf_version.pending = false;
}
// request for a general command
void AP_CRSF_Telem::process_command_frame(CommandFrame* command)
{
debug("process_command_frame: 0x%x -> 0x%x: 0x%x", command->origin, command->destination, command->payload[0]);
if (command->destination != 0 && command->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
return; // request was not for us
}
// we are only interested in commands from the RX
if (command->origin != 0 && command->origin != AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER) {
return;
}
switch (command->payload[0]) {
case AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL_CRSF_SPEED_PROPOSAL: {
uint32_t baud_rate = command->payload[2] << 24 | command->payload[3] << 16
| command->payload[4] << 8 | command->payload[5];
_baud_rate_request.port_id = command->payload[1];
_baud_rate_request.valid = AP::crsf()->change_baud_rate(baud_rate);
_baud_rate_request.pending = true;
debug("requested baud rate change %lu", baud_rate);
break;
}
default:
break; // do nothing
}
}
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,
@ -827,6 +866,33 @@ void AP_CRSF_Telem::calc_device_ping() {
_telem_pending = true;
}
// send a command response
void AP_CRSF_Telem::calc_command_response() {
_telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
_telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
_telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL;
_telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL_CRSF_SPEED_RESPONSE;
_telem.ext.command.payload[1] = _baud_rate_request.port_id;
_telem.ext.command.payload[2] = _baud_rate_request.valid;
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND;
// calculate command crc
uint8_t len = 6;
uint8_t* crcptr = &_telem.ext.command.destination;
uint8_t crc = crc8_dvb(0, AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND, 0xBA);
for (uint8_t i = 0; i < len; i++) {
crc = crc8_dvb(crc, crcptr[i], 0xBA);
}
crcptr[len] = crc;
_telem_size = len + 1;
_pending_request.frame_type = 0;
_baud_rate_request.pending = false;
debug("sent baud rate response: %u", _baud_rate_request.valid);
_telem_pending = true;
}
// return parameter information
void AP_CRSF_Telem::calc_parameter() {
#if OSD_PARAM_ENABLED

9
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -253,6 +253,7 @@ private:
FLIGHT_MODE,
PASSTHROUGH,
STATUS_TEXT,
GENERAL_COMMAND,
NUM_SENSORS
};
@ -271,6 +272,7 @@ private:
void calc_flight_mode();
void calc_device_info();
void calc_device_ping();
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);
@ -291,6 +293,7 @@ private:
void process_param_read_frame(ParameterSettingsReadFrame* read);
void process_param_write_frame(ParameterSettingsWriteFrame* write);
void process_device_info_frame(ParameterDeviceInfoFrame* info);
void process_command_frame(CommandFrame* command);
// setup ready for passthrough operation
void setup_wfq_scheduler(void) override;
@ -334,6 +337,12 @@ private:
bool params_mode_active;
} _custom_telem;
struct {
bool pending;
bool valid;
uint8_t port_id;
} _baud_rate_request;
// vtx state
bool _vtx_freq_update; // update using the frequency method or not
bool _vtx_dbm_update; // update using the dbm method or not