Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-25 01:05:10 +03:00
Code Issues Wiki Activity

Fix problem with multi frsky fallback (#5496)

Browse source 
* Fix problem with frsky fallback

* Try to fix code style
This commit is contained in:
Arne Schwabe 2017-12-18 22:58:24 +01:00 • committed by Bertrand Songis
parent 0dbad182e8
commit d0ec3e0266
1 changed files with 38 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

49
radio/src/telemetry/multi.cpp
View file

@ -46,7 +46,8 @@ enum MultiBufferState : uint8_t {
FrskyTelemetryFallback, FrskyTelemetryFallback,
FrskyTelemetryFallbackFirstByte, FrskyTelemetryFallbackFirstByte,
FrskyTelemetryFallbackNextBytes, FrskyTelemetryFallbackNextBytes,
FlyskyTelemetryFallback FlyskyTelemetryFallback,
MultiStatusOrFrskyData
}; };
MultiBufferState guessProtocol() MultiBufferState guessProtocol()
@ -347,6 +348,7 @@ static void processMultiTelemetryByte(const uint8_t data)
void processMultiTelemetryData(const uint8_t data) void processMultiTelemetryData(const uint8_t data)
{ {
// debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", multiTelemetryBufferState, data, telemetryRxBufferCount);
switch (multiTelemetryBufferState) { switch (multiTelemetryBufferState) {
case NoProtocolDetected: case NoProtocolDetected:
if (data == 'M') { if (data == 'M') {
@ -369,18 +371,22 @@ void processMultiTelemetryData(const uint8_t data)
break; break;
case FrskyTelemetryFallbackFirstByte: case FrskyTelemetryFallbackFirstByte:
processFrskyTelemetryData(data); if (data == 'M') {
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes; multiTelemetryBufferState = MultiStatusOrFrskyData;
}
else {
processFrskyTelemetryData(data);
if (data != 0x7e)
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
}
break; break;
case FrskyTelemetryFallbackNextBytes: case FrskyTelemetryFallbackNextBytes:
processFrskyTelemetryData(data); processFrskyTelemetryData(data);
if (data == 0x7e) if (data == 0x7e)
// might start a new packet // end of packet or start of new packet
multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte; multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte;
else if (telemetryRxBufferCount == 0 && data != 0x7d)
// Should be in a frame (no bytestuff), but the Frsky parser has discarded the byte
multiTelemetryBufferState = NoProtocolDetected;
break; break;
case FlyskyTelemetryFallback: case FlyskyTelemetryFallback:
@ -404,7 +410,7 @@ void processMultiTelemetryData(const uint8_t data)
// Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have // Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have
// a bit of validation // a bit of validation
multiTelemetryBufferState = ReceivingMultiStatus; multiTelemetryBufferState = ReceivingMultiStatus;
processMultiTelemetryData(data);
} }
else { else {
TRACE("[MP] invalid second byte 0x%02X", data); TRACE("[MP] invalid second byte 0x%02X", data);
@ -416,14 +422,35 @@ void processMultiTelemetryData(const uint8_t data)
processMultiTelemetryByte(data); processMultiTelemetryByte(data);
break; break;
case MultiStatusOrFrskyData:
// Check len byte if it makes sense for multi
if (data >= 5 && data <= 10) {
multiTelemetryBufferState = ReceivingMultiStatus;
telemetryRxBufferCount = 0;
}
else {
multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
processMultiTelemetryData('M');
}
processMultiTelemetryData(data);
break;
case ReceivingMultiStatus: case ReceivingMultiStatus:
// Ignore multi status
telemetryRxBuffer[telemetryRxBufferCount++] = data; telemetryRxBuffer[telemetryRxBufferCount++] = data;
if (telemetryRxBufferCount > 5) { if (telemetryRxBufferCount > 5 && telemetryRxBuffer[0] == telemetryRxBufferCount-1) {
processMultiStatusPacket(telemetryRxBuffer); processMultiStatusPacket(telemetryRxBuffer+1);
telemetryRxBufferCount = 0; telemetryRxBufferCount = 0;
multiTelemetryBufferState = NoProtocolDetected; multiTelemetryBufferState = NoProtocolDetected;
} }
if (telemetryRxBufferCount > 10) {
// too long ignore
TRACE("Overlong multi status packet detected ignoring, wanted %d", telemetryRxBuffer[0]);
telemetryRxBufferCount =0;
multiTelemetryBufferState = NoProtocolDetected;
}
} }
} }