AP_RCProtocol: use framing API to decode CRSF

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -193,6 +193,33 @@ void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool
     }
 }
 
+bool AP_RCProtocol::process_frame(const uint8_t* buf, ssize_t nbytes)
+{
+    uint32_t now = AP_HAL::millis();
+    bool searching = should_search(now);
+
+#if AP_RC_CHANNEL_ENABLED
+    rc_protocols_mask = rc().enabled_protocols();
+#endif
+
+    if (_detected_protocol != AP_RCProtocol::NONE &&
+        !protocol_enabled(_detected_protocol)) {
+        _detected_protocol = AP_RCProtocol::NONE;
+    }
+
+    // first try current protocol
+    if (_detected_protocol != AP_RCProtocol::NONE && !searching) {
+        backend[_detected_protocol]->process_frame(buf, nbytes);
+        if (backend[_detected_protocol]->new_input()) {
+            _new_input = true;
+            _last_input_ms = now;
+        }
+        return true;
+    }
+
+    return false;
+}
+
 bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
 {
     uint32_t now = AP_HAL::millis();
@@ -222,12 +249,14 @@ bool AP_RCProtocol::process_byte(uint8_t byte, uint32_t baudrate)
         return true;
     }
 
+
     // otherwise scan all protocols
     for (uint8_t i = 0; i < ARRAY_SIZE(backend); i++) {
         if (backend[i] != nullptr) {
             if (!protocol_enabled(rcprotocol_t(i))) {
                 continue;
             }
+
             const uint32_t frame_count = backend[i]->get_rc_frame_count();
             const uint32_t input_count = backend[i]->get_rc_input_count();
             backend[i]->process_byte(byte, baudrate);
@@ -323,20 +352,36 @@ void AP_RCProtocol::check_added_uart(void)
         added.last_config_change_ms = AP_HAL::millis();
         serial_configs[added.config_num].apply_to_uart(added.uart);
     }
-#if AP_RC_CHANNEL_ENABLED
-    rc_protocols_mask = rc().enabled_protocols();
-#endif
+
     const uint32_t current_baud = serial_configs[added.config_num].baud;
     process_handshake(current_baud);
 
-    uint32_t n = added.uart->available();
-    n = MIN(n, 255U);
-    for (uint8_t i=0; i<n; i++) {
-        int16_t b = added.uart->read();
-        if (b >= 0) {
-            process_byte(uint8_t(b), current_baud);
+    // processing in frame-based mode
+    if (!searching && backend[_detected_protocol]->frame_input_enabled()) {
+        uint8_t buf[64];
+        while (added.uart->frames_available()) {
+            ssize_t nbytes = added.uart->read_frame(buf, 64);
+            process_frame(buf, nbytes);
+        }
+    } else { // processing in byte-based mode
+        if (_detected_protocol != AP_RCProtocol::NONE && backend[_detected_protocol]->frame_input_enabled()) {
+            // searching again, make sure we are in byte mode
+            backend[_detected_protocol]->frame_input_enabled(added.uart, false);
+        }
+
+        uint32_t n = added.uart->available();
+        n = MIN(n, 255U);
+        for (uint8_t i=0; i<n; i++) {
+            int16_t b = added.uart->read();
+            if (b >= 0) {
+                if (process_byte(uint8_t(b), current_baud)) {
+                    // detected a protocol, switch to framing mode if possible
+                    backend[_detected_protocol]->frame_input_enabled(added.uart, true);
+                }
+            }
         }
     }
+
     if (searching) {
         if (now - added.last_config_change_ms > 1000) {
             // change configs if not detected once a second

libraries/AP_RCProtocol/AP_RCProtocol.h

@@ -91,6 +91,7 @@ class AP_RCProtocol {
     void process_pulse(uint32_t width_s0, uint32_t width_s1);
     void process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap);
     bool process_byte(uint8_t byte, uint32_t baudrate);
+    bool process_frame(const uint8_t* buf, ssize_t nbytes);
     void process_handshake(uint32_t baudrate);
     void update(void);
 

libraries/AP_RCProtocol/AP_RCProtocol_Backend.h

@@ -95,6 +95,11 @@ class AP_RCProtocol_Backend {
         return true;
     }
 
+    // framing API
+    virtual void frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) { _framing_enabled = onoff; }
+    bool frame_input_enabled() const { return _framing_enabled; }
+    virtual void process_frame(const uint8_t* buffer, uint16_t buflen) { }
+
 #if AP_VIDEOTX_ENABLED
     // called by static methods to confiig video transmitters:
     static void configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode);
@@ -128,6 +133,7 @@ class AP_RCProtocol_Backend {
     uint32_t rc_input_count;
     uint32_t last_rc_input_count;
     uint32_t rc_frame_count;
+    bool _framing_enabled;
 
     uint16_t _pwm_values[MAX_RCIN_CHANNELS];
     uint8_t  _num_channels;

libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp

@@ -231,6 +231,16 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
         _start_frame_time_us = timestamp_us;
     }
 
+    _process_raw_byte(timestamp_us, byte);
+}
+
+void AP_RCProtocol_CRSF::_process_raw_byte(uint32_t timestamp_us, uint8_t byte)
+{
+    // overflow check
+    if (_frame_ofs >= CRSF_FRAMELEN_MAX) {
+        _frame_ofs = 0;
+    }
+
     add_to_buffer(_frame_ofs++, byte);
 
     // need a header to get the length
@@ -280,6 +290,24 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
     }
 }
 
+void AP_RCProtocol_CRSF::frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff)
+{
+    AP_RCProtocol_Backend::frame_input_enabled(uart, onoff);
+    if (onoff) {
+        uart->begin_framing(CRSF_FRAMELEN_MAX, 5);
+    } else {
+        uart->begin_framing(1, 1);  // stop framing
+    }
+}
+
+void AP_RCProtocol_CRSF::process_frame(const uint8_t* buffer, uint16_t buflen)
+{
+    uint32_t now_us = AP_HAL::micros();
+    for (uint8_t i = 0; i < buflen; i++) {
+        _process_raw_byte(now_us, buffer[i]);
+    }
+}
+
 void AP_RCProtocol_CRSF::update(void)
 {
     // if we are in standalone mode, process data from the uart
@@ -412,6 +440,9 @@ bool AP_RCProtocol_CRSF::decode_crsf_packet()
             hal.scheduler->delay(4);
             // change the baud rate
             uart->begin(_new_baud_rate);
+            if (!AP_RCProtocol_Backend::frame_input_enabled()) {
+                uart->begin_framing(CRSF_FRAMELEN_MAX, 5);
+            }
         }
         _new_baud_rate = 0;
     }

libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h

@@ -54,6 +54,9 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
 #endif
     }
 
+    void frame_input_enabled(AP_HAL::UARTDriver* uart, bool onoff) override;
+    void process_frame(const uint8_t* buffer, uint16_t buflen) override;
+
     // is the receiver active, used to detect power loss and baudrate changes
     bool is_rx_active() const override {
         // later versions of CRSFv3 will send link rate frames every 200ms
@@ -298,6 +301,7 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
     static AP_RCProtocol_CRSF* _singleton;
 
     void _process_byte(uint32_t timestamp_us, uint8_t byte);
+    void _process_raw_byte(uint32_t timestamp_us, uint8_t byte);
     bool decode_crsf_packet();
     bool process_telemetry(bool check_constraint = true);
     void process_link_stats_frame(const void* data);