diff --git a/radio/src/telemetry/multi.cpp b/radio/src/telemetry/multi.cpp index 2cc92f564..a8c6b3fbf 100644 --- a/radio/src/telemetry/multi.cpp +++ b/radio/src/telemetry/multi.cpp @@ -62,14 +62,7 @@ enum MultiBufferState : uint8_t NoProtocolDetected, MultiFirstByteReceived, ReceivingMultiProtocol, - ReceivingMultiStatus, - SpektrumTelemetryFallback, - FrskyTelemetryFallback, - FrskyTelemetryFallbackFirstByte, - FrskyTelemetryFallbackNextBytes, - FlyskyTelemetryFallback, - HitecTelemetryFallback, - MultiStatusOrFrskyData + ReceivingMultiStatus }; @@ -160,16 +153,6 @@ bool isMultiModeScanning(uint8_t module) return getModuleMode(module) == MODULE_MODE_GET_HARDWARE_INFO; } -static MultiBufferState guessProtocol(uint8_t module) -{ - if (g_model.moduleData[module].multi.rfProtocol == MODULE_SUBTYPE_MULTI_DSM2) - return SpektrumTelemetryFallback; - else if (g_model.moduleData[module].multi.rfProtocol == MODULE_SUBTYPE_MULTI_FS_AFHDS2A) - return FlyskyTelemetryFallback; - else - return FrskyTelemetryFallback; -} - static void processMultiScannerPacket(const uint8_t *data, const uint8_t moduleIdx) { uint8_t cur_channel = data[0]; @@ -449,9 +432,9 @@ static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module) sportProcessTelemetryPacket(TX_LQI_ID, 0, instance, data[7], UNIT_RAW); } } - } - else + } else { TRACE("[MP] Received sport telemetry len %d < 4", len); + } break; case InputSync: @@ -575,8 +558,7 @@ static void processMultiTelemetryByte(const uint8_t data, uint8_t module) if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) { rxBuffer[rxBufferCount++] = data; - } - else { + } else { TRACE("[MP] array size %d error", rxBufferCount); setMultiTelemetryBufferState(module, NoProtocolDetected); } @@ -601,9 +583,9 @@ static void processMultiTelemetryByte(const uint8_t data, uint8_t module) void processMultiTelemetryData(uint8_t data, uint8_t module) { - uint8_t * rxBuffer = getTelemetryRxBuffer(module); + uint8_t *rxBuffer = getTelemetryRxBuffer(module); uint8_t &rxBufferCount = getTelemetryRxBufferCount(module); - + // debugPrintf("State: %d, byte received %02X, buflen: %d\r\n", // getMultiTelemetryBufferState(module), data, rxBufferCount); @@ -611,69 +593,21 @@ void processMultiTelemetryData(uint8_t data, uint8_t module) case NoProtocolDetected: if (data == 'M') { setMultiTelemetryBufferState(module, MultiFirstByteReceived); - } - else if (data == 0xAA || data == 0x7e) { - setMultiTelemetryBufferState(module, guessProtocol(module)); - - // Process the first byte by the protocol - processMultiTelemetryData(data, module); - } - else { + } else { TRACE("[MP] invalid start byte 0x%02X", data); } break; - case FrskyTelemetryFallback: - setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte); - processFrskySportTelemetryData(module, data, rxBuffer, &rxBufferCount); - break; - - case FrskyTelemetryFallbackFirstByte: - if (data == 'M') { - setMultiTelemetryBufferState(module, MultiStatusOrFrskyData); - } - else { - processFrskySportTelemetryData(module, data, rxBuffer, &rxBufferCount); - if (data != 0x7e) - setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes); - } - - break; - - case FrskyTelemetryFallbackNextBytes: - processFrskySportTelemetryData(module, data, rxBuffer, &rxBufferCount); - if (data == 0x7e) { - // end of packet or start of new packet - setMultiTelemetryBufferState(module, FrskyTelemetryFallbackFirstByte); - } - break; - - case FlyskyTelemetryFallback: - processFlySkyTelemetryData(data, rxBuffer, rxBufferCount); - if (rxBufferCount == 0) { - setMultiTelemetryBufferState(module, NoProtocolDetected); - } - break; - - case SpektrumTelemetryFallback: - processSpektrumTelemetryData(module, data, rxBuffer, rxBufferCount); - if (rxBufferCount == 0) { - setMultiTelemetryBufferState(module, NoProtocolDetected); - } - break; - case MultiFirstByteReceived: rxBufferCount = 0; if (data == 'P') { setMultiTelemetryBufferState(module, ReceivingMultiProtocol); - } - else if (data >= 5 && data <= 10) { - // Protocol indented for er9x/ersky9, accept only 5-10 as packet length to have - // a bit of validation + } else if (data >= 5 && data <= 10) { + // Protocol indented for er9x/ersky9, accept only 5-10 as packet length + // to have a bit of validation setMultiTelemetryBufferState(module, ReceivingMultiStatus); processMultiTelemetryData(data, module); - } - else { + } else { TRACE("[MP] invalid second byte 0x%02X", data); setMultiTelemetryBufferState(module, NoProtocolDetected); } @@ -683,19 +617,6 @@ void processMultiTelemetryData(uint8_t data, uint8_t module) processMultiTelemetryByte(data, module); break; - case MultiStatusOrFrskyData: - // Check len byte if it makes sense for multi - if (data >= 5 && data <= 10) { - setMultiTelemetryBufferState(module, ReceivingMultiStatus); - rxBufferCount = 0; - } - else { - setMultiTelemetryBufferState(module, FrskyTelemetryFallbackNextBytes); - processMultiTelemetryData('M', module); - } - processMultiTelemetryData(data, module); - break; - case ReceivingMultiStatus: if (rxBufferCount < TELEMETRY_RX_PACKET_SIZE) { rxBuffer[rxBufferCount++] = data; @@ -706,17 +627,16 @@ void processMultiTelemetryData(uint8_t data, uint8_t module) } if (rxBufferCount > 24) { // too long ignore - TRACE("Overlong multi status packet detected ignoring, wanted %d", rxBuffer[0]); + TRACE("Multi status packet > 24 (frame len = %d)", rxBuffer[0]); rxBufferCount = 0; setMultiTelemetryBufferState(module, NoProtocolDetected); } - } - else { + } else { TRACE("[MP] array size %d error", rxBufferCount); setMultiTelemetryBufferState(module, NoProtocolDetected); } break; - + default: break; }