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

AP_RCTelemetry: added support for passthrough telemetry over crossfire

This commit is contained in:
yaapu 2020-12-10 10:28:20 +01:00 committed by Peter Barker
parent 9db0862d61
commit 4933544489
4 changed files with 560 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

362
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -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;
}

144
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -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

53
libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
View File

@ -13,7 +13,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
/*
Abstract Telemetry library
*/
@ -60,12 +60,12 @@ void AP_RCTelemetry::update_avg_packet_rate()
uint32_t poll_now = AP_HAL::millis();
_scheduler.avg_packet_counter++;
if (poll_now - _scheduler.last_poll_timer > 1000) { //average in last 1000ms
// 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,14 +83,16 @@ 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;
float max_delay = 0;
float delay = 0;
bool packet_ready = false;
@ -99,7 +101,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
check_sensor_status_flags();
// build message queue for ekf_status
check_ekf_status();
// dynamic priorities
bool queue_empty;
{
@ -108,7 +110,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
}
adjust_packet_weight(queue_empty);
// search the packet with the longest delay after the scheduled time
for (int i=0; i<_time_slots; i++) {
// normalize packet delay relative to packet weight
@ -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;
}
/*
@ -248,7 +276,7 @@ void AP_RCTelemetry::check_ekf_status(void)
}
}
}
uint32_t AP_RCTelemetry::sensor_status_flags() const
{
uint32_t present;
@ -258,4 +286,3 @@ uint32_t AP_RCTelemetry::sensor_status_flags() const
return ~health & enabled & present;
}

61
libraries/AP_RCTelemetry/AP_RCTelemetry.h
View File

@ -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);