diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index 4ac9136ef7..7a46a07f6d 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -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; } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index 9a24ef5e72..79797252f8 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -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); diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 8aa21f2f93..724a9c615e 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -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 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(); +} +}; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 2994e2b7c6..99188f0a30 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -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,32 +48,16 @@ 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); } -protected: - - void send() override; - -private: - - AP_Frsky_Parameters *&_frsky_parameters; - - enum PassthroughParam : uint8_t { - FRAME_TYPE = 1, - BATT_FS_VOLTAGE = 2, - BATT_FS_CAPACITY = 3, - BATT_CAPACITY_1 = 4, - BATT_CAPACITY_2 = 5 - }; - enum PassthroughPacketType : uint8_t { TEXT = 0, // 0x5000 status text (dynamic) ATTITUDE = 1, // 0x5006 Attitude and range (dynamic) @@ -83,6 +73,23 @@ private: #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; + +private: + + AP_Frsky_Parameters *&_frsky_parameters; + + enum PassthroughParam : uint8_t { + FRAME_TYPE = 1, + BATT_FS_VOLTAGE = 2, + BATT_FS_CAPACITY = 3, + BATT_CAPACITY_1 = 4, + BATT_CAPACITY_2 = 5 }; // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format @@ -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(); }; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index bbb27ab45a..b6248f4b13 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -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; } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index ecdf9f6bb7..2b5d4d490c 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -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; };