piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Bryan Mayland
2022-03-06 13:02:43 +10:00
committed by GitHub
parent c4fa8c1ed4
commit 348b98d074
1 changed files with 45 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/crossfire.cpp
+45 -9
View File
@@ -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) {
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
}
}
}