AP_RCProtocol: added support for passthrough telemetry over crossfire

libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp

@@ -187,12 +187,6 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
     if (_frame_ofs == _frame.length + CSRF_HEADER_LEN) {
         log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CSRF_HEADER_LEN);
 
-        if ((timestamp_us - _last_frame_time_us) <= CRSF_INTER_FRAME_TIME_US_150HZ + CRSF_MAX_FRAME_TIME_US) {
-            _fast_telem = true;
-        } else {
-            _fast_telem = false;
-        }
-
         // we consumed the partial frame, reset
         _frame_ofs = 0;
 
@@ -209,9 +203,9 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
         _last_frame_time_us = timestamp_us;
         // decode here
         if (decode_csrf_packet()) {
-            add_input(MAX_CHANNELS, _channels, false, _current_rssi);
+            add_input(MAX_CHANNELS, _channels, false, _link_status.rssi);
         }
-    }    
+    }
 }
 
 void AP_RCProtocol_CRSF::update(void)
@@ -328,19 +322,6 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
         return false;
 #endif
     }
-    /*
-      check that we haven't been too slow in responding to the new
-      UART data. If we respond too late then we will corrupt the next
-      incoming control frame
-     */
-    uint64_t tend = uart->receive_time_constraint_us(1);
-    uint64_t now = AP_HAL::micros64();
-    uint64_t tdelay = now - tend;
-    if (tdelay > CRSF_MAX_FRAME_TIME_US && check_constraint) {
-        // we've been too slow in responding
-        return false;
-    }
-
     write_frame(&_telemetry_frame);
     // get fresh telem_data in the next call
     telem_available = false;
@@ -361,13 +342,15 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
     }
      // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
     if (rssi_dbm < 50) {
-        _current_rssi = 255;
+        _link_status.rssi = 255;
     } else if (rssi_dbm > 120) {
-        _current_rssi = 0;
+        _link_status.rssi = 0;
     } else {
         // this is an approximation recommended by Remo from TBS
-        _current_rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
+        _link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
     }
+
+    _link_status.rf_mode = static_cast<RFMode>(MIN(link->rf_mode, 3U));
 }
 
 // process a byte provided by a uart
@@ -397,4 +380,3 @@ namespace AP {
         return AP_RCProtocol_CRSF::get_singleton();
     }
 };
-

libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h

@@ -55,6 +55,9 @@ public:
         CRSF_FRAMETYPE_PARAMETER_READ = 0x2C,
         CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D,
         CRSF_FRAMETYPE_COMMAND = 0x32,
+        // Custom Telemetry Frames 0x7F,0x80
+        CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY = 0x7F,   // as suggested by Remo Masina for fw < 4.06
+        CRSF_FRAMETYPE_AP_CUSTOM_TELEM = 0x80,          // reserved for ArduPilot by TBS, requires fw >= 4.06
     };
 
     // Command IDs for CRSF_FRAMETYPE_COMMAND
@@ -116,6 +119,13 @@ public:
         CRSF_COMMAND_RX_BIND = 0x01,
     };
 
+    // SubType IDs for CRSF_FRAMETYPE_CUSTOM_TELEM
+    enum CustomTelemSubTypeID : uint8_t {
+        CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH = 0xF0,
+        CRSF_AP_CUSTOM_TELEM_STATUS_TEXT = 0xF1,
+        CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH = 0xF2,
+    };
+
     enum DeviceAddress {
         CRSF_ADDRESS_BROADCAST = 0x00,
         CRSF_ADDRESS_USB = 0x10,
@@ -161,6 +171,24 @@ public:
         int8_t downlink_dnr; // ( db )
     } PACKED;
 
+    enum class RFMode : uint8_t {
+        CRSF_RF_MODE_4HZ = 0,
+        CRSF_RF_MODE_50HZ,
+        CRSF_RF_MODE_150HZ,
+        CRSF_RF_MODE_UNKNOWN,
+    };
+
+    struct LinkStatus {
+        int16_t rssi = -1;
+        RFMode rf_mode;
+    };
+
+    // this will be used by AP_CRSF_Telem to access link status data
+    // from within AP_RCProtocol_CRSF thread so no need for cross-thread synch
+    const volatile LinkStatus& get_link_status() const {
+        return _link_status;
+    }
+
 private:
     struct Frame _frame;
     struct Frame _telemetry_frame;
@@ -187,8 +215,8 @@ private:
     uint32_t _last_rx_time_us;
     uint32_t _start_frame_time_us;
     bool telem_available;
-    bool _fast_telem; // is 150Hz telemetry active
-    int16_t _current_rssi = -1;
+
+    volatile struct LinkStatus _link_status;
 
     AP_HAL::UARTDriver *_uart;
 

libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp

@@ -148,11 +148,12 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
       status text messages
      */
     if (!telem_data.available) {
-        if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) {
+        uint8_t packet_count;
+        if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) {
             // nothing to send, send a null frame
-            telem_data.frame = 0x00; 
-            telem_data.appid = 0x00; 
-            telem_data.data = 0x00;
+            telem_data.packet.frame = 0x00;
+            telem_data.packet.appid = 0x00;
+            telem_data.packet.data = 0x00;
         }
         telem_data.available = true;
     }
@@ -172,10 +173,10 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
 
     buf[0] = 0x08;
     buf[1] = 0x81;
-    buf[2] = telem_data.frame;
-    buf[3] = telem_data.appid & 0xFF;
-    buf[4] = telem_data.appid >> 8;
-    memcpy(&buf[5], &telem_data.data, 4);
+    buf[2] = telem_data.packet.frame;
+    buf[3] = telem_data.packet.appid & 0xFF;
+    buf[4] = telem_data.packet.appid >> 8;
+    memcpy(&buf[5], &telem_data.packet.data, 4);
     buf[9] = crc_sum8(&buf[0], 9);
 
     // perform byte stuffing per FPort spec

libraries/AP_RCProtocol/AP_RCProtocol_FPort.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include <AP_Frsky_Telem/AP_Frsky_SPort.h>
+
 #include "AP_RCProtocol.h"
 #include "SoftSerial.h"
 
@@ -50,9 +52,7 @@ private:
 
     struct {
         bool available = false;
-        uint32_t data;
-        uint16_t appid;
-        uint8_t frame;
+        AP_Frsky_SPort::sport_packet_t packet;
     } telem_data;
 
     // receiver sends 0x10 when ready to receive telemetry frames (R-XSR)

libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp

@@ -137,10 +137,11 @@ void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
          send it in the next call, this prevents corruption of status text messages
         */
         if (!telem_data.available) {
-            if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) {
-                telem_data.frame = 0x00;
-                telem_data.appid = 0x00;
-                telem_data.data = 0x00;
+            uint8_t packet_count;
+            if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) {
+                telem_data.packet.frame = 0x00;
+                telem_data.packet.appid = 0x00;
+                telem_data.packet.data = 0x00;
             }
             telem_data.available = true;
         }
@@ -169,10 +170,10 @@ void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
     buf[1] = frame.type;
     // do not consume telemetry data for invalid downlink frames, i.e. incoming prim == 0x00
     if (frame.downlink.prim != FPORT2_PRIM_NULL) {
-        buf[2] = telem_data.frame;
-        buf[3] = telem_data.appid & 0xFF;
-        buf[4] = telem_data.appid >> 8;
-        memcpy(&buf[5], &telem_data.data, 4);
+        buf[2] = telem_data.packet.frame;
+        buf[3] = telem_data.packet.appid & 0xFF;
+        buf[4] = telem_data.packet.appid >> 8;
+        memcpy(&buf[5], &telem_data.packet.data, 4);
         // get fresh telem_data in the next call
         telem_data.available = false;
     }

libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include <AP_Frsky_Telem/AP_Frsky_SPort.h>
+
 #include "AP_RCProtocol.h"
 #include "SoftSerial.h"
 
@@ -53,8 +55,6 @@ private:
 
     struct {
         bool available;
-        uint32_t data;
-        uint16_t appid;
-        uint8_t frame;
+        AP_Frsky_SPort::sport_packet_t packet;
     } telem_data;
 };