mirror of https://github.com/ArduPilot/ardupilot
Ap_Frsky_Telem: replaced the passthrough scheduler with a WFQ one.
This replaces the default scheduler with a WFQ one
This commit is contained in:
yaapu
Andrew Tridgell
parent
e18883fddf
commit
dddfaa256f
|
|
@ -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 @@
|
|||
#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()
|
|||
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()
|
|||
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)
|
|||
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)
|
|||
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)
|
|||
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)
|
|||
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)
|
|||
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)
|
|||
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;
|
||||
|
|
@ -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)
|
|||
}
|
||||
}
|
||||
|
||||
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)
|
|||
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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
|||
#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:
|
||||
|
|
@ -141,7 +141,6 @@ private:
|
|||
|
||||
ObjectArray<mavlink_statustext_t> _statustext_queue;
|
||||
|
||||
|
||||
struct
|
||||
{
|
||||
char lat_ns, lon_ew;
|
||||
|
|
@ -157,21 +156,35 @@ private:
|
|||
uint16_t speed_in_centimeter;
|
||||
} _gps;
|
||||
|
||||
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;
|
||||
} _passthrough;
|
||||
|
||||
|
||||
struct
|
||||
{
|
||||
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;
|
||||
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
|
||||
{
|
||||
|
|
@ -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:
|
|||
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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue