From a4434bc5f3ab6a4c5626391b6d1ea0d920948aac Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Thu, 21 Feb 2019 16:57:17 +0100
Subject: [PATCH] Fixes. Flash an external module should work

---
 .../src/gui/common/stdlcd/radio_sdmanager.cpp |  1 +
 radio/src/io/frsky_device_firmware_update.cpp | 87 +++++++++++++------
 ...o_arm.h => frsky_device_firmware_update.h} | 35 ++------
 radio/src/io/frsky_pxx2.h                     | 65 ++++++++++++++
 radio/src/io/{pxx2.h => frsky_sport.h}        | 29 ++++++-
 radio/src/opentx.h                            |  2 +-
 radio/src/pulses/pulses.cpp                   |  2 +-
 radio/src/pulses/pxx2.h                       | 37 +-------
 .../common/arm/stm32/bluetooth_driver.cpp     |  6 +-
 .../arm/stm32/intmodule_serial_driver.cpp     |  2 +
 radio/src/targets/horus/extmodule_driver.cpp  |  2 +-
 radio/src/targets/horus/hal.h                 |  2 +-
 radio/src/targets/taranis/board.h             |  2 +
 .../src/targets/taranis/extmodule_driver.cpp  |  6 +-
 radio/src/targets/taranis/hal.h               |  4 +-
 radio/src/telemetry/frsky.cpp                 | 23 ++++-
 radio/src/telemetry/frsky.h                   |  3 +-
 17 files changed, 200 insertions(+), 108 deletions(-)
 rename radio/src/io/{io_arm.h => frsky_device_firmware_update.h} (71%)
 create mode 100644 radio/src/io/frsky_pxx2.h
 rename radio/src/io/{pxx2.h => frsky_sport.h} (54%)

diff --git a/radio/src/gui/common/stdlcd/radio_sdmanager.cpp b/radio/src/gui/common/stdlcd/radio_sdmanager.cpp
index 5e2d753fc..7792238d6 100644
--- a/radio/src/gui/common/stdlcd/radio_sdmanager.cpp
+++ b/radio/src/gui/common/stdlcd/radio_sdmanager.cpp
@@ -19,6 +19,7 @@
  */
 
 #include "opentx.h"
+#include "io/frsky_device_firmware_update.h"
 
 #define REFRESH_FILES()        do { reusableBuffer.sdmanager.offset = 65535; menuVerticalPosition = 0; } while(0)
 #define NODE_TYPE(fname)       fname[SD_SCREEN_FILE_LENGTH+1]
diff --git a/radio/src/io/frsky_device_firmware_update.cpp b/radio/src/io/frsky_device_firmware_update.cpp
index 66b005a17..37c42e811 100644
--- a/radio/src/io/frsky_device_firmware_update.cpp
+++ b/radio/src/io/frsky_device_firmware_update.cpp
@@ -19,6 +19,7 @@
  */
 
 #include "opentx.h"
+#include "frsky_device_firmware_update.h"
 
 #define PRIM_REQ_POWERUP    0
 #define PRIM_REQ_VERSION    1
@@ -32,10 +33,10 @@
 #define PRIM_END_DOWNLOAD   0x83
 #define PRIM_DATA_CRC_ERR   0x84
 
-void DeviceFirmwareUpdate::processFrame()
+void DeviceFirmwareUpdate::processFrame(const uint8_t * frame)
 {
-  if (frame[1] == 0x5E && frame[2] == 0x50) {
-    switch (frame[3]) {
+  if (frame[0] == 0x5E && frame[1] == 0x50) {
+    switch (frame[2]) {
       case PRIM_ACK_POWERUP :
         if (state == SPORT_POWERUP_REQ) {
           state = SPORT_POWERUP_ACK;
@@ -51,7 +52,7 @@ void DeviceFirmwareUpdate::processFrame()
 
       case PRIM_REQ_DATA_ADDR:
         if (state == SPORT_DATA_TRANSFER) {
-          address = *((uint32_t *)(&frame[4]));
+          address = *((uint32_t *)(&frame[3]));
           state = SPORT_DATA_REQ;
         }
         break;
@@ -76,6 +77,12 @@ void DeviceFirmwareUpdate::startup()
       break;
 #endif
 
+    case EXTERNAL_MODULE:
+#if defined(EXTMODULE_USART)
+      extmoduleInvertedSerialStart(57600);
+      break;
+#endif
+
     default:
       telemetryInit(PROTOCOL_TELEMETRY_FRSKY_SPORT);
       break;
@@ -87,21 +94,56 @@ void DeviceFirmwareUpdate::startup()
     EXTERNAL_MODULE_ON();
   else
     SPORT_UPDATE_POWER_ON();
-
-  watchdogSuspend(50);
-  RTOS_WAIT_MS(50);
 }
 
-bool DeviceFirmwareUpdate::readByte(uint8_t & byte)
+const uint8_t * DeviceFirmwareUpdate::readFullDuplexFrame(ModuleFifo & fifo, uint32_t timeout)
+{
+  uint8_t len = 0;
+  while (len < 10) {
+    uint32_t elapsed = 0;
+    while (!fifo.pop(frame[len])) {
+      RTOS_WAIT_MS(1);
+      if (elapsed++ >= timeout) {
+        return nullptr;
+      }
+    }
+    if (len > 0 || frame[len] == 0x7E) {
+      ++len;
+    }
+  }
+
+  return &frame[1];
+}
+
+const uint8_t * DeviceFirmwareUpdate::readHalfDuplexFrame(uint32_t timeout)
+{
+  for (int i=timeout; i>=0; i--) {
+    uint8_t byte ;
+    while (telemetryGetByte(&byte)) {
+      if (pushFrskyTelemetryData(byte)) {
+        return telemetryRxBuffer;
+      }
+    }
+    RTOS_WAIT_MS(1);
+  }
+  return nullptr;
+}
+
+const uint8_t * DeviceFirmwareUpdate::readFrame(uint32_t timeout)
 {
   switch(module) {
     case INTERNAL_MODULE:
 #if defined(INTMODULE_USART)
-      return intmoduleFifo.pop(byte);
+      return readFullDuplexFrame(intmoduleFifo, timeout);
+#endif
+
+    case EXTERNAL_MODULE:
+#if defined(EXTMODULE_USART)
+      return readFullDuplexFrame(extmoduleFifo, timeout);
 #endif
 
     default:
-      return telemetryGetByte(&byte);
+      return readHalfDuplexFrame(timeout);
   }
 }
 
@@ -114,22 +156,12 @@ bool DeviceFirmwareUpdate::waitState(State state, uint32_t timeout)
 #else
   watchdogSuspend(timeout / 10);
 
-  uint8_t len = 0;
-  while (len < 10) {
-    uint32_t elapsed = 0;
-    while (!readByte(frame[len])) {
-      RTOS_WAIT_MS(1);
-      if (elapsed++ >= timeout) {
-        return false;
-      }
-    }
-    if (len > 0 || frame[len] == 0x7E) {
-      ++len;
-    }
+  const uint8_t * frame = readFrame(timeout);
+  if (!frame) {
+    return false;
   }
 
-  processFrame();
-
+  processFrame(&frame[1]);
   return state == state;
 #endif
 }
@@ -164,6 +196,11 @@ void DeviceFirmwareUpdate::sendFrame()
       return intmoduleSendBuffer(outputTelemetryBuffer, ptr-outputTelemetryBuffer);
 #endif
 
+    case EXTERNAL_MODULE:
+#if defined(EXTMODULE_USART)
+      return extmoduleSendBuffer(outputTelemetryBuffer, ptr-outputTelemetryBuffer);
+#endif
+
     default:
       return sportSendBuffer(outputTelemetryBuffer, ptr-outputTelemetryBuffer);
   }
@@ -171,8 +208,8 @@ void DeviceFirmwareUpdate::sendFrame()
 
 const char * DeviceFirmwareUpdate::sendPowerOn()
 {
-  waitState(SPORT_IDLE, 20); // Clear the fifo
   state = SPORT_POWERUP_REQ;
+  waitState(SPORT_IDLE, 50); // Wait 50ms and clear the fifo
   for (int i=0; i<10; i++) {
     // max 10 attempts
     startFrame(PRIM_REQ_POWERUP);
diff --git a/radio/src/io/io_arm.h b/radio/src/io/frsky_device_firmware_update.h
similarity index 71%
rename from radio/src/io/io_arm.h
rename to radio/src/io/frsky_device_firmware_update.h
index 2a929d86e..729120895 100644
--- a/radio/src/io/io_arm.h
+++ b/radio/src/io/frsky_device_firmware_update.h
@@ -18,32 +18,11 @@
  * GNU General Public License for more details.
  */
 
-#ifndef _IO_ARM_H_
-#define _IO_ARM_H_
+#ifndef _FRSKY_DEVICE_FIRMWARE_UPDATE_H_
+#define _FRSKY_DEVICE_FIRMWARE_UPDATE_H_
 
 #include "dataconstants.h"
-
-#if defined(TELEMETRY_FRSKY_SPORT)
-PACK(union SportTelemetryPacket
-{
-  struct {
-    uint8_t physicalId;
-    uint8_t primId;
-    uint16_t dataId;
-    uint32_t value;
-  };
-  uint8_t raw[8];
-});
-
-bool isSportOutputBufferAvailable();
-void sportOutputPushPacket(SportTelemetryPacket * packet);
-void sportFlashDevice(ModuleIndex module, const char * filename);
-#endif
-
-#if defined(STM32)
-bool isBootloader(const char * filename);
-void bootloaderFlash(const char * filename);
-#endif
+#include "frsky_pxx2.h"
 
 class DeviceFirmwareUpdate {
     enum State {
@@ -76,9 +55,11 @@ class DeviceFirmwareUpdate {
     void startFrame(uint8_t command);
     void sendFrame();
 
-    bool readByte(uint8_t & byte);
+    const uint8_t * readFullDuplexFrame(ModuleFifo & fifo, uint32_t timeout);
+    const uint8_t * readHalfDuplexFrame(uint32_t timeout);
+    const uint8_t * readFrame(uint32_t timeout);
     bool waitState(State state, uint32_t timeout);
-    void processFrame();
+    void processFrame(const uint8_t * frame);
 
     const char * sendPowerOn();
     const char * sendReqVersion();
@@ -86,4 +67,4 @@ class DeviceFirmwareUpdate {
     const char * endTransfer();
 };
 
-#endif // _IO_ARM_H_
+#endif // _FRSKY_DEVICE_FIRMWARE_UPDATE_H_
diff --git a/radio/src/io/frsky_pxx2.h b/radio/src/io/frsky_pxx2.h
new file mode 100644
index 000000000..f19d2c5c9
--- /dev/null
+++ b/radio/src/io/frsky_pxx2.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Based on code named
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _IO_PXX2_H_
+#define _IO_PXX2_H_
+
+#include "fifo.h"
+
+#define PXX2_BAUDRATE            230400
+#define PXX2_PERIOD              4 // 4ms
+
+class ModuleFifo : public Fifo<uint8_t, 32> {
+  public:
+    bool getFrame(uint8_t * frame)
+    {
+      while (1) {
+        if (isEmpty()) {
+          return false;
+        }
+        else if (fifo[ridx] != 0x7E) {
+          skip();
+        }
+        else {
+          break;
+        }
+      }
+
+      uint32_t next = nextIndex(ridx);
+      uint8_t len = fifo[next];
+      if (unsigned(len + 4 /* 2 bytes header + 2 bytes CRC */) > size()) {
+        return false;
+      }
+
+      for (uint32_t i=0; i<=len; i++) {
+        frame[i] = fifo[next];
+        next = nextIndex(next);
+      }
+
+      // TODO CRC CHECK
+      next = nextIndex(next);
+      ridx = nextIndex(next);
+      return true;
+    }
+
+    uint32_t errors;
+};
+
+#endif
diff --git a/radio/src/io/pxx2.h b/radio/src/io/frsky_sport.h
similarity index 54%
rename from radio/src/io/pxx2.h
rename to radio/src/io/frsky_sport.h
index 3a1506a67..e00e11784 100644
--- a/radio/src/io/pxx2.h
+++ b/radio/src/io/frsky_sport.h
@@ -18,10 +18,31 @@
  * GNU General Public License for more details.
  */
 
-#ifndef _IO_PXX2_H_
-#define _IO_PXX2_H_
+#ifndef _IO_FRSKY_SPORT_H_
+#define _IO_FRSKY_SPORT_H_
 
-#define PXX2_BAUDRATE            230400
-#define PXX2_PERIOD              4 // 4ms
+#include "dataconstants.h"
 
+#if defined(TELEMETRY_FRSKY_SPORT)
+PACK(union SportTelemetryPacket
+{
+  struct {
+    uint8_t physicalId;
+    uint8_t primId;
+    uint16_t dataId;
+    uint32_t value;
+  };
+  uint8_t raw[8];
+});
+
+bool isSportOutputBufferAvailable();
+void sportOutputPushPacket(SportTelemetryPacket * packet);
+void sportFlashDevice(ModuleIndex module, const char * filename);
 #endif
+
+#if defined(STM32)
+bool isBootloader(const char * filename);
+void bootloaderFlash(const char * filename);
+#endif
+
+#endif // _IO_FRSKY_SPORT_H_
diff --git a/radio/src/opentx.h b/radio/src/opentx.h
index a5a9f388a..1324a6ae0 100644
--- a/radio/src/opentx.h
+++ b/radio/src/opentx.h
@@ -294,7 +294,7 @@ void memswap(void * a, void * b, uint8_t size);
 #endif
 
 #include "fifo.h"
-#include "io/io_arm.h"
+#include "io/frsky_sport.h"
 
 extern volatile tmr10ms_t g_tmr10ms;
 static inline tmr10ms_t get_tmr10ms()
diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp
index 799d0c3f9..2e2833430 100755
--- a/radio/src/pulses/pulses.cpp
+++ b/radio/src/pulses/pulses.cpp
@@ -19,7 +19,7 @@
  */
 
 #include "opentx.h"
-#include "io/pxx2.h"
+#include "io/frsky_pxx2.h"
 #include "pulses/pxx2.h"
 
 uint8_t s_pulses_paused = 0;
diff --git a/radio/src/pulses/pxx2.h b/radio/src/pulses/pxx2.h
index 362da452b..dcd64a9c3 100644
--- a/radio/src/pulses/pxx2.h
+++ b/radio/src/pulses/pxx2.h
@@ -22,6 +22,7 @@
 #define _PULSES_PXX2_H_
 
 #include "fifo.h"
+#include "io/frsky_pxx2.h"
 #include "./pxx.h"
 
 #define PXX2_TYPE_C_MODULE          0x01
@@ -56,42 +57,6 @@ enum PXX2BindSteps {
     BIND_OK
 };
 
-class ModuleFifo : public Fifo<uint8_t, 32> {
-  public:
-    bool getFrame(uint8_t * frame)
-    {
-      while (1) {
-        if (isEmpty()) {
-          return false;
-        }
-        else if (fifo[ridx] != 0x7E) {
-          skip();
-        }
-        else {
-          break;
-        }
-      }
-
-      uint32_t next = nextIndex(ridx);
-      uint8_t len = fifo[next];
-      if (size() < unsigned(len + 4 /* 2 bytes header + 2 bytes CRC */)) {
-        return false;
-      }
-
-      for (uint32_t i=0; i<=len; i++) {
-        frame[i] = fifo[next];
-        next = nextIndex(next);
-      }
-
-      // TODO CRC CHECK
-      next = nextIndex(next);
-      ridx = nextIndex(next);
-      return true;
-    }
-
-    uint32_t errors;
-};
-
 extern ModuleFifo intmoduleFifo;
 extern ModuleFifo extmoduleFifo;
 
diff --git a/radio/src/targets/common/arm/stm32/bluetooth_driver.cpp b/radio/src/targets/common/arm/stm32/bluetooth_driver.cpp
index 42973e7d4..3f2bb898e 100644
--- a/radio/src/targets/common/arm/stm32/bluetooth_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/bluetooth_driver.cpp
@@ -59,13 +59,13 @@ void bluetoothInit(uint32_t baudrate)
   GPIO_Init(BT_BCTS_GPIO, &GPIO_InitStructure);
 #endif
 
-  GPIO_PinAFConfig(BT_GPIO_TXRX, BT_TX_GPIO_PinSource, BT_GPIO_AF);
-  GPIO_PinAFConfig(BT_GPIO_TXRX, BT_RX_GPIO_PinSource, BT_GPIO_AF);
+  GPIO_PinAFConfig(BT_USART_GPIO, BT_TX_GPIO_PinSource, BT_GPIO_AF);
+  GPIO_PinAFConfig(BT_USART_GPIO, BT_RX_GPIO_PinSource, BT_GPIO_AF);
 
   GPIO_InitStructure.GPIO_Pin = BT_TX_GPIO_PIN | BT_RX_GPIO_PIN;
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
   GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
-  GPIO_Init(BT_GPIO_TXRX, &GPIO_InitStructure);
+  GPIO_Init(BT_USART_GPIO, &GPIO_InitStructure);
 
   USART_DeInit(BT_USART);
   USART_InitTypeDef USART_InitStructure;
diff --git a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp
index 36c6a2f45..30fcfda33 100755
--- a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp
@@ -66,6 +66,8 @@ void intmoduleSerialStart(uint32_t baudrate)
   USART_Init(INTMODULE_USART, &USART_InitStructure);
   USART_Cmd(INTMODULE_USART, ENABLE);
 
+  intmoduleFifo.clear();
+
   USART_ITConfig(INTMODULE_USART, USART_IT_RXNE, ENABLE);
   NVIC_SetPriority(INTMODULE_USART_IRQn, 6);
   NVIC_EnableIRQ(INTMODULE_USART_IRQn);
diff --git a/radio/src/targets/horus/extmodule_driver.cpp b/radio/src/targets/horus/extmodule_driver.cpp
index 86cd8257f..87a8a0ec5 100644
--- a/radio/src/targets/horus/extmodule_driver.cpp
+++ b/radio/src/targets/horus/extmodule_driver.cpp
@@ -185,7 +185,7 @@ void extmoduleSerialStart(uint32_t /*baudrate*/, uint32_t period_half_us)
 #endif
 
 #if defined(EXTMODULE_USART)
-void extmoduleSendSerialBuffer(const uint8_t * data, uint8_t size)
+void extmoduleSendBuffer(const uint8_t * data, uint8_t size)
 {
   DMA_InitTypeDef DMA_InitStructure;
   DMA_DeInit(EXTMODULE_DMA_STREAM);
diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h
index ea266b93e..1613ed2c0 100644
--- a/radio/src/targets/horus/hal.h
+++ b/radio/src/targets/horus/hal.h
@@ -631,7 +631,7 @@
 #define BT_USART                        USART6
 #define BT_GPIO_AF                      GPIO_AF_USART6
 #define BT_USART_IRQn                   USART6_IRQn
-#define BT_GPIO_TXRX                    GPIOG
+#define BT_USART_GPIO                   GPIOG
 #define BT_TX_GPIO_PIN                  GPIO_Pin_14 // PG.14
 #define BT_RX_GPIO_PIN                  GPIO_Pin_9  // PG.09
 #define BT_TX_GPIO_PinSource            GPIO_PinSource14
diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h
index 62b492951..1853d7cd2 100644
--- a/radio/src/targets/taranis/board.h
+++ b/radio/src/targets/taranis/board.h
@@ -192,6 +192,8 @@ void extmodulePpmStart();
 void extmodulePxxStart();
 void extmodulePxx2Start();
 void extmoduleSerialStart(uint32_t baudrate, uint32_t period_half_us);
+void extmoduleInvertedSerialStart(uint32_t baudrate);
+void extmoduleSendBuffer(const uint8_t * data, uint8_t size);
 void extmoduleSendNextFrame();
 
 // Trainer driver
diff --git a/radio/src/targets/taranis/extmodule_driver.cpp b/radio/src/targets/taranis/extmodule_driver.cpp
index 26a9554b9..dc94f11f2 100644
--- a/radio/src/targets/taranis/extmodule_driver.cpp
+++ b/radio/src/targets/taranis/extmodule_driver.cpp
@@ -147,12 +147,14 @@ void extmoduleInvertedSerialStart(uint32_t baudrate)
   USART_Init(EXTMODULE_USART, &USART_InitStructure);
   USART_Cmd(EXTMODULE_USART, ENABLE);
 
+  extmoduleFifo.clear();
+
   USART_ITConfig(INTMODULE_USART, USART_IT_RXNE, ENABLE);
   NVIC_SetPriority(EXTMODULE_USART_IRQn, 6);
   NVIC_EnableIRQ(EXTMODULE_USART_IRQn);
 }
 
-void extmoduleSendSerialBuffer(const uint8_t * data, uint8_t size)
+void extmoduleSendBuffer(const uint8_t * data, uint8_t size)
 {
   DMA_InitTypeDef DMA_InitStructure;
   DMA_DeInit(EXTMODULE_USART_DMA_STREAM);
@@ -295,7 +297,7 @@ void extmoduleSendNextFrame()
 #if defined(PXX2)
   else if (moduleSettings[EXTERNAL_MODULE].protocol == PROTOCOL_CHANNELS_PXX2) {
     #if defined(EXTMODULE_USART)
-      extmoduleSendSerialBuffer(extmodulePulsesData.pxx2.getData(), extmodulePulsesData.pxx2.getSize());
+      extmoduleSendBuffer(extmodulePulsesData.pxx2.getData(), extmodulePulsesData.pxx2.getSize());
     #else
       sportSendBuffer(extmodulePulsesData.pxx2.getData(), extmodulePulsesData.pxx2.getSize());
     #endif
diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h
index 3cede1e72..e25fcfdd8 100644
--- a/radio/src/targets/taranis/hal.h
+++ b/radio/src/targets/taranis/hal.h
@@ -1284,7 +1284,7 @@
   #define BT_RCC_AHB1Periph            (RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_GPIOE | RCC_AHB1Periph_GPIOG)
   #define BT_RCC_APB1Periph            0
   #define BT_RCC_APB2Periph            RCC_APB2Periph_USART6
-  #define BT_GPIO_TXRX                 GPIOG
+  #define BT_USART_GPIO                GPIOG
   #define BT_TX_GPIO_PIN               GPIO_Pin_14 // PG.14
   #define BT_RX_GPIO_PIN               GPIO_Pin_9  // PG.09
   #define BT_EN_GPIO                   GPIOD
@@ -1306,7 +1306,7 @@
   #else
     #define BT_EN_GPIO_PIN              GPIO_Pin_12 // PE.12
   #endif
-  #define BT_GPIO_TXRX                  GPIOB
+  #define BT_USART_GPIO                 GPIOB
   #define BT_TX_GPIO_PIN                GPIO_Pin_10 // PB.10
   #define BT_RX_GPIO_PIN                GPIO_Pin_11 // PB.11
   #define BT_TX_GPIO_PinSource          GPIO_PinSource10
diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp
index 053d907d9..8098a8a74 100644
--- a/radio/src/telemetry/frsky.cpp
+++ b/radio/src/telemetry/frsky.cpp
@@ -92,8 +92,6 @@ void processFrskyPXX2Data(uint8_t data)
 
 void processFrskyTelemetryData(uint8_t data)
 {
-  static uint8_t dataState = STATE_DATA_IDLE;
-
 #if defined(PCBSKY9X) && defined(BLUETOOTH)
   // TODO if (g_model.bt_telemetry)
   btPushByte(data);
@@ -111,6 +109,19 @@ void processFrskyTelemetryData(uint8_t data)
   }
 #endif
 
+  if (pushFrskyTelemetryData(data)) {
+    if (IS_FRSKY_SPORT_PROTOCOL()) {
+      sportProcessTelemetryPacket(telemetryRxBuffer);
+    } else {
+      frskyDProcessPacket(telemetryRxBuffer);
+    }
+  }
+}
+
+bool pushFrskyTelemetryData(uint8_t data)
+{
+  static uint8_t dataState = STATE_DATA_IDLE;
+
   switch (dataState) {
     case STATE_DATA_START:
       if (data == START_STOP) {
@@ -138,8 +149,8 @@ void processFrskyTelemetryData(uint8_t data)
         }
         else {
           // end of frame detected
-          frskyDProcessPacket(telemetryRxBuffer);
           dataState = STATE_DATA_IDLE;
+          return true;
         }
         break;
       }
@@ -166,9 +177,13 @@ void processFrskyTelemetryData(uint8_t data)
 
 #if defined(TELEMETRY_FRSKY_SPORT)
   if (IS_FRSKY_SPORT_PROTOCOL() && telemetryRxBufferCount >= FRSKY_SPORT_PACKET_SIZE) {
-    sportProcessTelemetryPacket(telemetryRxBuffer);
+    // end of frame detected
     dataState = STATE_DATA_IDLE;
+    return true;
   }
 #endif
+
+  return false;
 }
 
+
diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h
index 7e2068830..a30ec7ac0 100644
--- a/radio/src/telemetry/frsky.h
+++ b/radio/src/telemetry/frsky.h
@@ -23,7 +23,7 @@
 
 #include "../definitions.h"
 #include "telemetry_holders.h"
-#include "../io/pxx2.h"
+#include "io/frsky_pxx2.h"
 
 // Receive buffer state machine state enum
 enum FrSkyDataState {
@@ -313,6 +313,7 @@ struct TelemetryData {
 
 extern TelemetryData telemetryData;
 
+bool pushFrskyTelemetryData(uint8_t data); // returns true when end of frame detected
 void processFrskyTelemetryData(uint8_t data);
 void processFrskyPXX2Data(uint8_t data);