fix: On CRSF CRC error, search for second packet embedded (#1533)

radio/src/telemetry/crossfire.cpp

@@ -117,11 +117,6 @@ void processCrossfireTelemetryFrame(uint8_t module)
   uint8_t * rxBuffer = getTelemetryRxBuffer(module);
   uint8_t &rxBufferCount = getTelemetryRxBufferCount(module);
 
-  if (!checkCrossfireTelemetryFrameCRC(module)) {
-    TRACE("[XF] CRC error");
-    return;
-  }
-
   if (telemetryState == TELEMETRY_INIT &&
       moduleState[module].counter != CRSF_FRAME_MODELID_SENT) {
     moduleState[module].counter = CRSF_FRAME_MODELID;
@@ -256,10 +251,47 @@ void processCrossfireTelemetryFrame(uint8_t module)
   }
 }
 
+bool crossfireLenIsSane(uint8_t len)
+{
+  // packet len must be at least 3 bytes (type+payload+crc) and 2 bytes < MAX (hdr+len)
+  return (len > 2 && len < TELEMETRY_RX_PACKET_SIZE-1);
+}
+
+void crossfireTelemetrySeekStart(uint8_t *rxBuffer, uint8_t &rxBufferCount)
+{
+  // Bad telemetry packets frequently are just truncated packets, with the start
+  // of a new packet contained in the data. This causes multiple packet drops as
+  // the parser tries to resync.
+  // Search through the rxBuffer for a sync byte, shift the contents if found
+  // and reduce rxBufferCount
+  for (uint8_t idx=1; idx<rxBufferCount; ++idx) {
+    uint8_t data = rxBuffer[idx];
+    if (data == RADIO_ADDRESS || data == UART_SYNC) {
+      uint8_t remain = rxBufferCount - idx;
+      // If there's at least 2 bytes, check the length for validity too
+      if (remain > 1 && !crossfireLenIsSane(rxBuffer[idx+1]))
+        continue;
+
+      //TRACE("Found 0x%02x with %u remain", data, remain);
+      // copy the data to the front of the buffer
+      for (uint8_t src=idx; src<rxBufferCount; ++src) {
+        rxBuffer[src-idx] = rxBuffer[src];
+      }
+
+      rxBufferCount = remain;
+      return;
+    } // if found sync
+  }
+
+  // Not found, clear the buffer
+  rxBufferCount = 0;
+}
+
 void processCrossfireTelemetryData(uint8_t data, uint8_t module)
 {
   uint8_t * rxBuffer = getTelemetryRxBuffer(module);
   uint8_t &rxBufferCount = getTelemetryRxBufferCount(module);
+
 #if defined(AUX_SERIAL)
   if (g_eeGeneral.auxSerialMode == UART_MODE_TELEMETRY_MIRROR) {
     auxSerialPutc(data);
@@ -277,7 +309,7 @@ void processCrossfireTelemetryData(uint8_t data, uint8_t module)
     return;
   }
 
-  if (rxBufferCount == 1 && (data < 2 || data > TELEMETRY_RX_PACKET_SIZE-2)) {
+  if (rxBufferCount == 1 && !crossfireLenIsSane(data)) {
     TRACE("[XF] length 0x%02X error", data);
     rxBufferCount = 0;
     return;
@@ -291,9 +323,9 @@ void processCrossfireTelemetryData(uint8_t data, uint8_t module)
     rxBufferCount = 0;
   }
 
-  if (rxBufferCount > 4) {
+  // rxBuffer[1] holds the packet length-2, check if the whole packet was received
-    uint8_t length = rxBuffer[1];
+  while (rxBufferCount > 4 && (rxBuffer[1]+2) == rxBufferCount) {
-    if (length + 2 == rxBufferCount) {
+    if (checkCrossfireTelemetryFrameCRC(module)) {
 #if defined(BLUETOOTH)
       if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
           bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
@@ -303,6 +335,10 @@ void processCrossfireTelemetryData(uint8_t data, uint8_t module)
       processCrossfireTelemetryFrame(module);
       rxBufferCount = 0;
     }
+    else {
+      TRACE("[XF] CRC error ");
+      crossfireTelemetrySeekStart(rxBuffer, rxBufferCount); // adjusts rxBufferCount
+    }
   }
 }