AP_FrSky_Telem: allow for external library to send SPort data

this provides the API needed for AP_RCProtocol_FPort to send
passthrough data over FPORT

libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@@ -37,9 +37,49 @@
 
 extern const AP_HAL::HAL& hal;
 
+AP_Frsky_Telem *AP_Frsky_Telem::singleton;
+
 AP_Frsky_Telem::AP_Frsky_Telem(void) :
   _statustext_queue(FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY)
 {
+    singleton = this;
+}
+
+AP_Frsky_Telem::~AP_Frsky_Telem(void)
+{
+    singleton = nullptr;
+}
+
+/*
+  setup ready for passthrough telem
+ */
+void AP_Frsky_Telem::setup_passthrough(void)
+{
+    // 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);
+    }
+
+    // initialize packet weights for the WFQ scheduler
+    // weight[i] = 1/_passthrough.packet_period[i]
+    // rate[i] = LinkRate * ( weight[i] / (sum(weight[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
 }
 
 /*
@@ -56,31 +96,18 @@ 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)
-        // make frsky_telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK)
+        setup_passthrough();
-        // add firmware and frame info to message queue
+    } else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_RCIN, 0))) {
-        const char* _frame_string = gcs().frame_string();
+        _protocol = AP_SerialManager::SerialProtocol_RCIN; // FrSky SPort over FPort
-        if (_frame_string == nullptr) {
+        setup_passthrough();
-            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);
-        }
 
-        // initialize packet weights for the WFQ scheduler
+    if (_protocol == AP_SerialManager::SerialProtocol_RCIN) {
-        // weight[i] = 1/_passthrough.packet_period[i]
+        use_external_data = true;
-        // rate[i] = LinkRate * ( weight[i] / (sum(weight[1-n])) )
+        setup_passthrough();
-        _passthrough.packet_weight[0] = 35;   // 0x5000 status text (dynamic)
+        // we don't setup the thread, as output is driven by the AP_RCProtocol library
-        _passthrough.packet_weight[1] = 50;   // 0x5006 Attitude and range (dynamic)
+        // calling get_telem_data();
-        _passthrough.packet_weight[2] = 550;   // 0x800 GPS lat (600 with 1 sensor)
+        return true;
-        _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
     }
 
     if (_port != nullptr) {
@@ -499,6 +526,13 @@ void AP_Frsky_Telem::send_byte(uint8_t byte)
  */
 void  AP_Frsky_Telem::send_uint32(uint8_t frame, uint16_t id, uint32_t data)
 {
+    if (use_external_data) {
+        external_data.frame = frame;
+        external_data.appid = id;
+        external_data.data = data;
+        external_data.pending = true;
+        return;
+    }
     send_byte(frame); // frame type
     uint8_t *bytes = (uint8_t*)&id;
     send_byte(bytes[0]); // LSB
@@ -1063,3 +1097,29 @@ uint32_t AP_Frsky_Telem::sensor_status_flags() const
 
     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)
+{
+    if (!use_external_data) {
+        // not initialised for external data
+        return false;
+    }
+    passthrough_wfq_adaptive_scheduler(0);
+    if (!external_data.pending) {
+        return false;
+    }
+    frame = external_data.frame;
+    appid = external_data.appid;
+    data = external_data.data;
+    external_data.pending = false;
+    return true;
+}
+
+namespace AP {
+    AP_Frsky_Telem *frsky_telem() {
+        return AP_Frsky_Telem::get_singleton();
+    }
+};

libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

@@ -114,6 +114,8 @@ class AP_Frsky_Telem {
 public:
     AP_Frsky_Telem();
 
+    ~AP_Frsky_Telem();
+
     /* Do not allow copies */
     AP_Frsky_Telem(const AP_Frsky_Telem &other) = delete;
     AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete;
@@ -130,6 +132,13 @@ public:
     // functioning correctly
     uint32_t sensor_status_flags() const;
 
+    static AP_Frsky_Telem *get_singleton(void) {
+        return singleton;
+    }
+
+    // get next telemetry data for external consumers of SPort data
+    bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
+
 private:
     AP_HAL::UARTDriver *_port;                  // UART used to send data to FrSky receiver
     AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
@@ -244,4 +253,23 @@ private:
     void calc_nav_alt(void);
     float format_gps(float dec);
     void calc_gps_position(void);
+
+    // setup ready for passthrough operation
+    void setup_passthrough(void);
+
+    static AP_Frsky_Telem *singleton;
+
+    // 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;
+        bool pending;
+    } external_data;
+};
+
+namespace AP {
+    AP_Frsky_Telem *frsky_telem();
 };