AP_Frsky_Telem: added support for passthrough telemetry over crossfire

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;
     }

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);

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.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;
         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;
     }
-    frame = external_data.frame;
+
-    appid = external_data.appid;
+    uint8_t idx = 0;
-    data = external_data.data;
+
+    // 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;
-    return true;
+                }
+            }
+        }
+    } 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();
+}
+};

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,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;
+        sport_packet_t packet;
-        uint16_t appid;
-        uint32_t data;
         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();
 };

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
 
-/*
+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 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()) {
+    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) {
         return false;
     }

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;
 
 };