mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
e3a88f686d
commit
44e5171f2b
@ -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)
|
||||
/*
|
||||
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)
|
||||
*/
|
||||
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()
|
||||
_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()
|
||||
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();
|
||||
|
||||
_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;
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
// 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)
|
||||
}
|
||||
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)
|
||||
// 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)
|
||||
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)
|
||||
_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
|
||||
new AP_Frsky_Telem(true);
|
||||
// initialize the passthrough scheduler
|
||||
if (singleton) {
|
||||
singleton->setup_passthrough();
|
||||
singleton->init();
|
||||
}
|
||||
}
|
||||
if (!singleton) {
|
||||
|
||||
@ -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
|
||||
// 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:
|
||||
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:
|
||||
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:
|
||||
{
|
||||
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:
|
||||
|
||||
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:
|
||||
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:
|
||||
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);
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user