mirror of https://github.com/ArduPilot/ardupilot
AP_RCTelemetry: added support for passthrough telemetry over crossfire
This commit is contained in:
parent
9db0862d61
commit
4933544489
|
@ -26,6 +26,7 @@
|
|||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <AP_Common/AP_FWVersion.h>
|
||||
#include <AP_OSD/AP_OSD.h>
|
||||
#include <AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
|
@ -73,26 +74,218 @@ 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(200, 50); // parameters 20Hz (generally not active unless requested by the TX)
|
||||
add_scheduler_entry(50, 50); // parameters 20Hz (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
|
||||
add_scheduler_entry(550, 280); // GPS 3Hz
|
||||
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
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::setup_custom_telemetry()
|
||||
{
|
||||
if (_custom_telem.init_done) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!rc().crsf_custom_telemetry()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// we need crossfire firmware version
|
||||
if (_crsf_version.pending) {
|
||||
return;
|
||||
}
|
||||
|
||||
AP_Frsky_SPort_Passthrough* passthrough = AP::frsky_passthrough_telem();
|
||||
if (passthrough == nullptr) {
|
||||
return;
|
||||
}
|
||||
|
||||
// setup the frsky scheduler for crossfire
|
||||
passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LAT);
|
||||
passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LON);
|
||||
passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::TEXT);
|
||||
passthrough->set_scheduler_entry_min_period(AP_Frsky_SPort_Passthrough::ATTITUDE, 350); // 3Hz
|
||||
|
||||
// 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
|
||||
|
||||
_telem_rf_mode = get_rf_mode();
|
||||
// setup custom telemetry for current rf_mode
|
||||
update_custom_telemetry_rates(_telem_rf_mode);
|
||||
|
||||
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: custom telem init done, fw %d.%02d", _crsf_version.major, _crsf_version.minor);
|
||||
_custom_telem.init_done = true;
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode)
|
||||
{
|
||||
// ignore rf mode changes if we are processing parameter packets
|
||||
if (_custom_telem.params_mode_active) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (is_high_speed_telemetry(rf_mode)) {
|
||||
// custom telemetry for high data rates
|
||||
set_scheduler_entry(GPS, 550, 500); // 2.0Hz
|
||||
set_scheduler_entry(PASSTHROUGH, 100, 100); // 10Hz
|
||||
set_scheduler_entry(STATUS_TEXT, 200, 750); // 1.5Hz
|
||||
} else {
|
||||
// custom telemetry for low data rates
|
||||
set_scheduler_entry(GPS, 550, 1000); // 1.0Hz
|
||||
set_scheduler_entry(PASSTHROUGH, 500, 3000); // 0.3Hz
|
||||
set_scheduler_entry(STATUS_TEXT, 600, 2000); // 0.5Hz
|
||||
}
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::process_rf_mode_changes()
|
||||
{
|
||||
const AP_RCProtocol_CRSF::RFMode current_rf_mode = get_rf_mode();
|
||||
if ( _telem_rf_mode != current_rf_mode) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "CRSF: rf mode change %d->%d, rate is %dHz", (uint8_t)_telem_rf_mode, (uint8_t)current_rf_mode, _scheduler.avg_packet_rate);
|
||||
update_custom_telemetry_rates(current_rf_mode);
|
||||
_telem_rf_mode = current_rf_mode;
|
||||
}
|
||||
}
|
||||
|
||||
// return custom frame id based on fw version
|
||||
uint8_t AP_CRSF_Telem::get_custom_telem_frame_id() const
|
||||
{
|
||||
if (!_crsf_version.pending && _crsf_version.major >= 4 && _crsf_version.minor >= 6) {
|
||||
return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM;
|
||||
}
|
||||
return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY;
|
||||
}
|
||||
|
||||
AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const
|
||||
{
|
||||
AP_RCProtocol_CRSF* crsf = AP::crsf();
|
||||
if (crsf == nullptr) {
|
||||
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_UNKNOWN;
|
||||
}
|
||||
|
||||
if (!_crsf_version.pending && _crsf_version.use_rf_mode) {
|
||||
return crsf->get_link_status().rf_mode;
|
||||
}
|
||||
|
||||
/*
|
||||
Note:
|
||||
- rf mode 2 on UARTS with DMA runs @160Hz
|
||||
- rf mode 2 on UARTS with no DMA runs @70Hz
|
||||
*/
|
||||
if (get_avg_packet_rate() < 40U) {
|
||||
// no DMA rf mode 1
|
||||
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
|
||||
}
|
||||
if (get_avg_packet_rate() > 120U) {
|
||||
// DMA rf mode 2
|
||||
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
|
||||
}
|
||||
if (get_max_packet_rate() < 120U) {
|
||||
// no DMA rf mode 2
|
||||
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
|
||||
}
|
||||
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
|
||||
}
|
||||
|
||||
bool AP_CRSF_Telem::is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const
|
||||
{
|
||||
return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
|
||||
{
|
||||
// no need to queue status text messages when crossfire
|
||||
// custom telemetry is not enabled
|
||||
if (!rc().crsf_custom_telemetry()) {
|
||||
return;
|
||||
}
|
||||
AP_RCTelemetry::queue_message(severity, 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
|
||||
|
||||
disable_scheduler_entry(PASSTHROUGH);
|
||||
disable_scheduler_entry(STATUS_TEXT);
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::exit_scheduler_params_mode()
|
||||
{
|
||||
// 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(PASSTHROUGH);
|
||||
enable_scheduler_entry(STATUS_TEXT);
|
||||
|
||||
update_custom_telemetry_rates(_telem_rf_mode);
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
|
||||
{
|
||||
uint32_t now_ms = AP_HAL::millis();
|
||||
setup_custom_telemetry();
|
||||
|
||||
/*
|
||||
whenever we detect a pending request we configure the scheduler
|
||||
to allow faster parameters processing.
|
||||
We start a "fast parameter window" that we close after 5sec
|
||||
*/
|
||||
bool expired = (now_ms - _custom_telem.params_mode_start_ms) > 5000;
|
||||
if (!_custom_telem.params_mode_active && _pending_request.frame_type > 0) {
|
||||
// fast window start
|
||||
_custom_telem.params_mode_start_ms = now_ms;
|
||||
_custom_telem.params_mode_active = true;
|
||||
enter_scheduler_params_mode();
|
||||
} else if (expired && _custom_telem.params_mode_active) {
|
||||
// fast window stop
|
||||
_custom_telem.params_mode_active = false;
|
||||
exit_scheduler_params_mode();
|
||||
}
|
||||
}
|
||||
|
||||
// WFQ scheduler
|
||||
bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
|
||||
{
|
||||
process_rf_mode_changes();
|
||||
|
||||
switch (idx) {
|
||||
case PARAMETERS:
|
||||
return _request_pending > 0;
|
||||
// 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,"CRSF: RX device ping failed");
|
||||
} else {
|
||||
_pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
|
||||
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
|
||||
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info");
|
||||
}
|
||||
}
|
||||
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;
|
||||
case PASSTHROUGH:
|
||||
return rc().crsf_custom_telemetry();
|
||||
case STATUS_TEXT:
|
||||
return rc().crsf_custom_telemetry() && !queue_empty;
|
||||
default:
|
||||
return _enable_telemetry;
|
||||
}
|
||||
|
@ -124,6 +317,20 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
|
|||
case FLIGHT_MODE: // GPS
|
||||
calc_flight_mode();
|
||||
break;
|
||||
case PASSTHROUGH:
|
||||
if (is_high_speed_telemetry(_telem_rf_mode)) {
|
||||
// on fast links we have 1:1 ratio between
|
||||
// passthrough frames and crossfire frames
|
||||
get_single_packet_passthrough_telem_data();
|
||||
} else {
|
||||
// on slower links we pack many passthrough
|
||||
// frames in a single crossfire one (up to 9)
|
||||
get_multi_packet_passthrough_telem_data();
|
||||
}
|
||||
break;
|
||||
case STATUS_TEXT:
|
||||
calc_status_text();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
@ -159,6 +366,10 @@ bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, voi
|
|||
process_param_write_frame((ParameterSettingsWriteFrame*)data);
|
||||
break;
|
||||
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
|
||||
process_device_info_frame((ParameterDeviceInfoFrame*)data);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
@ -245,7 +456,45 @@ void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping)
|
|||
}
|
||||
|
||||
_param_request.origin = ping->origin;
|
||||
_request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
|
||||
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
|
||||
}
|
||||
|
||||
// request for device info
|
||||
void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
|
||||
{
|
||||
debug("process_device_info_frame: %d -> %d", info->origin, info->destination);
|
||||
if (info->destination != 0 && info->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
|
||||
return; // request was not for us
|
||||
}
|
||||
|
||||
// we are only interested in RC device info for firmware version detection
|
||||
if (info->origin != 0 && info->origin != AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER) {
|
||||
return;
|
||||
}
|
||||
/*
|
||||
Payload size is 58:
|
||||
char[] Device name ( Null-terminated string, max len is 42 )
|
||||
uint32_t Serial number
|
||||
uint32_t Hardware ID
|
||||
uint32_t Firmware ID (0x00:0x00:0xAA:0xBB AA=major, BB=minor)
|
||||
uint8_t Parameters count
|
||||
uint8_t Parameter version number
|
||||
*/
|
||||
// get the terminator of the device name string
|
||||
const uint8_t offset = strnlen((char*)info->payload,42U);
|
||||
/*
|
||||
fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes
|
||||
fw minor ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 4th byte of sw id = 12bytes
|
||||
*/
|
||||
_crsf_version.major = info->payload[offset+11];
|
||||
_crsf_version.minor = info->payload[offset+12];
|
||||
|
||||
// should we use rf_mode reported by link statistics?
|
||||
if (_crsf_version.major >= 3 && _crsf_version.minor >= 72) {
|
||||
_crsf_version.use_rf_mode = true;
|
||||
}
|
||||
|
||||
_crsf_version.pending = false;
|
||||
}
|
||||
|
||||
void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame)
|
||||
|
@ -257,7 +506,7 @@ void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_fr
|
|||
}
|
||||
|
||||
_param_request = *read_frame;
|
||||
_request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ;
|
||||
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ;
|
||||
}
|
||||
|
||||
// process any changed settings and schedule for transmission
|
||||
|
@ -268,13 +517,16 @@ void AP_CRSF_Telem::update()
|
|||
void AP_CRSF_Telem::update_params()
|
||||
{
|
||||
// handle general parameter requests
|
||||
switch (_request_pending) {
|
||||
switch (_pending_request.frame_type) {
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
|
||||
calc_device_info();
|
||||
break;
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
|
||||
calc_parameter();
|
||||
break;
|
||||
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
|
||||
calc_device_ping();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
@ -495,11 +747,24 @@ void AP_CRSF_Telem::calc_device_info() {
|
|||
_telem_size = n + 2;
|
||||
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
|
||||
|
||||
_request_pending = 0;
|
||||
_pending_request.frame_type = 0;
|
||||
_telem_pending = true;
|
||||
#endif
|
||||
}
|
||||
|
||||
// send a device ping
|
||||
void AP_CRSF_Telem::calc_device_ping() {
|
||||
_telem.ext.ping.destination = _pending_request.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;
|
||||
}
|
||||
|
||||
// return parameter information
|
||||
void AP_CRSF_Telem::calc_parameter() {
|
||||
#if OSD_PARAM_ENABLED
|
||||
|
@ -601,7 +866,7 @@ void AP_CRSF_Telem::calc_parameter() {
|
|||
_telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
|
||||
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
|
||||
|
||||
_request_pending = 0;
|
||||
_pending_request.frame_type = 0;
|
||||
_telem_pending = true;
|
||||
#endif
|
||||
}
|
||||
|
@ -750,7 +1015,7 @@ void AP_CRSF_Telem::calc_text_selection(AP_OSD_ParamSetting* param, uint8_t chun
|
|||
_telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + chunker.bytes_written();
|
||||
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
|
||||
|
||||
_request_pending = 0;
|
||||
_pending_request.frame_type = 0;
|
||||
_telem_pending = true;
|
||||
}
|
||||
#endif
|
||||
|
@ -826,6 +1091,86 @@ void AP_CRSF_Telem::process_param_write_frame(ParameterSettingsWriteFrame* write
|
|||
#endif
|
||||
}
|
||||
|
||||
// get status text data
|
||||
void AP_CRSF_Telem::calc_status_text()
|
||||
{
|
||||
if (!_statustext.available) {
|
||||
WITH_SEMAPHORE(_statustext.sem);
|
||||
// check link speed
|
||||
if (!is_high_speed_telemetry(_telem_rf_mode)) {
|
||||
// keep only warning/error/critical/alert/emergency status text messages
|
||||
bool got_message = false;
|
||||
while (_statustext.queue.pop(_statustext.next)) {
|
||||
if (_statustext.next.severity <= MAV_SEVERITY_WARNING) {
|
||||
got_message = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!got_message) {
|
||||
return;
|
||||
}
|
||||
} else if (!_statustext.queue.pop(_statustext.next)) {
|
||||
return;
|
||||
}
|
||||
_statustext.available = true;
|
||||
}
|
||||
|
||||
_telem_type = get_custom_telem_frame_id();
|
||||
_telem.bcast.custom_telem.status_text.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_STATUS_TEXT;
|
||||
_telem.bcast.custom_telem.status_text.severity = _statustext.next.severity;
|
||||
strncpy_noterm(_telem.bcast.custom_telem.status_text.text, _statustext.next.text, PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE);
|
||||
// add a potentially missing terminator
|
||||
_telem.bcast.custom_telem.status_text.text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE-1] = 0;
|
||||
_telem_size = 2 + PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE; // sub_type(1) + severity(1) + text(50)
|
||||
_telem_pending = true;
|
||||
_statustext.available = false;
|
||||
}
|
||||
|
||||
/*
|
||||
Get 1 packet of passthrough telemetry data
|
||||
*/
|
||||
void AP_CRSF_Telem::get_single_packet_passthrough_telem_data()
|
||||
{
|
||||
_telem_pending = false;
|
||||
uint8_t packet_count;
|
||||
AP_Frsky_SPort::sport_packet_t packet;
|
||||
if (!AP_Frsky_Telem::get_telem_data(&packet, packet_count, 1)) {
|
||||
return;
|
||||
}
|
||||
_telem.bcast.custom_telem.single_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH;
|
||||
_telem.bcast.custom_telem.single_packet_passthrough.appid = packet.appid;
|
||||
_telem.bcast.custom_telem.single_packet_passthrough.data = packet.data;
|
||||
_telem_size = sizeof(AP_CRSF_Telem::PassthroughSinglePacketFrame);
|
||||
_telem_type = get_custom_telem_frame_id();
|
||||
_telem_pending = true;
|
||||
}
|
||||
|
||||
/*
|
||||
Get up to PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packets of passthrough telemetry data (for slow links)
|
||||
Note: we have 2 distinct frame types (single packet vs multi packet) because
|
||||
whenever possible we use smaller frames for they have a higher "chance"
|
||||
of being transmitted by the crossfire RX scheduler.
|
||||
*/
|
||||
void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data()
|
||||
{
|
||||
_telem_pending = false;
|
||||
uint8_t count = 0;
|
||||
AP_Frsky_SPort::sport_packet_t buffer[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {};
|
||||
// we request a PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packet array, i.e. 9 packets
|
||||
if (!AP_Frsky_Telem::get_telem_data(buffer, count, ARRAY_SIZE(buffer))) {
|
||||
return;
|
||||
}
|
||||
_telem.bcast.custom_telem.multi_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH;
|
||||
for (uint8_t idx=0; idx<count; idx++) {
|
||||
_telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].appid = buffer[idx].appid;
|
||||
_telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].data = buffer[idx].data;
|
||||
}
|
||||
_telem.bcast.custom_telem.multi_packet_passthrough.size = count;
|
||||
_telem_size = sizeof(AP_CRSF_Telem::PassthroughMultiPacketFrame);
|
||||
_telem_type = get_custom_telem_frame_id();
|
||||
_telem_pending = true;
|
||||
}
|
||||
|
||||
/*
|
||||
fetch CRSF frame data
|
||||
*/
|
||||
|
@ -842,7 +1187,6 @@ bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data)
|
|||
data->type = _telem_type;
|
||||
|
||||
_telem_pending = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
@ -49,29 +49,34 @@ public:
|
|||
virtual bool init() override;
|
||||
|
||||
static AP_CRSF_Telem *get_singleton(void);
|
||||
void queue_message(MAV_SEVERITY severity, const char *text) override;
|
||||
|
||||
static const uint8_t PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE = 50U;
|
||||
static const uint8_t PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE = 9U;
|
||||
static const uint8_t CRSF_RX_DEVICE_PING_MAX_RETRY = 50U;
|
||||
|
||||
// Broadcast frame definitions courtesy of TBS
|
||||
struct GPSFrame { // curious fact, calling this GPS makes sizeof(GPS) return 1!
|
||||
struct PACKED GPSFrame { // curious fact, calling this GPS makes sizeof(GPS) return 1!
|
||||
int32_t latitude; // ( degree / 10`000`000 )
|
||||
int32_t longitude; // (degree / 10`000`000 )
|
||||
uint16_t groundspeed; // ( km/h / 100 )
|
||||
uint16_t gps_heading; // ( degree / 100 )
|
||||
uint16_t altitude; // ( meter - 1000m offset )
|
||||
uint8_t satellites; // in use ( counter )
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
struct HeartbeatFrame {
|
||||
uint8_t origin; // Device addres
|
||||
};
|
||||
|
||||
struct BatteryFrame {
|
||||
struct PACKED BatteryFrame {
|
||||
uint16_t voltage; // ( mV * 100 )
|
||||
uint16_t current; // ( mA * 100 )
|
||||
uint8_t capacity[3]; // ( mAh )
|
||||
uint8_t remaining; // ( percent )
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
struct VTXFrame {
|
||||
struct PACKED VTXFrame {
|
||||
#if __BYTE_ORDER != __LITTLE_ENDIAN
|
||||
#error "Only supported on little-endian architectures"
|
||||
#endif
|
||||
|
@ -88,45 +93,45 @@ public:
|
|||
uint16_t user_frequency;
|
||||
uint8_t power : 4; // 25mW = 0, 200mW = 1, 500mW = 2, 800mW = 3
|
||||
uint8_t pitmode : 4; // off = 0, In_Band = 1, Out_Band = 2;
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
struct VTXTelemetryFrame {
|
||||
struct PACKED VTXTelemetryFrame {
|
||||
uint8_t origin; // address
|
||||
uint8_t power; // power in dBm
|
||||
uint16_t frequency; // frequency in Mhz
|
||||
uint8_t pitmode; // disable 0, enable 1
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
struct AttitudeFrame {
|
||||
struct PACKED AttitudeFrame {
|
||||
int16_t pitch_angle; // ( rad * 10000 )
|
||||
int16_t roll_angle; // ( rad * 10000 )
|
||||
int16_t yaw_angle; // ( rad * 10000 )
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
struct FlightModeFrame {
|
||||
struct PACKED FlightModeFrame {
|
||||
char flight_mode[16]; // ( Null-terminated string )
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
// CRSF_FRAMETYPE_COMMAND
|
||||
struct CommandFrame {
|
||||
struct PACKED CommandFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t command_id;
|
||||
uint8_t payload[9]; // 8 maximum for LED command + crc8
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
// CRSF_FRAMETYPE_PARAM_DEVICE_PING
|
||||
struct ParameterPingFrame {
|
||||
struct PACKED ParameterPingFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
// CRSF_FRAMETYPE_PARAM_DEVICE_INFO
|
||||
struct ParameterDeviceInfoFrame {
|
||||
struct PACKED ParameterDeviceInfoFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t payload[58]; // largest possible frame is 60
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
enum ParameterType : uint8_t
|
||||
{
|
||||
|
@ -144,57 +149,90 @@ public:
|
|||
};
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
|
||||
struct ParameterSettingsEntryHeader {
|
||||
struct PACKED ParameterSettingsEntryHeader {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t param_num;
|
||||
uint8_t chunks_left;
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
|
||||
struct ParameterSettingsEntry {
|
||||
struct PACKED ParameterSettingsEntry {
|
||||
ParameterSettingsEntryHeader header;
|
||||
uint8_t payload[56]; // largest possible frame is 60
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_READ
|
||||
struct ParameterSettingsReadFrame {
|
||||
struct PACKED ParameterSettingsReadFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t param_num;
|
||||
uint8_t param_chunk;
|
||||
} PACKED _param_request;
|
||||
} _param_request;
|
||||
|
||||
// CRSF_FRAMETYPE_PARAMETER_WRITE
|
||||
struct ParameterSettingsWriteFrame {
|
||||
struct PACKED ParameterSettingsWriteFrame {
|
||||
uint8_t destination;
|
||||
uint8_t origin;
|
||||
uint8_t param_num;
|
||||
uint8_t payload[57]; // largest possible frame is 60
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
union BroadcastFrame {
|
||||
// Frame to hold passthrough telemetry
|
||||
struct PACKED PassthroughSinglePacketFrame {
|
||||
uint8_t sub_type;
|
||||
uint16_t appid;
|
||||
uint32_t data;
|
||||
};
|
||||
|
||||
// Frame to hold passthrough telemetry
|
||||
struct PACKED PassthroughMultiPacketFrame {
|
||||
uint8_t sub_type;
|
||||
uint8_t size;
|
||||
struct PACKED {
|
||||
uint16_t appid;
|
||||
uint32_t data;
|
||||
} frames[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE];
|
||||
};
|
||||
|
||||
// Frame to hold status text message
|
||||
struct PACKED StatusTextFrame {
|
||||
uint8_t sub_type;
|
||||
uint8_t severity;
|
||||
char text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE]; // ( Null-terminated string )
|
||||
};
|
||||
|
||||
// ardupilot frametype container
|
||||
union PACKED APCustomTelemFrame {
|
||||
PassthroughSinglePacketFrame single_packet_passthrough;
|
||||
PassthroughMultiPacketFrame multi_packet_passthrough;
|
||||
StatusTextFrame status_text;
|
||||
};
|
||||
|
||||
|
||||
union PACKED BroadcastFrame {
|
||||
GPSFrame gps;
|
||||
HeartbeatFrame heartbeat;
|
||||
BatteryFrame battery;
|
||||
VTXFrame vtx;
|
||||
AttitudeFrame attitude;
|
||||
FlightModeFrame flightmode;
|
||||
} PACKED;
|
||||
APCustomTelemFrame custom_telem;
|
||||
};
|
||||
|
||||
union ExtendedFrame {
|
||||
union PACKED ExtendedFrame {
|
||||
CommandFrame command;
|
||||
ParameterPingFrame ping;
|
||||
ParameterDeviceInfoFrame info;
|
||||
ParameterSettingsEntry param_entry;
|
||||
ParameterSettingsReadFrame param_read;
|
||||
ParameterSettingsWriteFrame param_write;
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
union TelemetryPayload {
|
||||
union PACKED TelemetryPayload {
|
||||
BroadcastFrame bcast;
|
||||
ExtendedFrame ext;
|
||||
} PACKED;
|
||||
};
|
||||
|
||||
// Process a frame from the CRSF protocol decoder
|
||||
static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
|
||||
|
@ -213,6 +251,8 @@ private:
|
|||
BATTERY,
|
||||
GPS,
|
||||
FLIGHT_MODE,
|
||||
PASSTHROUGH,
|
||||
STATUS_TEXT,
|
||||
NUM_SENSORS
|
||||
};
|
||||
|
||||
|
@ -220,6 +260,8 @@ private:
|
|||
bool is_packet_ready(uint8_t idx, bool queue_empty) override;
|
||||
void process_packet(uint8_t idx) override;
|
||||
void adjust_packet_weight(bool queue_empty) override;
|
||||
void setup_custom_telemetry();
|
||||
void update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode);
|
||||
|
||||
void calc_parameter_ping();
|
||||
void calc_heartbeat();
|
||||
|
@ -228,33 +270,65 @@ private:
|
|||
void calc_attitude();
|
||||
void calc_flight_mode();
|
||||
void calc_device_info();
|
||||
void calc_device_ping();
|
||||
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 update_vtx_params();
|
||||
void get_single_packet_passthrough_telem_data();
|
||||
void get_multi_packet_passthrough_telem_data();
|
||||
void calc_status_text();
|
||||
void process_rf_mode_changes();
|
||||
uint8_t get_custom_telem_frame_id() const;
|
||||
AP_RCProtocol_CRSF::RFMode get_rf_mode() const;
|
||||
bool is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const;
|
||||
|
||||
void process_vtx_frame(VTXFrame* vtx);
|
||||
void process_vtx_telem_frame(VTXTelemetryFrame* vtx);
|
||||
void process_ping_frame(ParameterPingFrame* ping);
|
||||
void process_param_read_frame(ParameterSettingsReadFrame* read);
|
||||
void process_param_write_frame(ParameterSettingsWriteFrame* write);
|
||||
void process_device_info_frame(ParameterDeviceInfoFrame* info);
|
||||
|
||||
// setup ready for passthrough operation
|
||||
// setup ready for passthrough operation
|
||||
void setup_wfq_scheduler(void) override;
|
||||
|
||||
// get next telemetry data for external consumers
|
||||
// setup the scheduler for parameters download
|
||||
void enter_scheduler_params_mode();
|
||||
void exit_scheduler_params_mode();
|
||||
|
||||
// get next telemetry data for external consumers
|
||||
bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data);
|
||||
bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
|
||||
|
||||
TelemetryPayload _telem;
|
||||
uint8_t _telem_size;
|
||||
uint8_t _telem_type;
|
||||
AP_RCProtocol_CRSF::RFMode _telem_rf_mode;
|
||||
|
||||
bool _telem_pending;
|
||||
bool _enable_telemetry;
|
||||
uint8_t _request_pending;
|
||||
|
||||
struct {
|
||||
uint8_t destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST;
|
||||
uint8_t frame_type;
|
||||
} _pending_request;
|
||||
|
||||
struct {
|
||||
uint8_t minor;
|
||||
uint8_t major;
|
||||
uint8_t retry_count;
|
||||
bool use_rf_mode;
|
||||
bool pending = true;
|
||||
} _crsf_version;
|
||||
|
||||
struct {
|
||||
bool init_done;
|
||||
uint32_t params_mode_start_ms;
|
||||
bool params_mode_active;
|
||||
} _custom_telem;
|
||||
|
||||
// vtx state
|
||||
bool _vtx_freq_update; // update using the frequency method or not
|
||||
|
|
|
@ -65,7 +65,7 @@ void AP_RCTelemetry::update_avg_packet_rate()
|
|||
// initialize
|
||||
if (_scheduler.avg_packet_rate == 0) _scheduler.avg_packet_rate = _scheduler.avg_packet_counter;
|
||||
// moving average
|
||||
_scheduler.avg_packet_rate = (uint8_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f;
|
||||
_scheduler.avg_packet_rate = (uint16_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f;
|
||||
// reset
|
||||
_scheduler.last_poll_timer = poll_now;
|
||||
_scheduler.avg_packet_counter = 0;
|
||||
|
@ -83,10 +83,12 @@ void AP_RCTelemetry::update_avg_packet_rate()
|
|||
|
||||
/*
|
||||
* WFQ scheduler
|
||||
* returns the actual packet type index (if any) sent by the scheduler
|
||||
*/
|
||||
void AP_RCTelemetry::run_wfq_scheduler(void)
|
||||
uint8_t AP_RCTelemetry::run_wfq_scheduler(const bool use_shaper)
|
||||
{
|
||||
update_avg_packet_rate();
|
||||
update_max_packet_rate();
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
int8_t max_delay_idx = -1;
|
||||
|
@ -116,8 +118,8 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
|
|||
// use >= so with equal delays we choose the packet with lowest priority
|
||||
// this is ensured by the packets being sorted by desc frequency
|
||||
// apply the rate limiter
|
||||
if (delay >= max_delay && ((now - _scheduler.packet_timer[i]) >= _scheduler.packet_min_period[i])) {
|
||||
packet_ready = is_packet_ready(i, queue_empty);
|
||||
if (delay >= max_delay && check_scheduler_entry_time_constraints(now, i, use_shaper)) {
|
||||
packet_ready = is_scheduler_entry_enabled(i) && is_packet_ready(i, queue_empty);
|
||||
|
||||
if (packet_ready) {
|
||||
max_delay = delay;
|
||||
|
@ -125,10 +127,10 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (max_delay_idx < 0) { // nothing was ready
|
||||
return;
|
||||
return max_delay_idx;
|
||||
}
|
||||
|
||||
now = AP_HAL::millis();
|
||||
#ifdef TELEM_DEBUG
|
||||
_scheduler.packet_rate[max_delay_idx] = (_scheduler.packet_rate[max_delay_idx] + 1000 / (now - _scheduler.packet_timer[max_delay_idx])) / 2;
|
||||
|
@ -137,6 +139,32 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
|
|||
//debug("process_packet(%d): %f", max_delay_idx, max_delay);
|
||||
// send packet
|
||||
process_packet(max_delay_idx);
|
||||
// let the caller know which packet type was sent
|
||||
return max_delay_idx;
|
||||
}
|
||||
|
||||
/*
|
||||
* do not run the scheduler and process a specific entry
|
||||
*/
|
||||
bool AP_RCTelemetry::process_scheduler_entry(const uint8_t slot )
|
||||
{
|
||||
if (slot >= TELEM_TIME_SLOT_MAX) {
|
||||
return false;
|
||||
}
|
||||
if (!is_scheduler_entry_enabled(slot)) {
|
||||
return false;
|
||||
}
|
||||
bool queue_empty;
|
||||
{
|
||||
WITH_SEMAPHORE(_statustext.sem);
|
||||
queue_empty = !_statustext.available && _statustext.queue.is_empty();
|
||||
}
|
||||
if (!is_packet_ready(slot, queue_empty)) {
|
||||
return false;
|
||||
}
|
||||
process_packet(slot);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -258,4 +286,3 @@ uint32_t AP_RCTelemetry::sensor_status_flags() const
|
|||
|
||||
return ~health & enabled & present;
|
||||
}
|
||||
|
||||
|
|
|
@ -17,6 +17,7 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <AP_HAL/utility/RingBuffer.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#define TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent)
|
||||
|
||||
|
@ -34,23 +35,66 @@ public:
|
|||
AP_RCTelemetry &operator=(const AP_RCTelemetry&) = delete;
|
||||
|
||||
// add statustext message to message queue
|
||||
void queue_message(MAV_SEVERITY severity, const char *text);
|
||||
virtual void queue_message(MAV_SEVERITY severity, const char *text);
|
||||
|
||||
// scheduler entry helpers
|
||||
void enable_scheduler_entry(const uint8_t slot) {
|
||||
if (slot >= TELEM_TIME_SLOT_MAX) {
|
||||
return;
|
||||
}
|
||||
BIT_CLEAR(_disabled_scheduler_entries_bitmask, slot);
|
||||
}
|
||||
void disable_scheduler_entry(const uint8_t slot) {
|
||||
if (slot >= TELEM_TIME_SLOT_MAX) {
|
||||
return;
|
||||
}
|
||||
BIT_SET(_disabled_scheduler_entries_bitmask, slot);
|
||||
}
|
||||
void set_scheduler_entry_min_period(const uint8_t slot, const uint32_t min_period_ms)
|
||||
{
|
||||
if (slot >= TELEM_TIME_SLOT_MAX) {
|
||||
return;
|
||||
}
|
||||
_scheduler.packet_min_period[slot] = min_period_ms;
|
||||
}
|
||||
bool is_scheduler_entry_enabled(const uint8_t slot) const {
|
||||
if (slot >= TELEM_TIME_SLOT_MAX) {
|
||||
return false;
|
||||
}
|
||||
return !BIT_IS_SET(_disabled_scheduler_entries_bitmask, slot);
|
||||
}
|
||||
// each derived class might provide a way to reset telemetry rates to default
|
||||
virtual void reset_scheduler_entry_min_periods() {}
|
||||
|
||||
// update error mask of sensors and subsystems. The mask uses the
|
||||
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
|
||||
// indicates that the sensor or subsystem is present but not
|
||||
// functioning correctly
|
||||
uint32_t sensor_status_flags() const;
|
||||
uint16_t get_avg_packet_rate() const {
|
||||
return _scheduler.avg_packet_rate;
|
||||
}
|
||||
uint16_t get_max_packet_rate() const {
|
||||
return _scheduler.max_packet_rate;
|
||||
}
|
||||
|
||||
protected:
|
||||
void run_wfq_scheduler();
|
||||
uint8_t run_wfq_scheduler(const bool use_shaper = true);
|
||||
// process a specific entry
|
||||
bool process_scheduler_entry(const uint8_t slot );
|
||||
// set an entry in the scheduler table
|
||||
void set_scheduler_entry(uint8_t slot, uint32_t weight, uint32_t min_period_ms) {
|
||||
if (slot >= TELEM_TIME_SLOT_MAX) {
|
||||
return;
|
||||
}
|
||||
_scheduler.packet_weight[slot] = weight;
|
||||
_scheduler.packet_min_period[slot] = min_period_ms;
|
||||
}
|
||||
// add an entry to the scheduler table
|
||||
void add_scheduler_entry(uint32_t weight, uint32_t min_period_ms) {
|
||||
if (_time_slots >= TELEM_TIME_SLOT_MAX) {
|
||||
return;
|
||||
}
|
||||
set_scheduler_entry(_time_slots++, weight, min_period_ms);
|
||||
}
|
||||
// setup ready for passthrough operation
|
||||
|
@ -65,7 +109,8 @@ protected:
|
|||
uint32_t packet_timer[TELEM_TIME_SLOT_MAX];
|
||||
uint32_t packet_weight[TELEM_TIME_SLOT_MAX];
|
||||
uint32_t packet_min_period[TELEM_TIME_SLOT_MAX];
|
||||
uint8_t avg_packet_rate;
|
||||
uint16_t avg_packet_rate;
|
||||
uint16_t max_packet_rate;
|
||||
#ifdef TELEM_DEBUG
|
||||
uint8_t packet_rate[TELEM_TIME_SLOT_MAX];
|
||||
#endif
|
||||
|
@ -81,6 +126,7 @@ protected:
|
|||
private:
|
||||
uint32_t check_sensor_status_timer;
|
||||
uint32_t check_ekf_status_timer;
|
||||
uint32_t _disabled_scheduler_entries_bitmask;
|
||||
|
||||
// passthrough WFQ scheduler
|
||||
virtual void setup_wfq_scheduler() = 0;
|
||||
|
@ -88,8 +134,17 @@ private:
|
|||
virtual bool is_packet_ready(uint8_t idx, bool queue_empty) { return true; }
|
||||
virtual void process_packet(uint8_t idx) = 0;
|
||||
virtual void adjust_packet_weight(bool queue_empty) {};
|
||||
bool check_scheduler_entry_time_constraints(const uint32_t now, uint8_t slot, const bool use_shaper) const {
|
||||
if (!use_shaper) {
|
||||
return true;
|
||||
}
|
||||
return ((now - _scheduler.packet_timer[slot]) >= _scheduler.packet_min_period[slot]);
|
||||
}
|
||||
|
||||
void update_avg_packet_rate();
|
||||
void update_max_packet_rate() {
|
||||
_scheduler.max_packet_rate = MAX(_scheduler.avg_packet_rate, _scheduler.max_packet_rate);
|
||||
}
|
||||
|
||||
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
|
||||
void check_sensor_status_flags(void);
|
||||
|
|
Loading…
Reference in New Issue