Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: refactor common code into AP_RCTelemetry

Browse Source 
refactor naming
when using external data AP_Frsky_Telem::init() has to call AP_RCTelemetry::init() and exit.
No need to initialize serial ports
This commit is contained in:
Andy Piper 2020-04-04 17:25:50 +01:00 committed by Andrew Tridgell
parent e3a88f686d
commit 44e5171f2b
2 changed files with 92 additions and 296 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

313
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -32,14 +32,13 @@
#include <AP_Common/Location.h> #include <AP_Common/Location.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
#include <stdio.h>
#include <math.h> #include <math.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_Frsky_Telem *AP_Frsky_Telem::singleton; 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) use_external_data(_external_data)
{ {
singleton = this; singleton = this;
@ -53,35 +52,22 @@ AP_Frsky_Telem::~AP_Frsky_Telem(void)
/* /*
setup ready for passthrough telem 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 // 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])) ) // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
_passthrough.packet_weight[0] = 35; // 0x5000 status text (dynamic) set_scheduler_entry(TEXT, 35, 28); // 0x5000 status text (dynamic)
_passthrough.packet_weight[1] = 50; // 0x5006 Attitude and range (dynamic) set_scheduler_entry(ATTITUDE, 50, 38); // 0x5006 Attitude and range (dynamic)
_passthrough.packet_weight[2] = 550; // 0x800 GPS lat (600 with 1 sensor) set_scheduler_entry(GPS_LAT, 550, 280); // 0x800 GPS lat
_passthrough.packet_weight[3] = 550; // 0x800 GPS lon (600 with 1 sensor) set_scheduler_entry(GPS_LON, 550, 280); // 0x800 GPS lon
_passthrough.packet_weight[4] = 400; // 0x5005 Vel and Yaw set_scheduler_entry(VEL_YAW, 400, 250); // 0x5005 Vel and Yaw
_passthrough.packet_weight[5] = 700; // 0x5001 AP status set_scheduler_entry(AP_STATUS, 700, 500); // 0x5001 AP status
_passthrough.packet_weight[6] = 700; // 0x5002 GPS Status set_scheduler_entry(GPS_STATUS, 700, 500); // 0x5002 GPS status
_passthrough.packet_weight[7] = 400; // 0x5004 Home set_scheduler_entry(HOME, 400, 500); // 0x5004 Home
_passthrough.packet_weight[8] = 1300; // 0x5008 Battery 2 status set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status
_passthrough.packet_weight[9] = 1300; // 0x5003 Battery 1 status set_scheduler_entry(BATT_1, 1300, 500); // 0x5008 Battery 1 status
_passthrough.packet_weight[10] = 1700; // 0x5007 parameters set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
} }
/* /*
@ -89,6 +75,10 @@ void AP_Frsky_Telem::setup_passthrough(void)
*/ */
bool AP_Frsky_Telem::init() bool AP_Frsky_Telem::init()
{ {
if (use_external_data) {
return AP_RCTelemetry::init();
}
const AP_SerialManager &serial_manager = AP::serialmanager(); 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) // 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()
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers) _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))) { } 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) _protocol = AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough; // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
setup_passthrough(); AP_RCTelemetry::init();
} }
if (_port != nullptr) { if (_port != nullptr) {
@ -115,128 +105,86 @@ bool AP_Frsky_Telem::init()
return false; 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
_passthrough.avg_packet_counter++; _scheduler.packet_weight[ATTITUDE] = 80; // attitude
} else {
if (poll_now - _passthrough.last_poll_timer > 1000) { //average in last 1000ms _scheduler.packet_weight[TEXT] = 5000; // messages
// initialize _scheduler.packet_weight[ATTITUDE] = 45; // attitude
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 * WFQ scheduler
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * 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 // send packet
switch (max_delay_idx) { switch (idx) {
case 0: // 0x5000 status text case TEXT: // 0x5000 status text
if (get_next_msg_chunk()) { if (get_next_msg_chunk()) {
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk);
} }
break; break;
case 1: // 0x5006 Attitude and range case ATTITUDE: // 0x5006 Attitude and range
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng()); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng());
break; break;
case 2: // 0x800 GPS lat case GPS_LAT: // 0x800 GPS lat
// sample both lat and lon at the same time // 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 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); _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 // force the scheduler to select GPS lon as packet that's been waiting the most
// this guarantees that gps coords are sent at max // this guarantees that gps coords are sent at max
// _passthrough.avg_polling_period*number_of_downlink_sensors time separation // _scheduler.avg_polling_period*number_of_downlink_sensors time separation
_passthrough.packet_timer[3] = _passthrough.packet_timer[2] - 10000; _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
break; 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 send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude
break; 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()); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw());
break; break;
case 5: // 0x5001 AP status case AP_STATUS: // 0x5001 AP status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status()); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status());
break; break;
case 6: // 0x5002 GPS Status case GPS_STATUS: // 0x5002 GPS Status
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status()); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status());
break; break;
case 7: // 0x5004 Home case HOME: // 0x5004 Home
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home()); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home());
break; 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)); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1));
break; 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)); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0));
break; break;
case 10: // 0x5007 parameters case PARAM: // 0x5007 parameters
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break; break;
} }
@ -268,7 +216,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
} }
if (prev_byte == START_STOP_SPORT) { if (prev_byte == START_STOP_SPORT) {
if (_passthrough.new_byte == SENSOR_ID_28) { // byte 0x7E is the header of each poll request 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)
// on slow links reduce the number of duplicate chunks // on slow links reduce the number of duplicate chunks
uint8_t extra_chunks = 2; 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 // with 3 or more extra frsky sensors on the bus
// send messages only once // send messages only once
extra_chunks = 0; 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 // with 1 or 2 extra frsky sensors on the bus
// send messages twice // send messages twice
extra_chunks = 1; extra_chunks = 1;
@ -623,121 +571,6 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void)
return true; 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 * prepare parameter data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
@ -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 _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 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) 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) { if (!external_data.pending) {
return false; return false;
} }
@ -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); new AP_Frsky_Telem(true);
// initialize the passthrough scheduler // initialize the passthrough scheduler
if (singleton) { if (singleton) {
singleton->setup_passthrough(); singleton->init();
} }
} }
if (!singleton) { if (!singleton) {

75
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -18,6 +18,7 @@
#include <AP_Notify/AP_Notify.h> #include <AP_Notify/AP_Notify.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <AP_HAL/utility/RingBuffer.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) #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
// for fair scheduler // for fair scheduler
#define TIME_SLOT_MAX 11 #define TIME_SLOT_MAX 11
class AP_Frsky_Telem { class AP_Frsky_Telem : public AP_RCTelemetry {
public: public:
AP_Frsky_Telem(bool external_data=false); AP_Frsky_Telem(bool external_data=false);
@ -122,16 +123,7 @@ public:
AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete; AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete;
// init - perform required initialisation // init - perform required initialisation
bool init(); virtual bool init() override;
// 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;
static AP_Frsky_Telem *get_singleton(void) { static AP_Frsky_Telem *get_singleton(void) {
return singleton; return singleton;
@ -145,17 +137,22 @@ private:
AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
uint16_t _crc; uint16_t _crc;
uint32_t check_sensor_status_timer;
uint32_t check_ekf_status_timer;
uint8_t _paramID; 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 struct
{ {
int32_t vario_vspd; int32_t vario_vspd;
@ -177,31 +174,8 @@ private:
{ {
bool send_latitude; // sizeof(bool) = 4 ? bool send_latitude; // sizeof(bool) = 4 ?
uint32_t gps_lng_sample; 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; uint8_t new_byte;
} _passthrough; } _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 struct
{ {
@ -229,8 +203,12 @@ private:
float get_vspeed_ms(void); float get_vspeed_ms(void);
// passthrough WFQ scheduler // passthrough WFQ scheduler
void update_avg_packet_rate(); bool is_packet_ready(uint8_t idx, bool queue_empty) override;
void passthrough_wfq_adaptive_scheduler(); 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) // main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
void send_SPort_Passthrough(void); void send_SPort_Passthrough(void);
// main transmission function when protocol is FrSky SPort // main transmission function when protocol is FrSky SPort
@ -247,9 +225,7 @@ private:
void send_uint32(uint8_t frame, uint16_t id, uint32_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 // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
bool get_next_msg_chunk(void); bool get_next_msg_chunk(void) override;
void check_sensor_status_flags(void);
void check_ekf_status(void);
uint32_t calc_param(void); uint32_t calc_param(void);
uint32_t calc_gps_latlng(bool *send_latitude); uint32_t calc_gps_latlng(bool *send_latitude);
uint32_t calc_gps_status(void); uint32_t calc_gps_status(void);
@ -265,9 +241,6 @@ private:
float format_gps(float dec); float format_gps(float dec);
void calc_gps_position(void); 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) // 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); bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);