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

AP_Frsky_Telem: added support for passthrough telemetry over crossfire

This commit is contained in:
yaapu 2020-12-10 10:28:56 +01:00 committed by Peter Barker
parent 5ef20aec27
commit 6b26a64f77
6 changed files with 123 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
View File

@ -19,6 +19,16 @@ public:
virtual bool init();
virtual void send() = 0;
typedef union {
struct PACKED {
uint8_t sensor;
uint8_t frame;
uint16_t appid;
uint32_t data;
};
uint8_t raw[8];
} sport_packet_t;
// SPort is at 57600, D overrides this
virtual uint32_t initial_baud() const
{
@ -26,7 +36,7 @@ public:
}
// get next telemetry data for external consumers of SPort data
virtual bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
virtual bool get_telem_data(sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size)
{
return false;
}

10
libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
View File

@ -11,16 +11,6 @@ public:
void send() override;
typedef union {
struct PACKED {
uint8_t sensor;
uint8_t frame;
uint16_t appid;
uint32_t data;
};
uint8_t raw[8];
} sport_packet_t;
protected:
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);

68
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -60,6 +60,8 @@ for FrSky SPort Passthrough
extern const AP_HAL::HAL& hal;
AP_Frsky_SPort_Passthrough *AP_Frsky_SPort_Passthrough::singleton;
bool AP_Frsky_SPort_Passthrough::init()
{
if (!AP_RCTelemetry::init()) {
@ -79,9 +81,9 @@ bool AP_Frsky_SPort_Passthrough::init_serial_port()
void AP_Frsky_SPort_Passthrough::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data)
{
if (_use_external_data) {
external_data.frame = frame;
external_data.appid = appid;
external_data.data = data;
external_data.packet.frame = frame;
external_data.packet.appid = appid;
external_data.packet.data = data;
external_data.pending = true;
return;
}
@ -546,19 +548,53 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void)
}
/*
fetch Sport data for an external transport, such as FPort
fetch Sport data for an external transport, such as FPort or crossfire
Note: we need to create a packet array with unique packet types
For very big frames we might have to relax the "unique packet type per frame"
constraint in order to maximize bandwidth usage
*/
bool AP_Frsky_SPort_Passthrough::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
bool AP_Frsky_SPort_Passthrough::get_telem_data(sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size)
{
run_wfq_scheduler();
if (!external_data.pending) {
if (!_use_external_data) {
return false;
}
frame = external_data.frame;
appid = external_data.appid;
data = external_data.data;
external_data.pending = false;
return true;
uint8_t idx = 0;
// max_size >= WFQ_LAST_ITEM
// get a packet per enabled type
if (max_size >= WFQ_LAST_ITEM) {
for (uint8_t i=0; i<WFQ_LAST_ITEM; i++) {
if (process_scheduler_entry(i)) {
if (external_data.pending) {
packet_array[idx].frame = external_data.packet.frame;
packet_array[idx].appid = external_data.packet.appid;
packet_array[idx].data = external_data.packet.data;
idx++;
external_data.pending = false;
}
}
}
} else {
// max_size < WFQ_LAST_ITEM
// call run_wfq_scheduler(false) enough times to create a packet of up to max_size unique elements
uint32_t item_mask = 0;
for (uint8_t i=0; i<max_size; i++) {
// call the scheduler with the shaper "disabled"
const uint8_t item = run_wfq_scheduler(false);
if (!BIT_IS_SET(item_mask, item) && external_data.pending) {
// ok got some data, flip the bitmask bit to prevent adding the same packet type more than once
BIT_SET(item_mask, item);
packet_array[idx].frame = external_data.packet.frame;
packet_array[idx].appid = external_data.packet.appid;
packet_array[idx].data = external_data.packet.data;
idx++;
external_data.pending = false;
}
}
}
packet_count = idx;
return idx > 0;
}
/*
@ -726,3 +762,11 @@ bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &tx
return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg);
}
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
namespace AP
{
AP_Frsky_SPort_Passthrough *frsky_passthrough_telem()
{
return AP_Frsky_SPort_Passthrough::get_singleton();
}
};

58
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

@ -28,7 +28,13 @@ public:
AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX),
_use_external_data(use_external_data),
_frsky_parameters(frsky_parameters)
{ }
{
singleton = this;
}
static AP_Frsky_SPort_Passthrough *get_singleton(void) {
return singleton;
}
bool init() override;
bool init_serial_port() override;
@ -42,16 +48,34 @@ public:
bool get_next_msg_chunk(void) override;
bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override;
bool get_telem_data(sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size) override;
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override;
#endif
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
void queue_text_message(MAV_SEVERITY severity, const char *text) override
{
AP_RCTelemetry::queue_message(severity, text);
}
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
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
MAV = 11, // mavlite
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
WFQ_LAST_ITEM // must be last
};
protected:
void send() override;
@ -68,23 +92,6 @@ private:
BATT_CAPACITY_2 = 5
};
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
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
MAV = 11, // mavlite
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
};
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
uint32_t calc_param(void);
uint32_t calc_batt(uint8_t instance);
@ -96,10 +103,9 @@ private:
// use_external_data is set when this library will
// be providing data to another transport, such as FPort
bool _use_external_data;
struct {
uint8_t frame;
uint16_t appid;
uint32_t data;
sport_packet_t packet;
bool pending;
} external_data;
@ -146,4 +152,10 @@ private:
uint32_t calc_gps_status(void);
uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power);
static AP_Frsky_SPort_Passthrough *singleton;
};
namespace AP {
AP_Frsky_SPort_Passthrough *frsky_passthrough_telem();
};

34
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -81,12 +81,15 @@ bool AP_Frsky_Telem::init(bool use_external_data)
return true;
}
bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
bool AP_Frsky_Telem::_get_telem_data(AP_Frsky_Backend::sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size)
{
if (_backend == nullptr) {
return false;
}
return _backend->get_telem_data(frame, appid, data);
if (packet_array == nullptr) {
return false;
}
return _backend->get_telem_data(packet_array, packet_count, max_size);
}
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
@ -99,24 +102,28 @@ bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t dat
}
#endif
/*
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)
void AP_Frsky_Telem::try_create_singleton_for_external_data()
{
// try to allocate an AP_Frsky_Telem object only if we are disarmed
if (!singleton && !hal.util->get_soft_armed()) {
// if telem data is requested when we are disarmed and don't
// yet have a AP_Frsky_Telem object then try to allocate one
new AP_Frsky_Telem();
// initialize the passthrough scheduler
if (singleton) {
singleton->init(true);
}
}
}
/*
fetch Sport data for an external transport, such as FPort
*/
bool AP_Frsky_Telem::get_telem_data(AP_Frsky_Backend::sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size)
{
try_create_singleton_for_external_data();
if (singleton == nullptr) {
return false;
}
return singleton->_get_telem_data(frame, appid, data);
return singleton->_get_telem_data(packet_array, packet_count, max_size);
}
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
@ -125,14 +132,7 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d
*/
bool AP_Frsky_Telem::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
{
if (!singleton && !hal.util->get_soft_armed()) {
// if telem data is requested when we are disarmed and don't
// yet have a AP_Frsky_Telem object then try to allocate one
new AP_Frsky_Telem();
if (singleton) {
singleton->init(true);
}
}
try_create_singleton_for_external_data();
if (singleton == nullptr) {
return false;
}

7
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -15,6 +15,7 @@
#pragma once
#include "AP_Frsky_Backend.h"
#include "AP_Frsky_SPort.h"
class AP_Frsky_Parameters;
@ -37,7 +38,7 @@ public:
}
// get next telemetry data for external consumers of SPort data
static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
static bool get_telem_data(AP_Frsky_Backend::sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size);
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
// set telemetry data from external producer of SPort data
static bool set_telem_data(const uint8_t frame,const uint16_t appid, const uint32_t data);
@ -56,12 +57,12 @@ private:
AP_Frsky_Parameters* _frsky_parameters;
// 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(AP_Frsky_Backend::sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size);
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
// set telemetry data from external producer of SPort data (internal function)
bool _set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data);
#endif
static void try_create_singleton_for_external_data(void);
static AP_Frsky_Telem *singleton;
};