From 50cb102a348b8e83bab0c357961f5dacbe2821bb Mon Sep 17 00:00:00 2001
From: Arne Schwabe <arne@rfc2549.org>
Date: Tue, 25 Apr 2017 12:07:34 +0200
Subject: [PATCH] [MULTI] Fix fallback to Flysky and Spektrum telemetry without
 MULTI_TELEMETRY (#4851)

---
 radio/src/telemetry/multi.cpp | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/radio/src/telemetry/multi.cpp b/radio/src/telemetry/multi.cpp
index 86fd04210..66fe58add 100644
--- a/radio/src/telemetry/multi.cpp
+++ b/radio/src/telemetry/multi.cpp
@@ -39,6 +39,7 @@ enum MultiBufferState : uint8_t {
   ReceivingMultiProtocol,
   ReceivingMultiStatus,
   SpektrumTelemetryFallback,
+  FrskyTelemetryFallback,
   FrskyTelemetryFallbackFirstByte,
   FrskyTelemetryFallbackNextBytes,
   FlyskyTelemetryFallback
@@ -51,7 +52,7 @@ MultiBufferState guessProtocol()
   else if (g_model.moduleData[EXTERNAL_MODULE].getMultiProtocol(false) == MM_RF_PROTO_FS_AFHDS2A)
     return FlyskyTelemetryFallback;
   else
-    return FrskyTelemetryFallbackFirstByte;
+    return FrskyTelemetryFallback;
 }
 
 static void processMultiStatusPacket(const uint8_t *data)
@@ -215,17 +216,25 @@ void processMultiTelemetryData(const uint8_t data)
     case NoProtocolDetected:
       if (data == 'M') {
         multiTelemetryBufferState = MultiFirstByteReceived;
-      } else if (data == 0x55 || data == 0x7e) {
+      } else if (data == 0xAA || data == 0x7e) {
         multiTelemetryBufferState = guessProtocol();
-        if (multiTelemetryBufferState == FrskyTelemetryFallbackFirstByte)
-          processFrskyTelemetryData(data);
+
+        // Process the first byte by the protocol
+        processMultiTelemetryData(data);
       } else {
         TRACE("[MP] invalid start byte 0x%02X", data);
       }
       break;
 
+    case FrskyTelemetryFallback:
+      multiTelemetryBufferState = FrskyTelemetryFallbackFirstByte;
+      processFrskyTelemetryData(data);
+      break;
+
     case FrskyTelemetryFallbackFirstByte:
+      processFrskyTelemetryData(data);
       multiTelemetryBufferState = FrskyTelemetryFallbackNextBytes;
+      break;
 
     case FrskyTelemetryFallbackNextBytes:
       processFrskyTelemetryData(data);