From b30c9b63cde1d10c96dd1ae528a6477770395987 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Mon, 26 Nov 2018 00:08:01 +0100
Subject: [PATCH] PXX2 S.PORT started

---
 radio/src/telemetry/frsky.cpp | 38 ++++++++++++++++++++++++++++++++++-
 1 file changed, 37 insertions(+), 1 deletion(-)

diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp
index 02622bfeb..e1ee9fa20 100644
--- a/radio/src/telemetry/frsky.cpp
+++ b/radio/src/telemetry/frsky.cpp
@@ -20,6 +20,42 @@
 
 #include "opentx.h"
 
+#if defined(PXX2)
+void processFrskyTelemetryData(uint8_t data)
+{
+  static uint8_t dataState = STATE_DATA_IDLE;
+
+  switch (dataState) {
+    case STATE_DATA_IDLE:
+      if (data == START_STOP) {
+        telemetryRxBufferCount = 0;
+        dataState = STATE_DATA_START;
+      }
+      break;
+
+    case STATE_DATA_START:
+      if (telemetryRxBufferCount == 0 && data > 0x0C) {
+        // wrong length
+        telemetryRxBufferCount = 0;
+        dataState = STATE_DATA_IDLE;
+      }
+      else if (telemetryRxBufferCount < TELEMETRY_RX_PACKET_SIZE) {
+        telemetryRxBuffer[telemetryRxBufferCount++] = data;
+        if (telemetryRxBuffer[0] + 2 == telemetryRxBufferCount) {
+          sportProcessPacket(telemetryRxBuffer + 2);
+          telemetryRxBufferCount = 0;
+          dataState = STATE_DATA_IDLE;
+        }
+      }
+      else {
+        // overflow guard
+        telemetryRxBufferCount = 0;
+        dataState = STATE_DATA_IDLE;
+      }
+      break;
+  }
+}
+#else
 void processFrskyTelemetryData(uint8_t data)
 {
   static uint8_t dataState = STATE_DATA_IDLE;
@@ -101,4 +137,4 @@ void processFrskyTelemetryData(uint8_t data)
   }
 #endif
 }
-
+#endif