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

AP_Frsky_Telem: refactor common code into AP_RCTelemetry 

refactor naming
when using external data AP_Frsky_Telem::init() has to call AP_RCTelemetry::init() and exit.
No need to initialize serial ports
c415-sdk
Andy Piper 5 years ago committed by Andrew Tridgell
parent
e3a88f686d
commit
44e5171f2b
2 changed files with 91 additions and 295 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. 311
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
  2. 75
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

311
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
Unescape View File

@ -32,14 +32,13 @@ @@ -32,14 +32,13 @@
#include <AP_Common/Location.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <stdio.h>
#include <math.h>
extern const AP_HAL::HAL& hal;
AP_Frsky_Telem *AP_Frsky_Telem::singleton;
AP_Frsky_Telem::AP_Frsky_Telem(bool _external_data) :
AP_Frsky_Telem::AP_Frsky_Telem(bool _external_data) : AP_RCTelemetry(TIME_SLOT_MAX),
use_external_data(_external_data)
{
singleton = this;
@ -53,35 +52,22 @@ AP_Frsky_Telem::~AP_Frsky_Telem(void) @@ -53,35 +52,22 @@ AP_Frsky_Telem::~AP_Frsky_Telem(void)
/*
setup ready for passthrough telem
*/
void AP_Frsky_Telem::setup_passthrough(void)
void AP_Frsky_Telem::setup_wfq_scheduler(void)
{
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
// make frsky_telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK)
// add firmware and frame info to message queue
const char* _frame_string = gcs().frame_string();
if (_frame_string == nullptr) {
queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string);
} else {
char firmware_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string);
queue_message(MAV_SEVERITY_INFO, firmware_buf);
}
#endif
// initialize packet weights for the WFQ scheduler
// priority[i] = 1/_passthrough.packet_weight[i]
// priority[i] = 1/_scheduler.packet_weight[i]
// rate[i] = LinkRate * ( priority[i] / (sum(priority[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
set_scheduler_entry(TEXT, 35, 28); // 0x5000 status text (dynamic)
set_scheduler_entry(ATTITUDE, 50, 38); // 0x5006 Attitude and range (dynamic)
set_scheduler_entry(GPS_LAT, 550, 280); // 0x800 GPS lat
set_scheduler_entry(GPS_LON, 550, 280); // 0x800 GPS lon
set_scheduler_entry(VEL_YAW, 400, 250); // 0x5005 Vel and Yaw
set_scheduler_entry(AP_STATUS, 700, 500); // 0x5001 AP status
set_scheduler_entry(GPS_STATUS, 700, 500); // 0x5002 GPS status
set_scheduler_entry(HOME, 400, 500); // 0x5004 Home
set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status
set_scheduler_entry(BATT_1, 1300, 500); // 0x5008 Battery 1 status
set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
}
/*
@ -89,6 +75,10 @@ void AP_Frsky_Telem::setup_passthrough(void) @@ -89,6 +75,10 @@ void AP_Frsky_Telem::setup_passthrough(void)
*/
bool AP_Frsky_Telem::init()
{
if (use_external_data) {
return AP_RCTelemetry::init();
}
const AP_SerialManager &serial_manager = AP::serialmanager();
// check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports)
@ -98,7 +88,7 @@ bool AP_Frsky_Telem::init() @@ -98,7 +88,7 @@ bool AP_Frsky_Telem::init()
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers)
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) {
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough; // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
setup_passthrough();
AP_RCTelemetry::init();
}
if (_port != nullptr) {
@ -115,128 +105,86 @@ bool AP_Frsky_Telem::init() @@ -115,128 +105,86 @@ bool AP_Frsky_Telem::init()
return false;
}
void AP_Frsky_Telem::update_avg_packet_rate()
void AP_Frsky_Telem::adjust_packet_weight(bool queue_empty)
{
uint32_t poll_now = AP_HAL::millis();
if (!queue_empty) {
_scheduler.packet_weight[TEXT] = 45; // messages
_scheduler.packet_weight[ATTITUDE] = 80; // attitude
} else {
_scheduler.packet_weight[TEXT] = 5000; // messages
_scheduler.packet_weight[ATTITUDE] = 45; // attitude
}
}
_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
bool AP_Frsky_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
{
bool packet_ready = false;
switch (idx) {
case TEXT:
packet_ready = !queue_empty;
break;
case AP_STATUS:
packet_ready = gcs().vehicle_initialised();
break;
case BATT_2:
packet_ready = AP::battery().num_instances() > 1;
break;
default:
packet_ready = true;
break;
}
return packet_ready;
}
/*
* WFQ scheduler
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
void AP_Frsky_Telem::passthrough_wfq_adaptive_scheduler(void)
void AP_Frsky_Telem::process_packet(uint8_t idx)
{
update_avg_packet_rate();
uint32_t now = AP_HAL::millis();
uint8_t max_delay_idx = 0;
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
bool queue_empty;
{
WITH_SEMAPHORE(_statustext.sem);
queue_empty = !_statustext.available && _statustext.queue.empty();
}
if (!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 = !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 0: // 0x5000 status text
switch (idx) {
case TEXT: // 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
case ATTITUDE: // 0x5006 Attitude and range
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng());
break;
case 2: // 0x800 GPS lat
case GPS_LAT: // 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;
// _scheduler.avg_polling_period*number_of_downlink_sensors time separation
_scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
break;
case 3: // 0x800 GPS lon
case GPS_LON: // 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
case VEL_YAW: // 0x5005 Vel and Yaw
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw());
break;
case 5: // 0x5001 AP status
case AP_STATUS: // 0x5001 AP status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status());
break;
case 6: // 0x5002 GPS Status
case GPS_STATUS: // 0x5002 GPS Status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status());
break;
case 7: // 0x5004 Home
case HOME: // 0x5004 Home
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home());
break;
case 8: // 0x5008 Battery 2 status
case BATT_2: // 0x5008 Battery 2 status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1));
break;
case 9: // 0x5003 Battery 1 status
case BATT_1: // 0x5003 Battery 1 status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0));
break;
case 10: // 0x5007 parameters
case PARAM: // 0x5007 parameters
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break;
}
@ -268,7 +216,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) @@ -268,7 +216,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
}
if (prev_byte == START_STOP_SPORT) {
if (_passthrough.new_byte == SENSOR_ID_28) { // byte 0x7E is the header of each poll request
passthrough_wfq_adaptive_scheduler();
run_wfq_scheduler();
}
}
}
@ -603,11 +551,11 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void) @@ -603,11 +551,11 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void)
// on slow links reduce the number of duplicate chunks
uint8_t extra_chunks = 2;
if (_passthrough.avg_packet_rate < 20) {
if (_scheduler.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) {
} else if (_scheduler.avg_packet_rate < 30) {
// with 1 or 2 extra frsky sensors on the bus
// send messages twice
extra_chunks = 1;
@ -623,121 +571,6 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void) @@ -623,121 +571,6 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void)
return true;
}
/*
* add message to message cue for transmission through FrSky link
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
void AP_Frsky_Telem::queue_message(MAV_SEVERITY severity, const char *text)
{
mavlink_statustext_t statustext{};
statustext.severity = severity;
strncpy(statustext.text, text, sizeof(statustext.text));
// The force push will ensure comm links do not block other comm links forever if they fail.
// If we push to a full buffer then we overwrite the oldest entry, effectively removing the
// block but not until the buffer fills up.
WITH_SEMAPHORE(_statustext.sem);
_statustext.queue.push_force(statustext);
}
/*
* add sensor_status_flags information to message cue, normally passed as sys_status mavlink messages to the GCS, for transmission through FrSky link
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
void AP_Frsky_Telem::check_sensor_status_flags(void)
{
uint32_t now = AP_HAL::millis();
const uint32_t _sensor_status_flags = sensor_status_flags();
if ((now - check_sensor_status_timer) >= 5000) { // prevent repeating any system_status messages unless 5 seconds have passed
// only one error is reported at a time (in order of preference). Same setup and displayed messages as Mission Planner.
if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_GPS) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad GPS Health");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_GYRO) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Gyro Health");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_ACCEL) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Accel Health");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_MAG) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Compass Health");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Baro Health");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_LASER_POSITION) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad LiDAR Health");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad OptFlow Health");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_TERRAIN) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad or No Terrain Data");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_GEOFENCE) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Geofence Breach");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_AHRS) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad AHRS");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_RC_RECEIVER) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "No RC Receiver");
check_sensor_status_timer = now;
} else if ((_sensor_status_flags & MAV_SYS_STATUS_LOGGING) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Logging");
check_sensor_status_timer = now;
}
}
}
/*
* add innovation variance information to message cue, normally passed as ekf_status_report mavlink messages to the GCS, for transmission through FrSky link
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
void AP_Frsky_Telem::check_ekf_status(void)
{
// get variances
bool get_variance;
float velVar, posVar, hgtVar, tasVar;
Vector3f magVar;
Vector2f offset;
{
AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
get_variance = _ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset);
}
if (get_variance) {
uint32_t now = AP_HAL::millis();
if ((now - check_ekf_status_timer) >= 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed
// multiple errors can be reported at a time. Same setup as Mission Planner.
if (velVar >= 1) {
queue_message(MAV_SEVERITY_CRITICAL, "Error velocity variance");
check_ekf_status_timer = now;
}
if (posVar >= 1) {
queue_message(MAV_SEVERITY_CRITICAL, "Error pos horiz variance");
check_ekf_status_timer = now;
}
if (hgtVar >= 1) {
queue_message(MAV_SEVERITY_CRITICAL, "Error pos vert variance");
check_ekf_status_timer = now;
}
if (magVar.length() >= 1) {
queue_message(MAV_SEVERITY_CRITICAL, "Error compass variance");
check_ekf_status_timer = now;
}
if (tasVar >= 1) {
queue_message(MAV_SEVERITY_CRITICAL, "Error terrain alt variance");
check_ekf_status_timer = now;
}
}
}
}
/*
* prepare parameter data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
@ -1130,22 +963,12 @@ void AP_Frsky_Telem::calc_gps_position(void) @@ -1130,22 +963,12 @@ void AP_Frsky_Telem::calc_gps_position(void)
_SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS
}
uint32_t AP_Frsky_Telem::sensor_status_flags() const
{
uint32_t present;
uint32_t enabled;
uint32_t health;
gcs().get_sensor_status_flags(present, enabled, health);
return ~health & enabled & present;
}
/*
fetch Sport data for an external transport, such as FPort
*/
bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
{
passthrough_wfq_adaptive_scheduler();
run_wfq_scheduler();
if (!external_data.pending) {
return false;
}
@ -1167,7 +990,7 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d @@ -1167,7 +990,7 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d
new AP_Frsky_Telem(true);
// initialize the passthrough scheduler
if (singleton) {
singleton->setup_passthrough();
singleton->init();
}
}
if (!singleton) {

75
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
Unescape View File

@ -18,6 +18,7 @@ @@ -18,6 +18,7 @@
#include <AP_Notify/AP_Notify.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_RCTelemetry/AP_RCTelemetry.h>
#define FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent)
@ -111,7 +112,7 @@ for FrSky SPort Passthrough @@ -111,7 +112,7 @@ for FrSky SPort Passthrough
// for fair scheduler
#define TIME_SLOT_MAX 11
class AP_Frsky_Telem {
class AP_Frsky_Telem : public AP_RCTelemetry {
public:
AP_Frsky_Telem(bool external_data=false);
@ -122,16 +123,7 @@ public: @@ -122,16 +123,7 @@ public:
AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete;
// init - perform required initialisation
bool init();
// add statustext message to FrSky lib message queue
void queue_message(MAV_SEVERITY severity, const char *text);
// 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;
virtual bool init() override;
static AP_Frsky_Telem *get_singleton(void) {
return singleton;
@ -145,17 +137,22 @@ private: @@ -145,17 +137,22 @@ private:
AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
uint16_t _crc;
uint32_t check_sensor_status_timer;
uint32_t check_ekf_status_timer;
uint8_t _paramID;
struct {
HAL_Semaphore sem;
ObjectBuffer<mavlink_statustext_t> queue{FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY};
mavlink_statustext_t next;
bool available;
} _statustext;
enum PassthroughPacketType : uint8_t {
TEXT = 0, // 0x5000 status text (dynamic)
ATTITUDE = 1, // 0x5006 Attitude and range (dynamic)
GPS_LAT = 2, // 0x800 GPS lat
GPS_LON = 3, // 0x800 GPS lon
VEL_YAW = 4, // 0x5005 Vel and Yaw
AP_STATUS = 5, // 0x5001 AP status
GPS_STATUS = 6, // 0x5002 GPS status
HOME = 7, // 0x5004 Home
BATT_2 = 8, // 0x5008 Battery 2 status
BATT_1 = 9, // 0x5008 Battery 1 status
PARAM = 10 // 0x5007 parameters
};
struct
{
int32_t vario_vspd;
@ -177,31 +174,8 @@ private: @@ -177,31 +174,8 @@ private:
{
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
{
const uint32_t packet_min_period[TIME_SLOT_MAX] = {
28, //0x5000 text, 25Hz
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
{
@ -229,8 +203,12 @@ private: @@ -229,8 +203,12 @@ private:
float get_vspeed_ms(void);
// passthrough WFQ scheduler
void update_avg_packet_rate();
void passthrough_wfq_adaptive_scheduler();
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;
// setup ready for passthrough operation
void setup_wfq_scheduler(void) override;
// main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
void send_SPort_Passthrough(void);
// main transmission function when protocol is FrSky SPort
@ -247,9 +225,7 @@ private: @@ -247,9 +225,7 @@ private:
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);
void check_sensor_status_flags(void);
void check_ekf_status(void);
bool get_next_msg_chunk(void) override;
uint32_t calc_param(void);
uint32_t calc_gps_latlng(bool *send_latitude);
uint32_t calc_gps_status(void);
@ -265,9 +241,6 @@ private: @@ -265,9 +241,6 @@ private:
float format_gps(float dec);
void calc_gps_position(void);
// setup ready for passthrough operation
void setup_passthrough(void);
// get next telemetry data for external consumers of SPort data (internal function)
bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);

Write Preview
Loading…
Cancel
Save