diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 0033e7c85..a1dbb1629 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/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 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 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) { - uint8_t length = rxBuffer[1]; - if (length + 2 == rxBufferCount) { + // rxBuffer[1] holds the packet length-2, check if the whole packet was received + while (rxBufferCount > 4 && (rxBuffer[1]+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 + } } }