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

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 2019-07-12 12:06:12 +02:00 committed by Andrew Tridgell
parent e18883fddf
commit dddfaa256f
2 changed files with 219 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

280
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -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;
@ -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)
/*
* 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;
}

53
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

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