AP_RCProtocol: decode high duty-cycle CRSF frames using frame markers…

… rather than timeouts
ArduPilot · Feb 10, 2024 · d1ba6f0 · d1ba6f0
1 parent e8b4010
commit d1ba6f0
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 10 deletions.
diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
@@ -146,7 +146,7 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0)
 # define debug(fmt, args...)	do {} while(0)
 #endif
 
-#define CRSF_FRAME_TIMEOUT_US      10000U // 10ms to account for scheduling delays
+#define CRSF_FRAME_TIMEOUT_US      100000U // 10ms to account for scheduling delays
 #define CRSF_INTER_FRAME_TIME_US_250HZ    4000U // At fastest, frames are sent by the transmitter every 4 ms, 250 Hz
 #define CRSF_INTER_FRAME_TIME_US_150HZ    6667U // At medium, frames are sent by the transmitter every 6.667 ms, 150 Hz
 #define CRSF_INTER_FRAME_TIME_US_50HZ    20000U // At slowest, frames are sent by the transmitter every 20ms, 50 Hz
@@ -223,7 +223,7 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
 
     // overflow check
     if (_frame_ofs >= CRSF_FRAMELEN_MAX) {
-        _frame_ofs = 0;
+        skip_to_next_frame(timestamp_us);
     }
 
     // start of a new frame
@@ -239,6 +239,7 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
     }
 
     if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) {
+        skip_to_next_frame(timestamp_us);
         return;
     }
 
@@ -247,7 +248,7 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
         _frame_crc = crc8_dvb_s2(0, _frame.type);
         // check for garbage frame
         if (_frame.length > CRSF_FRAME_PAYLOAD_MAX) {
-            _frame_ofs = 0;
+            skip_to_next_frame(timestamp_us);
         }
         return;
     }
@@ -257,24 +258,24 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
         _frame_crc = crc8_dvb_s2(_frame_crc, byte);
     }
 
-    // overflow check
+    // overflow check, should never happen
     if (_frame_ofs > _frame.length + CSRF_HEADER_LEN) {
-        _frame_ofs = 0;
+        skip_to_next_frame(timestamp_us);
         return;
     }
 
     // decode whatever we got and expect
     if (_frame_ofs == _frame.length + CSRF_HEADER_LEN) {
-        log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CSRF_HEADER_LEN);
-
-        // we consumed the partial frame, reset
-        _frame_ofs = 0;
-
         // bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index)
         if (_frame_crc != _frame.payload[_frame.length - 2]) {
+            skip_to_next_frame(timestamp_us);
             return;
         }
 
+        log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CSRF_HEADER_LEN);
+
+        // we consumed the partial frame, reset
+        _frame_ofs = 0;
         _last_frame_time_us = _last_rx_frame_time_us = timestamp_us;
         // decode here
         if (decode_crsf_packet()) {
@@ -284,6 +285,27 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
     }
 }
 
+bool AP_RCProtocol_CRSF::skip_to_next_frame(uint32_t timestamp_us)
+{
+    // need to check from after the current device address marker (0xC8)
+    uint8_t* frame_start = (uint8_t*)memchr(&_frame + sizeof(uint8_t), DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER, _frame_ofs - 1);
+    uint8_t frame_bytes = (uint32_t)&_frame - (uint32_t)frame_start + _frame_ofs - 1;
+
+    _frame_ofs = 0;
+
+    if (frame_start == nullptr) {
+        return false;
+    }
+
+    _frame_ofs = 0;
+
+    for (uint8_t ofs = 0; ofs < frame_bytes; ofs++) {
+        _process_byte(timestamp_us, (uint8_t)frame_start[ofs]);
+    }
+
+    return true;
+}
+
 void AP_RCProtocol_CRSF::update(void)
 {
     // if we are in standalone mode, process data from the uart

diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
@@ -298,6 +298,7 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
     static AP_RCProtocol_CRSF* _singleton;
 
     void _process_byte(uint32_t timestamp_us, uint8_t byte);
+    bool skip_to_next_frame(uint32_t timestamp_us);
     bool decode_crsf_packet();
     bool process_telemetry(bool check_constraint = true);
     void process_link_stats_frame(const void* data);