zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Ap_Frsky_Telem: replaced the passthrough scheduler with a WFQ one. 

This replaces the default scheduler with a WFQ one
master
yaapu 6 years ago committed by Randy Mackay
parent
98263f429a
commit
3640568231
2 changed files with 219 additions and 114 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 280
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
  2. 53
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

280
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
Unescape View File

@ -1,6 +1,8 @@ @@ -1,6 +1,8 @@
/*
Inspired by work done here https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado <px4@sradonia.net>
Inspired by work done here
https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado <px4@sradonia.net>
https://github.com/opentx/opentx/tree/2.3/radio/src/telemetry from the OpenTX team
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@ -29,7 +31,9 @@ @@ -29,7 +31,9 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_Common/Location.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Logger/AP_Logger.h>
#include <stdio.h>
#include <math.h>
extern const AP_HAL::HAL& hal;
@ -62,6 +66,21 @@ bool AP_Frsky_Telem::init() @@ -62,6 +66,21 @@ bool AP_Frsky_Telem::init()
snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string);
queue_message(MAV_SEVERITY_INFO, firmware_buf);
}
// initialize packet weights for the WFQ scheduler
// weight[i] = 1/_passthrough.packet_period[i]
// rate[i] = LinkRate * ( weight[i] / (sum(weight[1-n])) )
_passthrough.packet_weight[0] = 35; // 0x5000 status text (dynamic)
_passthrough.packet_weight[1] = 50; // 0x5006 Attitude and range (dynamic)
_passthrough.packet_weight[2] = 550; // 0x800 GPS lat (600 with 1 sensor)
_passthrough.packet_weight[3] = 550; // 0x800 GPS lon (600 with 1 sensor)
_passthrough.packet_weight[4] = 400; // 0x5005 Vel and Yaw
_passthrough.packet_weight[5] = 700; // 0x5001 AP status
_passthrough.packet_weight[6] = 700; // 0x5002 GPS Status
_passthrough.packet_weight[7] = 400; // 0x5004 Home
_passthrough.packet_weight[8] = 1300; // 0x5008 Battery 2 status
_passthrough.packet_weight[9] = 1300; // 0x5003 Battery 1 status
_passthrough.packet_weight[10] = 1700; // 0x5007 parameters
}
if (_port != nullptr) {
@ -78,6 +97,127 @@ bool AP_Frsky_Telem::init() @@ -78,6 +97,127 @@ bool AP_Frsky_Telem::init()
return false;
}
void AP_Frsky_Telem::update_avg_packet_rate()
{
uint32_t poll_now = AP_HAL::millis();
_passthrough.avg_packet_counter++;
if (poll_now - _passthrough.last_poll_timer > 1000) { //average in last 1000ms
// initialize
if (_passthrough.avg_packet_rate == 0) _passthrough.avg_packet_rate = _passthrough.avg_packet_counter;
// moving average
_passthrough.avg_packet_rate = (uint8_t)_passthrough.avg_packet_rate * 0.75 + _passthrough.avg_packet_counter * 0.25;
// reset
_passthrough.last_poll_timer = poll_now;
_passthrough.avg_packet_counter = 0;
}
}
/*
* WFQ scheduler
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
void AP_Frsky_Telem::passthrough_wfq_adaptive_scheduler(uint8_t prev_byte)
{
uint32_t now = AP_HAL::millis();
uint8_t max_delay_idx = TIME_SLOT_MAX;
float max_delay = 0;
float delay = 0;
bool packet_ready = false;
// build message queue for sensor_status_flags
check_sensor_status_flags();
// build message queue for ekf_status
check_ekf_status();
// dynamic priorities
if (!_statustext_queue.empty()) {
_passthrough.packet_weight[0] = 45; // messages
_passthrough.packet_weight[1] = 80; // attitude
} else {
_passthrough.packet_weight[0] = 5000; // messages
_passthrough.packet_weight[1] = 45; // attitude
}
// search the packet with the longest delay after the scheduled time
for (int i=0;i<TIME_SLOT_MAX;i++) {
//normalize packet delay relative to packet weight
delay = (now - _passthrough.packet_timer[i])/static_cast<float>(_passthrough.packet_weight[i]);
// 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 - _passthrough.packet_timer[i]) >= _sport_config.packet_min_period[i])) {
switch (i) {
case 0:
packet_ready = !_statustext_queue.empty();
break;
case 5:
packet_ready = gcs().vehicle_initialised();
break;
case 8:
packet_ready = AP::battery().num_instances() > 1;
break;
default:
packet_ready = true;
break;
}
if (packet_ready) {
max_delay = delay;
max_delay_idx = i;
}
}
}
_passthrough.packet_timer[max_delay_idx] = AP_HAL::millis();
// send packet
switch (max_delay_idx) {
case TIME_SLOT_MAX: // nothing to send
break;
case 0: // 0x5000 status text
if (get_next_msg_chunk()) {
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk);
}
break;
case 1: // 0x5006 Attitude and range
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng());
break;
case 2: // 0x800 GPS lat
// sample both lat and lon at the same time
send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
_passthrough.gps_lng_sample = calc_gps_latlng(&_passthrough.send_latitude);
// force the scheduler to select GPS lon as packet that's been waiting the most
// this guarantees that gps coords are sent at max
// _passthrough.avg_polling_period*number_of_downlink_sensors time separation
_passthrough.packet_timer[3] = _passthrough.packet_timer[2] - 10000;
break;
case 3: // 0x800 GPS lon
send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude
break;
case 4: // 0x5005 Vel and Yaw
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw());
break;
case 5: // 0x5001 AP status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status());
break;
case 6: // 0x5002 GPS Status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status());
break;
case 7: // 0x5004 Home
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home());
break;
case 8: // 0x5008 Battery 2 status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1));
break;
case 9: // 0x5003 Battery 1 status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0));
break;
case 10: // 0x5007 parameters
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break;
}
}
/*
* send telemetry data
@ -97,83 +237,16 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) @@ -97,83 +237,16 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
if (_port->txspace() < 19) {
return;
}
// keep only the last two bytes of the data found in the serial buffer, as we shouldn't respond to old poll requests
uint8_t prev_byte = 0;
for (int16_t i = 0; i < numc; i++) {
prev_byte = _passthrough.new_byte;
_passthrough.new_byte = _port->read();
}
if ((prev_byte == START_STOP_SPORT) && (_passthrough.new_byte == SENSOR_ID_28)) { // byte 0x7E is the header of each poll request
if (_passthrough.send_chunk) { // skip other data and send a message chunk during this iteration
_passthrough.send_chunk = false;
if (get_next_msg_chunk()) {
send_uint32(DIY_FIRST_ID, _msg_chunk.chunk);
}
} else {
// build message queue for sensor_status_flags
check_sensor_status_flags();
// build message queue for ekf_status
check_ekf_status();
// if there are pending messages in the queue, send them during next iteration
if (!_statustext_queue.empty()) {
_passthrough.send_chunk = true;
}
if (_passthrough.send_attiandrng) { // skip other data, send attitude (roll, pitch) and range only this iteration
_passthrough.send_attiandrng = false; // next iteration, check if we should send something other
} else { // send other sensor data if it's time for them, and reset the corresponding timer if sent
_passthrough.send_attiandrng = true; // next iteration, send attitude b/c it needs frequent updates to remain smooth
uint32_t now = AP_HAL::millis();
if ((now - _passthrough.params_timer) >= 1000) {
send_uint32(DIY_FIRST_ID+7, calc_param());
_passthrough.params_timer = AP_HAL::millis();
return;
}
if ((now - _passthrough.ap_status_timer) >= 500) {
if (gcs().vehicle_initialised()) { // send ap status only once vehicle has been initialised
send_uint32(DIY_FIRST_ID+1, calc_ap_status());
_passthrough.ap_status_timer = AP_HAL::millis();
return;
}
}
if ((now - _passthrough.batt_timer) >= 1000) {
send_uint32(DIY_FIRST_ID+3, calc_batt(0));
_passthrough.batt_timer = AP_HAL::millis();
return;
}
if (AP::battery().num_instances() > 1) {
if ((now - _passthrough.batt_timer2) >= 1000) {
send_uint32(DIY_FIRST_ID+8, calc_batt(1));
_passthrough.batt_timer2 = AP_HAL::millis();
return;
}
}
if ((now - _passthrough.gps_status_timer) >= 1000) {
send_uint32(DIY_FIRST_ID+2, calc_gps_status());
_passthrough.gps_status_timer = AP_HAL::millis();
return;
}
if ((now - _passthrough.home_timer) >= 500) {
send_uint32(DIY_FIRST_ID+4, calc_home());
_passthrough.home_timer = AP_HAL::millis();
return;
}
if ((now - _passthrough.velandyaw_timer) >= 500) {
send_uint32(DIY_FIRST_ID+5, calc_velandyaw());
_passthrough.velandyaw_timer = AP_HAL::millis();
return;
}
if ((now - _passthrough.gps_latlng_timer) >= 1000) {
send_uint32(GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
if (!_passthrough.send_latitude) { // we've cycled and sent one each of longitude then latitude, so reset the timer
_passthrough.gps_latlng_timer = AP_HAL::millis();
}
return;
}
}
// if nothing else needed to be sent, send attitude (roll, pitch) and range data
send_uint32(DIY_FIRST_ID+6, calc_attiandrng());
if (prev_byte == START_STOP_SPORT) {
if (_passthrough.new_byte == SENSOR_ID_28) { // byte 0x7E is the header of each poll request
update_avg_packet_rate();
passthrough_wfq_adaptive_scheduler(prev_byte);
}
}
}
@ -211,10 +284,10 @@ void AP_Frsky_Telem::send_SPort(void) @@ -211,10 +284,10 @@ void AP_Frsky_Telem::send_SPort(void)
case SENSOR_ID_FAS:
switch (_SPort.fas_call) {
case 0:
send_uint32(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
send_uint32(SPORT_DATA_FRAME, DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
break;
case 1:
send_uint32(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
send_uint32(SPORT_DATA_FRAME, DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
break;
case 2:
{
@ -222,9 +295,10 @@ void AP_Frsky_Telem::send_SPort(void) @@ -222,9 +295,10 @@ void AP_Frsky_Telem::send_SPort(void)
if (!_battery.current_amps(current)) {
current = 0;
}
send_uint32(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
send_uint32(SPORT_DATA_FRAME, DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
break;
}
}
break;
}
if (_SPort.fas_call++ > 2) _SPort.fas_call = 0;
break;
@ -232,37 +306,37 @@ void AP_Frsky_Telem::send_SPort(void) @@ -232,37 +306,37 @@ void AP_Frsky_Telem::send_SPort(void)
switch (_SPort.gps_call) {
case 0:
calc_gps_position(); // gps data is not recalculated until all of it has been sent
send_uint32(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
break;
case 1:
send_uint32(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
break;
case 2:
send_uint32(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
break;
case 3:
send_uint32(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
break;
case 4:
send_uint32(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
break;
case 5:
send_uint32(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
break;
case 6:
send_uint32(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
break;
case 7:
send_uint32(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
break;
case 8:
send_uint32(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
break;
case 9:
send_uint32(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals
break;
case 10:
send_uint32(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
break;
}
if (_SPort.gps_call++ > 10) _SPort.gps_call = 0;
@ -271,10 +345,10 @@ void AP_Frsky_Telem::send_SPort(void) @@ -271,10 +345,10 @@ void AP_Frsky_Telem::send_SPort(void)
switch (_SPort.vario_call) {
case 0 :
calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
send_uint32(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part
send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part
break;
case 1:
send_uint32(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part
send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part
break;
}
if (_SPort.vario_call++ > 1) _SPort.vario_call = 0;
@ -282,10 +356,10 @@ void AP_Frsky_Telem::send_SPort(void) @@ -282,10 +356,10 @@ void AP_Frsky_Telem::send_SPort(void)
case SENSOR_ID_SP2UR:
switch (_SPort.various_call) {
case 0 :
send_uint32(DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
break;
case 1:
send_uint32(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
break;
}
if (_SPort.various_call++ > 1) _SPort.various_call = 0;
@ -319,7 +393,7 @@ void AP_Frsky_Telem::send_D(void) @@ -319,7 +393,7 @@ void AP_Frsky_Telem::send_D(void)
if (!_battery.current_amps(current)) {
current = 0;
}
send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
calc_nav_alt();
send_uint16(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part
send_uint16(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part
@ -423,9 +497,9 @@ void AP_Frsky_Telem::send_byte(uint8_t byte) @@ -423,9 +497,9 @@ void AP_Frsky_Telem::send_byte(uint8_t byte)
/*
* send one uint32 frame of FrSky data - for FrSky SPort protocol (X-receivers)
*/
void AP_Frsky_Telem::send_uint32(uint16_t id, uint32_t data)
void AP_Frsky_Telem::send_uint32(uint8_t frame, uint16_t id, uint32_t data)
{
send_byte(0x10); // DATA_FRAME
send_byte(frame); // frame type
uint8_t *bytes = (uint8_t*)&id;
send_byte(bytes[0]); // LSB
send_byte(bytes[1]); // MSB
@ -483,7 +557,21 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void) @@ -483,7 +557,21 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void)
}
}
if (_msg_chunk.repeats++ > 2) { // repeat each message chunk 3 times to ensure transmission
// repeat each message chunk 3 times to ensure transmission
// on slow links reduce the number of duplicate chunks
uint8_t extra_chunks = 2;
if (_passthrough.avg_packet_rate < 20) {
// with 3 or more extra frsky sensors on the bus
// send messages only once
extra_chunks = 0;
} else if (_passthrough.avg_packet_rate < 30) {
// with 1 or 2 extra frsky sensors on the bus
// send messages twice
extra_chunks = 1;
}
if (_msg_chunk.repeats++ > extra_chunks ) {
_msg_chunk.repeats = 0;
if (_msg_chunk.char_index == 0) { // if we're ready for the next message
_statustext_queue.remove(0);
@ -740,6 +828,8 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void) @@ -740,6 +828,8 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void)
ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET;
// IMU temperature
ap_status |= imu_temp << AP_IMU_TEMP_OFFSET;
//hal.console->printf("flying=%d\n",AP_Notify::flags.flying);
//hal.console->printf("ap_status=%08X\n",ap_status);
return ap_status;
}

53
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
Unescape View File

@ -63,7 +63,7 @@ for FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers) @@ -63,7 +63,7 @@ for FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
#define START_STOP_SPORT 0x7E
#define BYTESTUFF_SPORT 0x7D
#define SPORT_DATA_FRAME 0x10
/*
for FrSky SPort Passthrough
*/
@ -107,8 +107,8 @@ for FrSky SPort Passthrough @@ -107,8 +107,8 @@ for FrSky SPort Passthrough
#define ATTIANDRNG_PITCH_LIMIT 0x3FF
#define ATTIANDRNG_PITCH_OFFSET 11
#define ATTIANDRNG_RNGFND_OFFSET 21
// for fair scheduler
#define TIME_SLOT_MAX 11
class AP_Frsky_Telem {
public:
@ -138,9 +138,8 @@ private: @@ -138,9 +138,8 @@ private:
uint32_t check_sensor_status_timer;
uint32_t check_ekf_status_timer;
uint8_t _paramID;
ObjectArray<mavlink_statustext_t> _statustext_queue;
struct
{
@ -157,22 +156,36 @@ private: @@ -157,22 +156,36 @@ private:
uint16_t speed_in_centimeter;
} _gps;
struct
struct PACKED
{
bool send_latitude; // sizeof(bool) = 4 ?
uint32_t gps_lng_sample;
uint32_t last_poll_timer;
uint32_t avg_packet_counter;
uint32_t packet_timer[TIME_SLOT_MAX];
uint32_t packet_weight[TIME_SLOT_MAX];
uint8_t avg_packet_rate;
uint8_t new_byte;
bool send_attiandrng;
bool send_latitude;
bool send_chunk;
uint32_t params_timer;
uint32_t ap_status_timer;
uint32_t batt_timer;
uint32_t batt_timer2;
uint32_t gps_status_timer;
uint32_t home_timer;
uint32_t velandyaw_timer;
uint32_t gps_latlng_timer;
} _passthrough;
struct
{
const uint32_t packet_min_period[TIME_SLOT_MAX] = {
0, //0x5000 text, no rate limiter
38, //0x5006 attitude 20Hz
280, //0x800 GPS 3Hz
280, //0x800 GPS 3Hz
250, //0x5005 vel&yaw 4Hz
500, //0x5001 AP status 2Hz
500, //0x5002 GPS status 2Hz
500, //0x5004 home 2Hz
500, //0x5008 batt 2 2Hz
500, //0x5003 batt 1 2Hz
1000 //0x5007 parameters 1Hz
};
} _sport_config;
struct
{
bool sport_status;
@ -195,6 +208,9 @@ private: @@ -195,6 +208,9 @@ private:
uint8_t char_index; // index of which character to get in the message
} _msg_chunk;
// passthrough WFQ scheduler
void update_avg_packet_rate();
void passthrough_wfq_adaptive_scheduler(uint8_t prev_byte);
// main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
void send_SPort_Passthrough(void);
// main transmission function when protocol is FrSky SPort
@ -203,13 +219,12 @@ private: @@ -203,13 +219,12 @@ private:
void send_D(void);
// tick - main call to send updates to transmitter (called by scheduler at 1kHz)
void loop(void);
// methods related to the nuts-and-bolts of sending data
void calc_crc(uint8_t byte);
void send_crc(void);
void send_byte(uint8_t value);
void send_uint32(uint16_t id, uint32_t data);
void send_uint16(uint16_t id, uint16_t data);
void send_uint32(uint8_t frame, uint16_t id, uint32_t data);
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
bool get_next_msg_chunk(void);

Write Preview
Loading…
Cancel
Save