AP_RCProtocol: use framing API to decode CRSF

check sync byte for GHST and CRSF to avoid confusing the two protocols
do not change baudrate when scanning GHST protocol unless specifically configured

libraries/AP_RCProtocol/AP_RCProtocol.cpp

@@ -147,6 +147,7 @@ void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
             // this protocol is disabled for pulse input
             continue;
         }
+
         if (backend[i] != nullptr) {
             if (!protocol_enabled(rcprotocol_t(i))) {
                 continue;
@@ -197,6 +198,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();
@@ -226,12 +254,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);
@@ -309,7 +339,7 @@ static const AP_RCProtocol::SerialConfig serial_configs[] {
 #endif
 };
 
-static_assert(ARRAY_SIZE(serial_configs) > 1, "must have at least one serial config");
+static_assert(ARRAY_SIZE(serial_configs) > 0, "must have at least one serial config");
 
 void AP_RCProtocol::check_added_uart(void)
 {
@@ -333,14 +363,32 @@ void AP_RCProtocol::check_added_uart(void)
     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[PROTOCOL_MAX_FRAME_SIZE];
+        while (added.uart->frame_available()) {
+            ssize_t nbytes = added.uart->read_frame(buf, PROTOCOL_MAX_FRAME_SIZE);
+            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)) {
+                    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

@@ -94,6 +94,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

@@ -99,6 +99,11 @@ class AP_RCProtocol_Backend {
         return frontend._detected_protocol != AP_RCProtocol::NONE && frontend.backend[frontend._detected_protocol] == this;
     }
 
+    // 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);
@@ -132,11 +137,14 @@ 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;
     int16_t rssi = -1;
     int16_t rx_link_quality = -1;
 };
 
+#define PROTOCOL_MAX_FRAME_SIZE 64
+
 #endif  // AP_RCPROTOCOL_ENABLED

libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp

@@ -231,13 +231,27 @@ 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
     if (_frame_ofs < CSRF_HEADER_TYPE_LEN) {
         return;
     }
 
+    if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
+        return;
+    }
+
     // parse the length
     if (_frame_ofs == CSRF_HEADER_TYPE_LEN) {
         _frame_crc = crc8_dvb_s2(0, _frame.type);
@@ -280,6 +294,34 @@ 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)
+{
+    // nothing to do
+    if (onoff == AP_RCProtocol_Backend::frame_input_enabled()) {
+        return;
+    }
+
+    // due to bugs in CRSFv3, framing is not possible at higher baudrates
+    if (onoff && uart->get_baud_rate() > CRSF_BAUDRATE) {
+        return;
+    }
+
+    AP_RCProtocol_Backend::frame_input_enabled(uart, onoff);
+    if (onoff) {
+        uart->begin_framing(CRSF_FRAMELEN_MAX);
+    } else {
+        uart->end_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 +454,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);
+            }
         }
         _new_baud_rate = 0;
     }
@@ -598,6 +643,11 @@ bool AP_RCProtocol_CRSF::change_baud_rate(uint32_t baudrate)
         return false;
     }
 #endif
+
+    if (AP_RCProtocol_Backend::frame_input_enabled()) {
+        return false;
+    }
+
     if (baudrate > CRSF_BAUDRATE_2MBIT) {
         return false;
     }

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

libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp

@@ -152,6 +152,10 @@ void AP_RCProtocol_GHST::_process_byte(uint32_t timestamp_us, uint8_t byte)
         return;
     }
 
+    if (_frame.device_address != DeviceAddress::GHST_ADDRESS_FLIGHT_CONTROLLER) {
+        return;
+    }
+
     // parse the length
     if (_frame_ofs == GHST_HEADER_TYPE_LEN) {
         _frame_crc = crc8_dvb_s2(0, _frame.type);
@@ -421,17 +425,17 @@ void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate)
     _process_byte(AP_HAL::micros(), byte);
 }
 
-// change the bootstrap baud rate to ELRS standard if configured
+// change the bootstrap baud rate to Ghost standard if configured
 void AP_RCProtocol_GHST::process_handshake(uint32_t baudrate)
 {
     AP_HAL::UARTDriver *uart = get_current_UART();
 
-    // only change the baudrate if we are bootstrapping CRSF
+    // only change the baudrate if we are specifically bootstrapping Ghost
     if (uart == nullptr
         || baudrate != CRSF_BAUDRATE
         || baudrate == GHST_BAUDRATE
         || uart->get_baud_rate() == GHST_BAUDRATE
-        || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::GHST)+1))+1)) == 0) {
+        || (get_rc_protocols_mask() & (1U<<(uint8_t(AP_RCProtocol::GHST)+1))) == 0) {
         return;
     }