AP_RCTelemetry: added support for passthrough telemetry over crossfire

libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp

@@ -26,6 +26,7 @@
 #include <AP_Notify/AP_Notify.h>
 #include <AP_Common/AP_FWVersion.h>
 #include <AP_OSD/AP_OSD.h>
+#include <AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h>
 #include <math.h>
 #include <stdio.h>
 
@@ -73,26 +74,218 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
 
     // CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit
     add_scheduler_entry(50, 100);   // heartbeat        10Hz
-    add_scheduler_entry(200, 50);   // parameters       20Hz (generally not active unless requested by the TX)
+    add_scheduler_entry(50, 50);    // parameters       20Hz (generally not active unless requested by the TX)
     add_scheduler_entry(50, 120);   // Attitude and compass 8Hz
     add_scheduler_entry(200, 1000); // VTX parameters    1Hz
     add_scheduler_entry(1300, 500); // battery           2Hz
     add_scheduler_entry(550, 280);  // GPS               3Hz
     add_scheduler_entry(550, 500);  // flight mode       2Hz
+    add_scheduler_entry(5000, 100); // passthrough       max 10Hz
+    add_scheduler_entry(5000, 500); // status text       max 2Hz
+}
+
+void AP_CRSF_Telem::setup_custom_telemetry()
+{
+    if (_custom_telem.init_done) {
+        return;
+    }
+
+    if (!rc().crsf_custom_telemetry()) {
+        return;
+    }
+
+    // we need crossfire firmware version
+    if (_crsf_version.pending) {
+        return;
+    }
+
+    AP_Frsky_SPort_Passthrough* passthrough = AP::frsky_passthrough_telem();
+    if (passthrough == nullptr) {
+        return;
+    }
+
+    // setup the frsky scheduler for crossfire
+    passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LAT);
+    passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LON);
+    passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::TEXT);
+    passthrough->set_scheduler_entry_min_period(AP_Frsky_SPort_Passthrough::ATTITUDE, 350); // 3Hz
+
+    // setup the crossfire scheduler for custom telemetry
+    set_scheduler_entry(BATTERY, 1000, 1000);       // 1Hz
+    set_scheduler_entry(ATTITUDE, 1000, 1000);      // 1Hz
+    set_scheduler_entry(FLIGHT_MODE, 1200, 2000);   // 0.5Hz
+    set_scheduler_entry(HEARTBEAT, 2000, 5000);     // 0.2Hz
+
+    _telem_rf_mode = get_rf_mode();
+    // setup custom telemetry for current rf_mode
+    update_custom_telemetry_rates(_telem_rf_mode);
+
+    gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: custom telem init done, fw %d.%02d", _crsf_version.major, _crsf_version.minor);
+    _custom_telem.init_done = true;
+}
+
+void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode)
+{
+    // ignore rf mode changes if we are processing parameter packets
+    if (_custom_telem.params_mode_active) {
+        return;
+    }
+
+    if (is_high_speed_telemetry(rf_mode)) {
+        // custom telemetry for high data rates
+        set_scheduler_entry(GPS, 550, 500);           // 2.0Hz
+        set_scheduler_entry(PASSTHROUGH, 100, 100);   // 10Hz
+        set_scheduler_entry(STATUS_TEXT, 200, 750);   // 1.5Hz
+    } else {
+        // custom telemetry for low data rates
+        set_scheduler_entry(GPS, 550, 1000);              // 1.0Hz
+        set_scheduler_entry(PASSTHROUGH, 500, 3000);      // 0.3Hz
+        set_scheduler_entry(STATUS_TEXT, 600, 2000);      // 0.5Hz
+    }
+}
+
+void AP_CRSF_Telem::process_rf_mode_changes()
+{
+    const AP_RCProtocol_CRSF::RFMode current_rf_mode = get_rf_mode();
+    if ( _telem_rf_mode != current_rf_mode) {
+        gcs().send_text(MAV_SEVERITY_INFO, "CRSF: rf mode change %d->%d, rate is %dHz", (uint8_t)_telem_rf_mode, (uint8_t)current_rf_mode, _scheduler.avg_packet_rate);
+        update_custom_telemetry_rates(current_rf_mode);
+        _telem_rf_mode = current_rf_mode;
+    }
+}
+
+// return custom frame id based on fw version
+uint8_t AP_CRSF_Telem::get_custom_telem_frame_id() const
+{
+    if (!_crsf_version.pending && _crsf_version.major >= 4 && _crsf_version.minor >= 6) {
+        return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM;
+    }
+    return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY;
+}
+
+AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const
+{
+    AP_RCProtocol_CRSF* crsf = AP::crsf();
+    if (crsf == nullptr) {
+        return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_UNKNOWN;
+    }
+
+    if (!_crsf_version.pending && _crsf_version.use_rf_mode) {
+        return crsf->get_link_status().rf_mode;
+    }
+
+    /*
+     Note:
+     - rf mode 2 on UARTS with DMA runs @160Hz
+     - rf mode 2 on UARTS with no DMA runs @70Hz
+    */
+    if (get_avg_packet_rate() < 40U) {
+        // no DMA rf mode 1
+        return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
+    }
+    if (get_avg_packet_rate() > 120U) {
+        // DMA rf mode 2
+        return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
+    }
+    if (get_max_packet_rate() < 120U) {
+        // no DMA rf mode 2
+        return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
+    }
+    return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
+}
+
+bool AP_CRSF_Telem::is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const
+{
+    return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
+}
+
+void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
+{
+    // no need to queue status text messages when crossfire
+    // custom telemetry is not enabled
+    if (!rc().crsf_custom_telemetry()) {
+        return;
+    }
+    AP_RCTelemetry::queue_message(severity, text);
+}
+
+void AP_CRSF_Telem::enter_scheduler_params_mode()
+{
+    set_scheduler_entry(HEARTBEAT, 50, 100);            // heartbeat        10Hz
+    set_scheduler_entry(ATTITUDE, 50, 120);             // Attitude and compass 8Hz
+    set_scheduler_entry(BATTERY, 1300, 500);            // battery           2Hz
+    set_scheduler_entry(GPS, 550, 280);                 // GPS               3Hz
+    set_scheduler_entry(FLIGHT_MODE, 550, 500);         // flight mode       2Hz
+
+    disable_scheduler_entry(PASSTHROUGH);
+    disable_scheduler_entry(STATUS_TEXT);
+}
+
+void AP_CRSF_Telem::exit_scheduler_params_mode()
+{
+    // setup the crossfire scheduler for custom telemetry
+    set_scheduler_entry(BATTERY, 1000, 1000);       // 1Hz
+    set_scheduler_entry(ATTITUDE, 1000, 1000);      // 1Hz
+    set_scheduler_entry(FLIGHT_MODE, 1200, 2000);   // 0.5Hz
+    set_scheduler_entry(HEARTBEAT, 2000, 5000);     // 0.2Hz
+
+    enable_scheduler_entry(PASSTHROUGH);
+    enable_scheduler_entry(STATUS_TEXT);
+
+    update_custom_telemetry_rates(_telem_rf_mode);
 }
 
 void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
 {
+    uint32_t now_ms = AP_HAL::millis();
+    setup_custom_telemetry();
+
+    /*
+     whenever we detect a pending request we configure the scheduler
+     to allow faster parameters processing.
+     We start a "fast parameter window" that we close after 5sec
+    */
+    bool expired = (now_ms - _custom_telem.params_mode_start_ms) > 5000;
+    if (!_custom_telem.params_mode_active && _pending_request.frame_type > 0) {
+        // fast window start
+        _custom_telem.params_mode_start_ms = now_ms;
+        _custom_telem.params_mode_active = true;
+        enter_scheduler_params_mode();
+    } else if (expired && _custom_telem.params_mode_active) {
+        // fast window stop
+        _custom_telem.params_mode_active = false;
+        exit_scheduler_params_mode();
+    }
 }
 
 // WFQ scheduler
 bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
 {
+    process_rf_mode_changes();
+
     switch (idx) {
     case PARAMETERS:
-        return _request_pending > 0;
+        // to get crossfire firmware version we send an RX device ping until we get a response
+        // but only if there are no other requests pending
+        if (_crsf_version.pending && _pending_request.frame_type == 0) {
+            if (_crsf_version.retry_count++ > CRSF_RX_DEVICE_PING_MAX_RETRY) {
+                _crsf_version.pending = false;
+                _crsf_version.minor = 0;
+                _crsf_version.major = 0;
+                gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: RX device ping failed");
+            } else {
+                _pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
+                _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
+                gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info");
+            }
+        }
+        return _pending_request.frame_type > 0;
     case VTX_PARAMETERS:
         return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending;
+    case PASSTHROUGH:
+        return rc().crsf_custom_telemetry();
+    case STATUS_TEXT:
+        return rc().crsf_custom_telemetry() && !queue_empty;
     default:
         return _enable_telemetry;
     }
@@ -124,6 +317,20 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
         case FLIGHT_MODE: // GPS
             calc_flight_mode();
             break;
+        case PASSTHROUGH:
+            if (is_high_speed_telemetry(_telem_rf_mode)) {
+                // on fast links we have 1:1 ratio between
+                // passthrough frames and crossfire frames
+                get_single_packet_passthrough_telem_data();
+            } else {
+                // on slower links we pack many passthrough
+                // frames in a single crossfire one (up to 9)
+                get_multi_packet_passthrough_telem_data();
+            }
+            break;
+        case STATUS_TEXT:
+            calc_status_text();
+            break;
         default:
             break;
     }
@@ -159,6 +366,10 @@ bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, voi
         process_param_write_frame((ParameterSettingsWriteFrame*)data);
         break;
 
+    case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
+        process_device_info_frame((ParameterDeviceInfoFrame*)data);
+        break;
+
     default:
         break;
     }
@@ -245,7 +456,45 @@ void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping)
     }
 
     _param_request.origin = ping->origin;
-    _request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
+    _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
+}
+
+// request for device info
+void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
+{
+    debug("process_device_info_frame: %d -> %d", info->origin, info->destination);
+    if (info->destination != 0 && info->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
+        return; // request was not for us
+    }
+
+    // we are only interested in RC device info for firmware version detection
+    if (info->origin != 0 && info->origin != AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER) {
+        return;
+    }
+    /*
+        Payload size is 58:
+        char[] Device name ( Null-terminated string, max len is 42 )
+        uint32_t Serial number
+        uint32_t Hardware ID
+        uint32_t Firmware ID (0x00:0x00:0xAA:0xBB AA=major, BB=minor)
+        uint8_t Parameters count
+        uint8_t Parameter version number
+    */
+    // get the terminator of the device name string
+    const uint8_t offset = strnlen((char*)info->payload,42U);
+    /*
+        fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes
+        fw minor ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 4th byte of sw id = 12bytes
+    */
+    _crsf_version.major = info->payload[offset+11];
+    _crsf_version.minor = info->payload[offset+12];
+
+    // should we use rf_mode reported by link statistics?
+    if (_crsf_version.major >= 3 && _crsf_version.minor >= 72) {
+        _crsf_version.use_rf_mode = true;
+    }
+
+    _crsf_version.pending = false;
 }
 
 void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame)
@@ -257,7 +506,7 @@ void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_fr
     }
 
     _param_request = *read_frame;
-    _request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ;
+    _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ;
 }
 
 // process any changed settings and schedule for transmission
@@ -268,13 +517,16 @@ void AP_CRSF_Telem::update()
 void AP_CRSF_Telem::update_params()
 {
     // handle general parameter requests
-    switch (_request_pending) {
+    switch (_pending_request.frame_type) {
     case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
         calc_device_info();
         break;
     case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
         calc_parameter();
         break;
+    case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
+        calc_device_ping();
+        break;
     default:
         break;
     }
@@ -495,11 +747,24 @@ void AP_CRSF_Telem::calc_device_info() {
     _telem_size = n + 2;
     _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
 
-    _request_pending = 0;
+    _pending_request.frame_type = 0;
     _telem_pending = true;
 #endif
 }
 
+// send a device ping
+void AP_CRSF_Telem::calc_device_ping() {
+    _telem.ext.ping.destination = _pending_request.destination;
+    _telem.ext.ping.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
+    _telem_size = 2;
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
+
+    _pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST;
+    _pending_request.frame_type = 0;
+
+    _telem_pending = true;
+}
+
 // return parameter information
 void AP_CRSF_Telem::calc_parameter() {
 #if OSD_PARAM_ENABLED
@@ -601,7 +866,7 @@ void AP_CRSF_Telem::calc_parameter() {
     _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx;
     _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
 
-    _request_pending = 0;
+    _pending_request.frame_type = 0;
     _telem_pending = true;
 #endif
 }
@@ -750,7 +1015,7 @@ void AP_CRSF_Telem::calc_text_selection(AP_OSD_ParamSetting* param, uint8_t chun
     _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + chunker.bytes_written();
     _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY;
 
-    _request_pending = 0;
+    _pending_request.frame_type = 0;
     _telem_pending = true;
 }
 #endif
@@ -826,6 +1091,86 @@ void AP_CRSF_Telem::process_param_write_frame(ParameterSettingsWriteFrame* write
 #endif
 }
 
+// get status text data
+void AP_CRSF_Telem::calc_status_text()
+{
+    if (!_statustext.available) {
+        WITH_SEMAPHORE(_statustext.sem);
+        // check link speed
+        if (!is_high_speed_telemetry(_telem_rf_mode)) {
+            // keep only warning/error/critical/alert/emergency status text messages
+            bool got_message = false;
+            while (_statustext.queue.pop(_statustext.next)) {
+                if (_statustext.next.severity <= MAV_SEVERITY_WARNING) {
+                    got_message = true;
+                    break;
+                }
+            }
+            if (!got_message) {
+                return;
+            }
+        } else if (!_statustext.queue.pop(_statustext.next)) {
+            return;
+        }
+        _statustext.available = true;
+    }
+
+    _telem_type = get_custom_telem_frame_id();
+    _telem.bcast.custom_telem.status_text.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_STATUS_TEXT;
+    _telem.bcast.custom_telem.status_text.severity = _statustext.next.severity;
+    strncpy_noterm(_telem.bcast.custom_telem.status_text.text, _statustext.next.text, PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE);
+    // add a potentially missing terminator
+    _telem.bcast.custom_telem.status_text.text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE-1] = 0;
+    _telem_size = 2 + PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE; // sub_type(1) + severity(1) + text(50)
+    _telem_pending = true;
+    _statustext.available = false;
+}
+
+/*
+ Get 1 packet of passthrough telemetry data
+*/
+void AP_CRSF_Telem::get_single_packet_passthrough_telem_data()
+{
+    _telem_pending = false;
+    uint8_t packet_count;
+    AP_Frsky_SPort::sport_packet_t packet;
+    if (!AP_Frsky_Telem::get_telem_data(&packet, packet_count, 1)) {
+        return;
+    }
+    _telem.bcast.custom_telem.single_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH;
+    _telem.bcast.custom_telem.single_packet_passthrough.appid = packet.appid;
+    _telem.bcast.custom_telem.single_packet_passthrough.data = packet.data;
+    _telem_size = sizeof(AP_CRSF_Telem::PassthroughSinglePacketFrame);
+    _telem_type = get_custom_telem_frame_id();
+    _telem_pending = true;
+}
+
+/*
+ Get up to PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packets of passthrough telemetry data (for slow links)
+ Note: we have 2 distinct frame types (single packet vs multi packet) because
+ whenever possible we use smaller frames for they have a higher "chance"
+ of being transmitted by the crossfire RX scheduler.
+*/
+void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data()
+{
+    _telem_pending = false;
+    uint8_t count = 0;
+    AP_Frsky_SPort::sport_packet_t buffer[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {};
+    // we request a PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packet array, i.e. 9 packets
+    if (!AP_Frsky_Telem::get_telem_data(buffer, count, ARRAY_SIZE(buffer))) {
+        return;
+    }
+    _telem.bcast.custom_telem.multi_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH;
+    for (uint8_t idx=0; idx<count; idx++) {
+        _telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].appid = buffer[idx].appid;
+        _telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].data = buffer[idx].data;
+    }
+    _telem.bcast.custom_telem.multi_packet_passthrough.size = count;
+    _telem_size = sizeof(AP_CRSF_Telem::PassthroughMultiPacketFrame);
+    _telem_type = get_custom_telem_frame_id();
+    _telem_pending = true;
+}
+
 /*
   fetch CRSF frame data
  */
@@ -842,7 +1187,6 @@ bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data)
     data->type = _telem_type;
 
     _telem_pending = false;
-
     return true;
 }
 

libraries/AP_RCTelemetry/AP_CRSF_Telem.h

@@ -49,29 +49,34 @@ public:
     virtual bool init() override;
 
     static AP_CRSF_Telem *get_singleton(void);
+    void queue_message(MAV_SEVERITY severity, const char *text) override;
+
+    static const uint8_t PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE = 50U;
+    static const uint8_t PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE = 9U;
+    static const uint8_t CRSF_RX_DEVICE_PING_MAX_RETRY = 50U;
 
     // Broadcast frame definitions courtesy of TBS
-    struct GPSFrame {   // curious fact, calling this GPS makes sizeof(GPS) return 1!
+    struct PACKED GPSFrame {   // curious fact, calling this GPS makes sizeof(GPS) return 1!
         int32_t latitude; // ( degree / 10`000`000 )
         int32_t longitude; // (degree / 10`000`000 )
         uint16_t groundspeed; // ( km/h / 100 )
         uint16_t gps_heading; // ( degree / 100 )
         uint16_t altitude; // ( meter - 1000m offset )
         uint8_t satellites; // in use ( counter )
-    } PACKED;
+    };
 
     struct HeartbeatFrame {
         uint8_t origin; // Device addres
     };
 
-    struct BatteryFrame {
+    struct PACKED BatteryFrame {
         uint16_t voltage; // ( mV * 100 )
         uint16_t current; // ( mA * 100 )
         uint8_t capacity[3]; // ( mAh )
         uint8_t remaining; // ( percent )
-    } PACKED;
+    };
 
-    struct VTXFrame {
+    struct PACKED VTXFrame {
 #if __BYTE_ORDER != __LITTLE_ENDIAN
 #error "Only supported on little-endian architectures"
 #endif
@@ -88,45 +93,45 @@ public:
         uint16_t user_frequency;
         uint8_t power : 4;              // 25mW = 0, 200mW = 1, 500mW = 2, 800mW = 3
         uint8_t pitmode : 4;            // off = 0, In_Band = 1, Out_Band = 2;
-    } PACKED;
+    };
 
-    struct VTXTelemetryFrame {
+    struct PACKED VTXTelemetryFrame {
         uint8_t origin; // address
         uint8_t power;              // power in dBm
         uint16_t frequency;         // frequency in Mhz
         uint8_t pitmode;            // disable 0, enable 1
-    } PACKED;
+    };
 
-    struct AttitudeFrame {
+    struct PACKED AttitudeFrame {
         int16_t pitch_angle; // ( rad * 10000 )
         int16_t roll_angle; // ( rad * 10000 )
         int16_t yaw_angle; // ( rad * 10000 )
-    } PACKED;
+    };
 
-    struct FlightModeFrame {
+    struct PACKED FlightModeFrame {
         char flight_mode[16]; // ( Null-terminated string )
-    } PACKED;
+    };
 
     // CRSF_FRAMETYPE_COMMAND
-    struct CommandFrame {
+    struct PACKED CommandFrame {
         uint8_t destination;
         uint8_t origin;
         uint8_t command_id;
         uint8_t payload[9]; // 8 maximum for LED command + crc8
-    } PACKED;
+    };
 
     // CRSF_FRAMETYPE_PARAM_DEVICE_PING
-    struct ParameterPingFrame {
+    struct PACKED ParameterPingFrame {
         uint8_t destination;
         uint8_t origin;
-    } PACKED;
+    };
 
     // CRSF_FRAMETYPE_PARAM_DEVICE_INFO
-    struct ParameterDeviceInfoFrame {
+    struct PACKED ParameterDeviceInfoFrame {
         uint8_t destination;
         uint8_t origin;
         uint8_t payload[58];   // largest possible frame is 60
-    } PACKED;
+    };
 
     enum ParameterType : uint8_t
     {
@@ -144,57 +149,90 @@ public:
     };
 
     // CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
-    struct ParameterSettingsEntryHeader {
+    struct PACKED ParameterSettingsEntryHeader {
         uint8_t destination;
         uint8_t origin;
         uint8_t param_num;
         uint8_t chunks_left;
-    } PACKED;
+    };
 
     // CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY
-    struct ParameterSettingsEntry {
+    struct PACKED ParameterSettingsEntry {
         ParameterSettingsEntryHeader header;
         uint8_t payload[56];   // largest possible frame is 60
-    } PACKED;
+    };
 
     // CRSF_FRAMETYPE_PARAMETER_READ
-    struct ParameterSettingsReadFrame {
+    struct PACKED ParameterSettingsReadFrame {
         uint8_t destination;
         uint8_t origin;
         uint8_t param_num;
         uint8_t param_chunk;
-    } PACKED _param_request;
+    } _param_request;
 
     // CRSF_FRAMETYPE_PARAMETER_WRITE
-    struct ParameterSettingsWriteFrame {
+    struct PACKED ParameterSettingsWriteFrame {
         uint8_t destination;
         uint8_t origin;
         uint8_t param_num;
         uint8_t payload[57];   // largest possible frame is 60
-    } PACKED;
+    };
 
-    union BroadcastFrame {
+    // Frame to hold passthrough telemetry
+    struct PACKED PassthroughSinglePacketFrame {
+        uint8_t sub_type;
+        uint16_t appid;
+        uint32_t data;
+    };
+
+    // Frame to hold passthrough telemetry
+    struct PACKED PassthroughMultiPacketFrame {
+        uint8_t sub_type;
+        uint8_t size;
+        struct PACKED {
+            uint16_t appid;
+            uint32_t data;
+        } frames[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE];
+    };
+
+    // Frame to hold status text message
+    struct PACKED StatusTextFrame {
+        uint8_t sub_type;
+        uint8_t severity;
+        char text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE];  // ( Null-terminated string )
+    };
+
+    // ardupilot frametype container
+    union PACKED APCustomTelemFrame {
+        PassthroughSinglePacketFrame single_packet_passthrough;
+        PassthroughMultiPacketFrame multi_packet_passthrough;
+        StatusTextFrame status_text;
+    };
+
+
+    union PACKED BroadcastFrame {
         GPSFrame gps;
         HeartbeatFrame heartbeat;
         BatteryFrame battery;
         VTXFrame vtx;
         AttitudeFrame attitude;
         FlightModeFrame flightmode;
-    } PACKED;
+        APCustomTelemFrame custom_telem;
+    };
 
-    union ExtendedFrame {
+    union PACKED ExtendedFrame {
         CommandFrame command;
         ParameterPingFrame ping;
         ParameterDeviceInfoFrame info;
         ParameterSettingsEntry param_entry;
         ParameterSettingsReadFrame param_read;
         ParameterSettingsWriteFrame param_write;
-    } PACKED;
+    };
 
-    union TelemetryPayload {
+    union PACKED TelemetryPayload {
         BroadcastFrame bcast;
         ExtendedFrame ext;
-    } PACKED;
+    };
 
     // Process a frame from the CRSF protocol decoder
     static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
@@ -213,6 +251,8 @@ private:
         BATTERY,
         GPS,
         FLIGHT_MODE,
+        PASSTHROUGH,
+        STATUS_TEXT,
         NUM_SENSORS
     };
 
@@ -220,6 +260,8 @@ private:
     bool is_packet_ready(uint8_t idx, bool queue_empty) override;
     void process_packet(uint8_t idx) override;
     void adjust_packet_weight(bool queue_empty) override;
+    void setup_custom_telemetry();
+    void update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode);
 
     void calc_parameter_ping();
     void calc_heartbeat();
@@ -228,33 +270,65 @@ private:
     void calc_attitude();
     void calc_flight_mode();
     void calc_device_info();
+    void calc_device_ping();
     void calc_parameter();
 #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED
     void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk);
 #endif
     void update_params();
     void update_vtx_params();
+    void get_single_packet_passthrough_telem_data();
+    void get_multi_packet_passthrough_telem_data();
+    void calc_status_text();
+    void process_rf_mode_changes();
+    uint8_t get_custom_telem_frame_id() const;
+    AP_RCProtocol_CRSF::RFMode get_rf_mode() const;
+    bool is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const;
 
     void process_vtx_frame(VTXFrame* vtx);
     void process_vtx_telem_frame(VTXTelemetryFrame* vtx);
     void process_ping_frame(ParameterPingFrame* ping);
     void process_param_read_frame(ParameterSettingsReadFrame* read);
     void process_param_write_frame(ParameterSettingsWriteFrame* write);
+    void process_device_info_frame(ParameterDeviceInfoFrame* info);
 
-     // setup ready for passthrough operation
+    // setup ready for passthrough operation
     void setup_wfq_scheduler(void) override;
 
-     // get next telemetry data for external consumers
+    // setup the scheduler for parameters download
+    void enter_scheduler_params_mode();
+    void exit_scheduler_params_mode();
+
+    // get next telemetry data for external consumers
     bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data);
     bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
 
     TelemetryPayload _telem;
     uint8_t _telem_size;
     uint8_t _telem_type;
+    AP_RCProtocol_CRSF::RFMode _telem_rf_mode;
 
     bool _telem_pending;
     bool _enable_telemetry;
-    uint8_t _request_pending;
+
+    struct {
+        uint8_t destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST;
+        uint8_t frame_type;
+    } _pending_request;
+
+    struct {
+        uint8_t minor;
+        uint8_t major;
+        uint8_t retry_count;
+        bool use_rf_mode;
+        bool pending = true;
+    } _crsf_version;
+
+    struct {
+        bool init_done;
+        uint32_t params_mode_start_ms;
+        bool params_mode_active;
+    } _custom_telem;
 
     // vtx state
     bool _vtx_freq_update;  // update using the frequency method or not

libraries/AP_RCTelemetry/AP_RCTelemetry.cpp

@@ -13,7 +13,7 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-/* 
+/*
    Abstract Telemetry library
 */
 
@@ -60,12 +60,12 @@ void AP_RCTelemetry::update_avg_packet_rate()
     uint32_t poll_now = AP_HAL::millis();
 
     _scheduler.avg_packet_counter++;
-    
+
     if (poll_now - _scheduler.last_poll_timer > 1000) { //average in last 1000ms
         // initialize
         if (_scheduler.avg_packet_rate == 0) _scheduler.avg_packet_rate = _scheduler.avg_packet_counter;
         // moving average
-        _scheduler.avg_packet_rate = (uint8_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f;
+        _scheduler.avg_packet_rate = (uint16_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f;
         // reset
         _scheduler.last_poll_timer = poll_now;
         _scheduler.avg_packet_counter = 0;
@@ -83,14 +83,16 @@ void AP_RCTelemetry::update_avg_packet_rate()
 
 /*
  * WFQ scheduler
+ * returns the actual packet type index (if any) sent by the scheduler
  */
-void AP_RCTelemetry::run_wfq_scheduler(void)
+uint8_t AP_RCTelemetry::run_wfq_scheduler(const bool use_shaper)
 {
     update_avg_packet_rate();
+    update_max_packet_rate();
 
     uint32_t now = AP_HAL::millis();
     int8_t max_delay_idx = -1;
-    
+
     float max_delay = 0;
     float delay = 0;
     bool packet_ready = false;
@@ -99,7 +101,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
     check_sensor_status_flags();
     // build message queue for ekf_status
     check_ekf_status();
-    
+
     // dynamic priorities
     bool queue_empty;
     {
@@ -108,7 +110,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
     }
 
     adjust_packet_weight(queue_empty);
-    
+
     // search the packet with the longest delay after the scheduled time
     for (int i=0; i<_time_slots; i++) {
         // normalize packet delay relative to packet weight
@@ -116,8 +118,8 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
         // use >= so with equal delays we choose the packet with lowest priority
         // this is ensured by the packets being sorted by desc frequency
         // apply the rate limiter
-        if (delay >= max_delay && ((now - _scheduler.packet_timer[i]) >= _scheduler.packet_min_period[i])) {
-            packet_ready = is_packet_ready(i, queue_empty);
+        if (delay >= max_delay && check_scheduler_entry_time_constraints(now, i, use_shaper)) {
+            packet_ready = is_scheduler_entry_enabled(i) && is_packet_ready(i, queue_empty);
 
             if (packet_ready) {
                 max_delay = delay;
@@ -125,10 +127,10 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
             }
         }
     }
-
     if (max_delay_idx < 0) {  // nothing was ready
-        return;
+        return max_delay_idx;
     }
+
     now = AP_HAL::millis();
 #ifdef TELEM_DEBUG
     _scheduler.packet_rate[max_delay_idx] = (_scheduler.packet_rate[max_delay_idx] + 1000 / (now - _scheduler.packet_timer[max_delay_idx])) / 2;
@@ -137,6 +139,32 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
     //debug("process_packet(%d): %f", max_delay_idx, max_delay);
     // send packet
     process_packet(max_delay_idx);
+    // let the caller know which packet type was sent
+    return max_delay_idx;
+}
+
+/*
+ * do not run the scheduler and process a specific entry
+ */
+bool AP_RCTelemetry::process_scheduler_entry(const uint8_t slot )
+{
+    if (slot >= TELEM_TIME_SLOT_MAX) {
+        return false;
+    }
+    if (!is_scheduler_entry_enabled(slot)) {
+        return false;
+    }
+    bool queue_empty;
+    {
+        WITH_SEMAPHORE(_statustext.sem);
+        queue_empty = !_statustext.available && _statustext.queue.is_empty();
+    }
+    if (!is_packet_ready(slot, queue_empty)) {
+        return false;
+    }
+    process_packet(slot);
+
+    return true;
 }
 
 /*
@@ -248,7 +276,7 @@ void AP_RCTelemetry::check_ekf_status(void)
         }
     }
 }
-      
+
 uint32_t AP_RCTelemetry::sensor_status_flags() const
 {
     uint32_t present;
@@ -258,4 +286,3 @@ uint32_t AP_RCTelemetry::sensor_status_flags() const
 
     return ~health & enabled & present;
 }
-

libraries/AP_RCTelemetry/AP_RCTelemetry.h

@@ -17,6 +17,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_HAL/utility/RingBuffer.h>
+#include <AP_Math/AP_Math.h>
 
 #define TELEM_PAYLOAD_STATUS_CAPACITY          5 // size of the message buffer queue (max number of messages waiting to be sent)
 
@@ -34,23 +35,66 @@ public:
     AP_RCTelemetry &operator=(const AP_RCTelemetry&) = delete;
 
     // add statustext message to message queue
-    void queue_message(MAV_SEVERITY severity, const char *text);
+    virtual void queue_message(MAV_SEVERITY severity, const char *text);
+
+    // scheduler entry helpers
+    void enable_scheduler_entry(const uint8_t slot) {
+        if (slot >= TELEM_TIME_SLOT_MAX) {
+            return;
+        }
+        BIT_CLEAR(_disabled_scheduler_entries_bitmask, slot);
+    }
+    void disable_scheduler_entry(const uint8_t slot) {
+        if (slot >= TELEM_TIME_SLOT_MAX) {
+            return;
+        }
+        BIT_SET(_disabled_scheduler_entries_bitmask, slot);
+    }
+    void set_scheduler_entry_min_period(const uint8_t slot, const uint32_t min_period_ms)
+    {
+        if (slot >= TELEM_TIME_SLOT_MAX) {
+            return;
+        }
+        _scheduler.packet_min_period[slot] = min_period_ms;
+    }
+    bool is_scheduler_entry_enabled(const uint8_t slot) const {
+        if (slot >= TELEM_TIME_SLOT_MAX) {
+            return false;
+        }
+        return !BIT_IS_SET(_disabled_scheduler_entries_bitmask, slot);
+    }
+    // each derived class might provide a way to reset telemetry rates to default
+    virtual void reset_scheduler_entry_min_periods() {}
 
     // update error mask of sensors and subsystems. The mask uses the
     // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
     // indicates that the sensor or subsystem is present but not
     // functioning correctly
     uint32_t sensor_status_flags() const;
+    uint16_t get_avg_packet_rate() const {
+        return _scheduler.avg_packet_rate;
+    }
+    uint16_t get_max_packet_rate() const {
+        return _scheduler.max_packet_rate;
+    }
 
 protected:
-    void run_wfq_scheduler();
+    uint8_t run_wfq_scheduler(const bool use_shaper = true);
+    // process a specific entry
+    bool process_scheduler_entry(const uint8_t slot );
     // set an entry in the scheduler table
     void set_scheduler_entry(uint8_t slot, uint32_t weight, uint32_t min_period_ms) {
+        if (slot >= TELEM_TIME_SLOT_MAX) {
+            return;
+        }
         _scheduler.packet_weight[slot] = weight;
         _scheduler.packet_min_period[slot] = min_period_ms;
     }
     // add an entry to the scheduler table
     void add_scheduler_entry(uint32_t weight, uint32_t min_period_ms) {
+        if (_time_slots >= TELEM_TIME_SLOT_MAX) {
+            return;
+        }
         set_scheduler_entry(_time_slots++, weight, min_period_ms);
     }
     // setup ready for passthrough operation
@@ -65,7 +109,8 @@ protected:
         uint32_t packet_timer[TELEM_TIME_SLOT_MAX];
         uint32_t packet_weight[TELEM_TIME_SLOT_MAX];
         uint32_t packet_min_period[TELEM_TIME_SLOT_MAX];
-        uint8_t avg_packet_rate;
+        uint16_t avg_packet_rate;
+        uint16_t max_packet_rate;
 #ifdef TELEM_DEBUG
         uint8_t packet_rate[TELEM_TIME_SLOT_MAX];
 #endif
@@ -81,6 +126,7 @@ protected:
 private:
     uint32_t check_sensor_status_timer;
     uint32_t check_ekf_status_timer;
+    uint32_t _disabled_scheduler_entries_bitmask;
 
     // passthrough WFQ scheduler
     virtual void setup_wfq_scheduler() = 0;
@@ -88,8 +134,17 @@ private:
     virtual bool is_packet_ready(uint8_t idx, bool queue_empty) { return true; }
     virtual void process_packet(uint8_t idx) = 0;
     virtual void adjust_packet_weight(bool queue_empty) {};
+    bool check_scheduler_entry_time_constraints(const uint32_t now, uint8_t slot, const bool use_shaper) const {
+        if (!use_shaper) {
+            return true;
+        }
+        return ((now - _scheduler.packet_timer[slot]) >= _scheduler.packet_min_period[slot]);
+    }
 
     void update_avg_packet_rate();
+    void update_max_packet_rate() {
+        _scheduler.max_packet_rate = MAX(_scheduler.avg_packet_rate, _scheduler.max_packet_rate);
+    }
 
     // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
     void check_sensor_status_flags(void);