mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: added support for passthrough telemetry over crossfire
This commit is contained in:
parent
5ef20aec27
commit
6b26a64f77
|
@ -19,6 +19,16 @@ public:
|
||||||
virtual bool init();
|
virtual bool init();
|
||||||
virtual void send() = 0;
|
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
|
// SPort is at 57600, D overrides this
|
||||||
virtual uint32_t initial_baud() const
|
virtual uint32_t initial_baud() const
|
||||||
{
|
{
|
||||||
|
@ -26,7 +36,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// get next telemetry data for external consumers of SPort data
|
// 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;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -11,16 +11,6 @@ public:
|
||||||
|
|
||||||
void send() override;
|
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:
|
protected:
|
||||||
|
|
||||||
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
|
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
|
||||||
|
|
|
@ -60,6 +60,8 @@ for FrSky SPort Passthrough
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
AP_Frsky_SPort_Passthrough *AP_Frsky_SPort_Passthrough::singleton;
|
||||||
|
|
||||||
bool AP_Frsky_SPort_Passthrough::init()
|
bool AP_Frsky_SPort_Passthrough::init()
|
||||||
{
|
{
|
||||||
if (!AP_RCTelemetry::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)
|
void AP_Frsky_SPort_Passthrough::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data)
|
||||||
{
|
{
|
||||||
if (_use_external_data) {
|
if (_use_external_data) {
|
||||||
external_data.frame = frame;
|
external_data.packet.frame = frame;
|
||||||
external_data.appid = appid;
|
external_data.packet.appid = appid;
|
||||||
external_data.data = data;
|
external_data.packet.data = data;
|
||||||
external_data.pending = true;
|
external_data.pending = true;
|
||||||
return;
|
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 (!_use_external_data) {
|
||||||
if (!external_data.pending) {
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
frame = external_data.frame;
|
|
||||||
appid = external_data.appid;
|
uint8_t idx = 0;
|
||||||
data = external_data.data;
|
|
||||||
external_data.pending = false;
|
// max_size >= WFQ_LAST_ITEM
|
||||||
return true;
|
// 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);
|
return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg);
|
||||||
}
|
}
|
||||||
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
|
||||||
|
namespace AP
|
||||||
|
{
|
||||||
|
AP_Frsky_SPort_Passthrough *frsky_passthrough_telem()
|
||||||
|
{
|
||||||
|
return AP_Frsky_SPort_Passthrough::get_singleton();
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
|
@ -28,7 +28,13 @@ public:
|
||||||
AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX),
|
AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX),
|
||||||
_use_external_data(use_external_data),
|
_use_external_data(use_external_data),
|
||||||
_frsky_parameters(frsky_parameters)
|
_frsky_parameters(frsky_parameters)
|
||||||
{ }
|
{
|
||||||
|
singleton = this;
|
||||||
|
}
|
||||||
|
|
||||||
|
static AP_Frsky_SPort_Passthrough *get_singleton(void) {
|
||||||
|
return singleton;
|
||||||
|
}
|
||||||
|
|
||||||
bool init() override;
|
bool init() override;
|
||||||
bool init_serial_port() override;
|
bool init_serial_port() override;
|
||||||
|
@ -42,16 +48,34 @@ public:
|
||||||
|
|
||||||
bool get_next_msg_chunk(void) override;
|
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
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override;
|
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
|
void queue_text_message(MAV_SEVERITY severity, const char *text) override
|
||||||
{
|
{
|
||||||
AP_RCTelemetry::queue_message(severity, text);
|
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:
|
protected:
|
||||||
|
|
||||||
void send() override;
|
void send() override;
|
||||||
|
@ -68,23 +92,6 @@ private:
|
||||||
BATT_CAPACITY_2 = 5
|
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
|
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
|
||||||
uint32_t calc_param(void);
|
uint32_t calc_param(void);
|
||||||
uint32_t calc_batt(uint8_t instance);
|
uint32_t calc_batt(uint8_t instance);
|
||||||
|
@ -96,10 +103,9 @@ private:
|
||||||
// use_external_data is set when this library will
|
// use_external_data is set when this library will
|
||||||
// be providing data to another transport, such as FPort
|
// be providing data to another transport, such as FPort
|
||||||
bool _use_external_data;
|
bool _use_external_data;
|
||||||
|
|
||||||
struct {
|
struct {
|
||||||
uint8_t frame;
|
sport_packet_t packet;
|
||||||
uint16_t appid;
|
|
||||||
uint32_t data;
|
|
||||||
bool pending;
|
bool pending;
|
||||||
} external_data;
|
} external_data;
|
||||||
|
|
||||||
|
@ -146,4 +152,10 @@ private:
|
||||||
|
|
||||||
uint32_t calc_gps_status(void);
|
uint32_t calc_gps_status(void);
|
||||||
uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power);
|
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();
|
||||||
};
|
};
|
||||||
|
|
|
@ -81,12 +81,15 @@ bool AP_Frsky_Telem::init(bool use_external_data)
|
||||||
return true;
|
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) {
|
if (_backend == nullptr) {
|
||||||
return false;
|
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
|
#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
|
#endif
|
||||||
|
|
||||||
/*
|
void AP_Frsky_Telem::try_create_singleton_for_external_data()
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
|
// try to allocate an AP_Frsky_Telem object only if we are disarmed
|
||||||
if (!singleton && !hal.util->get_soft_armed()) {
|
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();
|
new AP_Frsky_Telem();
|
||||||
// initialize the passthrough scheduler
|
// initialize the passthrough scheduler
|
||||||
if (singleton) {
|
if (singleton) {
|
||||||
singleton->init(true);
|
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) {
|
if (singleton == nullptr) {
|
||||||
return false;
|
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
|
#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)
|
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()) {
|
try_create_singleton_for_external_data();
|
||||||
// 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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if (singleton == nullptr) {
|
if (singleton == nullptr) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -15,6 +15,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_Frsky_Backend.h"
|
#include "AP_Frsky_Backend.h"
|
||||||
|
#include "AP_Frsky_SPort.h"
|
||||||
|
|
||||||
class AP_Frsky_Parameters;
|
class AP_Frsky_Parameters;
|
||||||
|
|
||||||
|
@ -37,7 +38,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// get next telemetry data for external consumers of SPort data
|
// 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
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
// set telemetry data from external producer of SPort data
|
// 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);
|
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;
|
AP_Frsky_Parameters* _frsky_parameters;
|
||||||
|
|
||||||
// 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(AP_Frsky_Backend::sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size);
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
// set telemetry data from external producer of SPort data (internal function)
|
// 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);
|
bool _set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data);
|
||||||
#endif
|
#endif
|
||||||
|
static void try_create_singleton_for_external_data(void);
|
||||||
static AP_Frsky_Telem *singleton;
|
static AP_Frsky_Telem *singleton;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue