diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h
index 6ace554bf..e330aafec 100644
--- a/radio/src/datastructs.h
+++ b/radio/src/datastructs.h
@@ -463,8 +463,18 @@ PACK(struct ModuleData {
 #if defined(AFHDS2)
     NOBACKUP(struct {
       uint8_t rx_id[4];
-      uint8_t mode;
+      uint8_t mode:3;
+      uint8_t rfPower:1;
+      uint8_t reserved:4;
       uint8_t rx_freq[2];
+
+      void setDefault() {
+        rx_id[0] = rx_id[1] = rx_id[2] = rx_id[3] = 0;
+        mode = 0;
+        rfPower = 0;
+        rx_freq[0] = 50;
+        rx_freq[1] = 0;
+      }
     } flysky);
 #endif
 #if defined(AFHDS3)
diff --git a/radio/src/gui/colorlcd/model_setup.cpp b/radio/src/gui/colorlcd/model_setup.cpp
index 6347755c5..ddfc5006c 100644
--- a/radio/src/gui/colorlcd/model_setup.cpp
+++ b/radio/src/gui/colorlcd/model_setup.cpp
@@ -21,6 +21,7 @@
 #include "model_setup.h"
 #include "multi_rfprotos.h"
 #include "io/multi_protolist.h"
+#include "pulses/flysky.h"
 
 #include "opentx.h"
 #include "libopenui.h"
@@ -803,45 +804,130 @@ class ModuleWindow : public FormGroup {
 
       // Module parameters
       if (isModuleXJT(moduleIdx)) {
-        auto xjtChoice = new Choice(this, grid.getFieldSlot(2, 1), STR_XJT_ACCST_RF_PROTOCOLS, MODULE_SUBTYPE_PXX1_OFF, MODULE_SUBTYPE_PXX1_ACCST_LR12,
-                                    GET_SET_DEFAULT(g_model.moduleData[moduleIdx].rfProtocol));
-        xjtChoice->setAvailableHandler([](int index) {
-          return index != MODULE_SUBTYPE_PXX1_OFF;
-        });
-      }
-      else if (isModuleDSM2(moduleIdx)) {
-        new Choice(this, grid.getFieldSlot(2, 1), STR_DSM_PROTOCOLS, DSM2_PROTO_LP45, DSM2_PROTO_DSMX,
+        auto xjtChoice = new Choice(
+            this, grid.getFieldSlot(2, 1), STR_XJT_ACCST_RF_PROTOCOLS,
+            MODULE_SUBTYPE_PXX1_OFF, MODULE_SUBTYPE_PXX1_ACCST_LR12,
+            GET_SET_DEFAULT(g_model.moduleData[moduleIdx].rfProtocol));
+        xjtChoice->setAvailableHandler(
+            [](int index) { return index != MODULE_SUBTYPE_PXX1_OFF; });
+      } else if (isModuleDSM2(moduleIdx)) {
+        new Choice(this, grid.getFieldSlot(2, 1), STR_DSM_PROTOCOLS,
+                   DSM2_PROTO_LP45, DSM2_PROTO_DSMX,
                    GET_SET_DEFAULT(g_model.moduleData[moduleIdx].rfProtocol));
-      }
-      else if (isModuleR9M(moduleIdx)) {
-        rfChoice = new Choice(this, grid.getFieldSlot(2, 1), STR_R9M_REGION, MODULE_SUBTYPE_R9M_FCC, MODULE_SUBTYPE_R9M_EU,
-                              GET_DEFAULT(g_model.moduleData[moduleIdx].subType),
-                              [=](int32_t newValue) {
-                                  g_model.moduleData[moduleIdx].subType = newValue;
-                                  SET_DIRTY();
-                                  update();
-                                  rfChoice->setFocus(SET_FOCUS_DEFAULT);
-                              });
-      }
-      else if (isModulePXX2(moduleIdx)) {
-        rfChoice = new Choice(this, grid.getFieldSlot(2, 1), STR_ISRM_RF_PROTOCOLS, 0, MODULE_SUBTYPE_ISRM_PXX2_ACCST_LR12,
-                              GET_DEFAULT(g_model.moduleData[moduleIdx].subType),
-                              [=](int32_t newValue) {
-                                  g_model.moduleData[moduleIdx].subType = newValue;
-                                  SET_DIRTY();
-                                  update();
-                                  rfChoice->setFocus(SET_FOCUS_DEFAULT);
-                              });
-      }
-      else if (isModuleFlySky(moduleIdx)) {
-        rfChoice = new Choice(this, grid.getFieldSlot(2, 1), STR_FLYSKY_PROTOCOLS, 0, MODULE_SUBTYPE_ISRM_PXX2_ACCST_LR12,
-                              GET_DEFAULT(g_model.moduleData[moduleIdx].subType),
-                              [=](int32_t newValue) {
-                                  g_model.moduleData[moduleIdx].subType = newValue;
-                                  SET_DIRTY();
-                                  update();
-                                  rfChoice->setFocus(SET_FOCUS_DEFAULT);
-                              });
+      } else if (isModuleR9M(moduleIdx)) {
+        rfChoice =
+            new Choice(this, grid.getFieldSlot(2, 1), STR_R9M_REGION,
+                       MODULE_SUBTYPE_R9M_FCC, MODULE_SUBTYPE_R9M_EU,
+                       GET_DEFAULT(g_model.moduleData[moduleIdx].subType),
+                       [=](int32_t newValue) {
+                         g_model.moduleData[moduleIdx].subType = newValue;
+                         SET_DIRTY();
+                         update();
+                         rfChoice->setFocus(SET_FOCUS_DEFAULT);
+                       });
+      } else if (isModulePXX2(moduleIdx)) {
+        rfChoice =
+            new Choice(this, grid.getFieldSlot(2, 1), STR_ISRM_RF_PROTOCOLS, 0,
+                       MODULE_SUBTYPE_ISRM_PXX2_ACCST_LR12,
+                       GET_DEFAULT(g_model.moduleData[moduleIdx].subType),
+                       [=](int32_t newValue) {
+                         g_model.moduleData[moduleIdx].subType = newValue;
+                         SET_DIRTY();
+                         update();
+                         rfChoice->setFocus(SET_FOCUS_DEFAULT);
+                       });
+      } else if (isModuleFlySky(moduleIdx)) {
+        rfChoice =
+            new Choice(this, grid.getFieldSlot(2, 1),
+                       STR_FLYSKY_PROTOCOLS, 0, FLYSKY_SUBTYPE_AFHDS2A,
+                       GET_DEFAULT(g_model.moduleData[moduleIdx].subType),
+                       [=](int32_t newValue) {
+                         g_model.moduleData[moduleIdx].subType = newValue;
+                         SET_DIRTY();
+                         update();
+                         rfChoice->setFocus(SET_FOCUS_DEFAULT);
+                       });
+#if defined(PCBNV14)
+        if (moduleIdx == INTERNAL_MODULE)
+          rfChoice->setAvailableHandler(
+              [](int v) { return v == FLYSKY_SUBTYPE_AFHDS2A; });
+#endif
+
+        // TODO:
+        // - options (currently taken from 'subType'...):
+        //   - PWM / PPM
+        //   - SBUS / iBUS
+        
+#if defined(AFHDS3)
+        if (isModuleAFHDS3(moduleIdx)) {
+
+          // TYPE
+          grid.nextLine();
+          new StaticText(this, grid.getLabelSlot(true), STR_TYPE, 0,
+                         COLOR_THEME_PRIMARY1);
+
+          // This is chosen when binding (menu? see stdlcd/model_setup_afhds3.cpp)
+          new StaticText(this, grid.getFieldSlot(),
+                         g_model.moduleData[moduleIdx].afhds3.telemetry
+                             ? STR_AFHDS3_ONE_TO_ONE_TELEMETRY
+                             : TR_AFHDS3_ONE_TO_MANY,
+                         0, COLOR_THEME_PRIMARY1);
+
+          // Status
+          grid.nextLine();
+          new StaticText(this, grid.getLabelSlot(true), STR_MODULE_STATUS, 0,
+                         COLOR_THEME_PRIMARY1);
+          new DynamicText(this, grid.getFieldSlot(), [=] {
+            char msg[64] = "";
+            getModuleStatusString(moduleIdx, msg);
+            return std::string(msg);
+          });
+
+          // Power source
+          grid.nextLine();
+          new StaticText(this, grid.getLabelSlot(true), STR_AFHDS3_POWER_SOURCE,
+                         0, COLOR_THEME_PRIMARY1);
+          new DynamicText(this, grid.getFieldSlot(), [=] {
+            char msg[64] = "";
+            getModuleSyncStatusString(moduleIdx, msg);
+            return std::string(msg);
+          });
+
+          // RX Freq
+          grid.nextLine();
+          new StaticText(this, grid.getLabelSlot(true), STR_AFHDS3_RX_FREQ, 0,
+                         COLOR_THEME_PRIMARY1);
+          auto edit = new NumberEdit(
+              this, grid.getFieldSlot(2, 0), MIN_FREQ, MAX_FREQ,
+              GET_DEFAULT(g_model.moduleData[moduleIdx].afhds3.rxFreq()));
+          edit->setSetValueHandler([=](int32_t newValue) {
+            g_model.moduleData[moduleIdx].afhds3.setRxFreq(
+                (uint16_t)newValue);
+          });
+          edit->setSuffix(STR_HZ);
+
+          // Module actual power
+          grid.nextLine();
+          new StaticText(this, grid.getLabelSlot(true), STR_AFHDS3_ACTUAL_POWER,
+                         0, COLOR_THEME_PRIMARY1);
+          new DynamicText(this, grid.getFieldSlot(), [=] {
+            char msg[64] = "";
+            getStringAtIndex(msg, STR_AFHDS3_POWERS,
+                             actualAfhdsRunPower(moduleIdx));
+            return std::string(msg);
+          });
+
+          // Module power
+          grid.nextLine();
+          new StaticText(this, grid.getLabelSlot(true), STR_RF_POWER, 0,
+                         COLOR_THEME_PRIMARY1);
+          new Choice(
+              this, grid.getFieldSlot(2, 0), STR_AFHDS3_POWERS,
+              afhds3::RUN_POWER::RUN_POWER_FIRST,
+              afhds3::RUN_POWER::RUN_POWER_LAST,
+              GET_SET_DEFAULT(g_model.moduleData[moduleIdx].afhds3.runPower));
+        }
+#endif
       }
 #if defined(MULTIMODULE)
       else if (isModuleMultimodule(moduleIdx)) {
@@ -1030,62 +1116,6 @@ class ModuleWindow : public FormGroup {
                        GET_SET_DEFAULT(g_model.moduleData[moduleIdx].multi.disableMapping));
         }
       }
-#endif
-#if defined(AFHDS3)
-      else if (isModuleAFHDS3(moduleIdx)) {
-        rfChoice = new Choice(this, grid.getFieldSlot(2, 1), STR_AFHDS3_PROTOCOLS, AFHDS_SUBTYPE_FIRST, AFHDS_SUBTYPE_LAST,
-                              GET_SET_DEFAULT(g_model.moduleData[moduleIdx].subType));
-
-        // TYPE
-        grid.nextLine();
-        new StaticText(this, grid.getLabelSlot(true), STR_TYPE, 0, COLOR_THEME_PRIMARY1);
-        new StaticText(this, grid.getFieldSlot(),
-                       g_model.moduleData[EXTERNAL_MODULE].afhds3.telemetry ? STR_AFHDS3_ONE_TO_ONE_TELEMETRY : TR_AFHDS3_ONE_TO_MANY,
-                       0, COLOR_THEME_PRIMARY1);
-
-        // Status
-        grid.nextLine();
-        new StaticText(this, grid.getLabelSlot(true), STR_MODULE_STATUS, 0, COLOR_THEME_PRIMARY1);
-        new DynamicText(this, grid.getFieldSlot(), [=] {
-            char msg[64] = "";
-            getModuleStatusString(moduleIdx, msg);
-            return std::string(msg);
-        });
-
-
-        // Power source
-        grid.nextLine();
-        new StaticText(this, grid.getLabelSlot(true), STR_AFHDS3_POWER_SOURCE, 0, COLOR_THEME_PRIMARY1);
-        new DynamicText(this, grid.getFieldSlot(), [=] {
-            char msg[64] = "";
-            getModuleSyncStatusString(moduleIdx, msg);
-            return std::string(msg);
-        });
-
-        // RX Freq
-        grid.nextLine();
-        new StaticText(this, grid.getLabelSlot(true), STR_AFHDS3_RX_FREQ, 0, COLOR_THEME_PRIMARY1);
-        auto edit = new NumberEdit(this, grid.getFieldSlot(2,0), MIN_FREQ, MAX_FREQ, GET_DEFAULT(g_model.moduleData[moduleIdx].afhds3.rxFreq()));
-        edit->setSetValueHandler([=](int32_t newValue) {
-            g_model.moduleData[EXTERNAL_MODULE].afhds3.setRxFreq((uint16_t)newValue);
-        });
-        edit->setSuffix(STR_HZ);
-
-        // Module actual power
-        grid.nextLine();
-        new StaticText(this, grid.getLabelSlot(true), STR_AFHDS3_ACTUAL_POWER, 0, COLOR_THEME_PRIMARY1);
-        new DynamicText(this, grid.getFieldSlot(), [=] {
-            char msg[64] = "";
-            getStringAtIndex(msg, STR_AFHDS3_POWERS, actualAfhdsRunPower(moduleIdx));
-            return std::string(msg);
-        });
-
-        // Module power
-        grid.nextLine();
-        new StaticText(this, grid.getLabelSlot(true), STR_RF_POWER, 0, COLOR_THEME_PRIMARY1);
-        new Choice(this, grid.getFieldSlot(2, 0), STR_AFHDS3_POWERS, afhds3::RUN_POWER::RUN_POWER_FIRST, afhds3::RUN_POWER::RUN_POWER_LAST,
-                    GET_SET_DEFAULT(g_model.moduleData[moduleIdx].afhds3.runPower));
-      }
 #endif
       grid.nextLine();
 
@@ -1126,7 +1156,9 @@ class ModuleWindow : public FormGroup {
       // Module parameters
 
       // Bind and Range buttons
-      if (!isModuleRFAccess(moduleIdx) && (isModuleModelIndexAvailable(moduleIdx)|| isModuleBindRangeAvailable(moduleIdx))) {
+      if (!isModuleRFAccess(moduleIdx) &&
+          (isModuleModelIndexAvailable(moduleIdx) ||
+           isModuleBindRangeAvailable(moduleIdx))) {
         uint8_t thirdColumn = 0;
         new StaticText(this, grid.getLabelSlot(true), STR_RECEIVER, 0, COLOR_THEME_PRIMARY1);
 
@@ -1155,6 +1187,7 @@ class ModuleWindow : public FormGroup {
               }
               if (moduleState[moduleIdx].mode == MODULE_MODE_BIND) {
                 moduleState[moduleIdx].mode = MODULE_MODE_NORMAL;
+                if (isModuleFlySky(moduleIdx)) resetPulsesAFHDS2();
                 return 0;
               } else {
 #if defined(MULTIMODULE)
@@ -1163,6 +1196,10 @@ class ModuleWindow : public FormGroup {
                 }
 #endif
                 moduleState[moduleIdx].mode = MODULE_MODE_BIND;
+                if (isModuleFlySky(moduleIdx)) {
+                  resetPulsesAFHDS2();
+                  setFlyskyState(1/*STATE_INIT*/);
+                }
                 return 1;
               }
               return 0;
@@ -1172,7 +1209,8 @@ class ModuleWindow : public FormGroup {
                 bindButton->check(false);
               }
 #if defined(MULTIMODULE)
-              if (isModuleMultimodule(moduleIdx) && getMultiBindStatus(moduleIdx) == MULTI_BIND_FINISHED) {
+              if (isModuleMultimodule(moduleIdx) &&
+                  getMultiBindStatus(moduleIdx) == MULTI_BIND_FINISHED) {
                 setMultiBindStatus(moduleIdx, MULTI_BIND_NONE);
                 moduleState[moduleIdx].mode = MODULE_MODE_NORMAL;
                 bindButton->check(false);
diff --git a/radio/src/gui/colorlcd/radio_version.cpp b/radio/src/gui/colorlcd/radio_version.cpp
index b207816a8..94713e69a 100644
--- a/radio/src/gui/colorlcd/radio_version.cpp
+++ b/radio/src/gui/colorlcd/radio_version.cpp
@@ -243,6 +243,8 @@ void RadioVersionPage::build(FormWindow * window)
   auto options = new OptionsText(window, grid.getFieldSlot(1,0));
   grid.nextLine(options->height() + 4);
 
+  //TODO: NV14 internal module version (NV14internalModuleFwVersion)
+
 #if defined(PXX2)
   // Module and receivers versions
   auto moduleVersions = new TextButton(window, grid.getLineSlot(), STR_MODULES_RX_VERSION);
diff --git a/radio/src/pulses/afhds2.h b/radio/src/pulses/afhds2.h
index e6bab681b..ff421ef5a 100644
--- a/radio/src/pulses/afhds2.h
+++ b/radio/src/pulses/afhds2.h
@@ -27,6 +27,8 @@
 #include <map>
 #include <list>
 
+#define AFHDS2_PERIOD (2 * 1000) /* us */
+
 struct FlySkySerialPulsesData {
   uint8_t  pulses[64];
   uint8_t  * ptr;
diff --git a/radio/src/pulses/flysky.cpp b/radio/src/pulses/flysky.cpp
new file mode 100644
index 000000000..3ab95f609
--- /dev/null
+++ b/radio/src/pulses/flysky.cpp
@@ -0,0 +1,741 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Dedicate for FlySky NV14 board.
+ *
+ * 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.
+ */
+
+#include "opentx.h"
+
+#include "flysky.h"
+#include "telemetry/flysky_nv14.h"
+
+#define IS_VALID_COMMAND_ID(id) ((id) < CMD_LAST)
+
+#ifndef custom_log
+#define custom_log
+#endif
+
+// 0:OFF, 1:RF frame only, 2:TX frame only, 3:Both RF and TX frame
+enum DEBUG_RF_FRAME_PRINT_E {
+  FRAME_PRINT_OFF = 0,
+  RF_FRAME_ONLY = 1,
+  TX_FRAME_ONLY = 2,
+  BOTH_FRAME_PRINT = 3
+};
+
+#define DEBUG_RF_FRAME_PRINT FRAME_PRINT_OFF
+#define FLYSKY_MODULE_TIMEOUT 155             /* ms */
+#define FLYSKY_PERIOD 4                       /*ms*/
+#define NUM_OF_NV14_CHANNELS (14)
+#define VALID_CH_DATA(v) ((v) > 900 && (v) < 2100)
+#define FAILSAVE_SEND_COUNTER_MAX (400)
+
+#define gRomData g_model.moduleData[INTERNAL_MODULE].flysky
+#define SET_DIRTY() storageDirty(EE_MODEL)
+
+enum FlySkyBindState_E {
+  BIND_LOW_POWER,
+  BIND_NORMAL_POWER,
+  BIND_EXIT,
+};
+
+enum FlySkyRxPulse_E { FLYSKY_PWM, FLYSKY_PPM };
+
+enum FlySkyRxPort_E { FLYSKY_IBUS, FLYSKY_SBUS };
+
+enum FlySkyFirmwareType_E { FLYSKY_RX_FIRMWARE, FLYSKY_RF_FIRMWARE };
+enum FlySkyChannelDataType_E {
+  FLYSKY_CHANNEL_DATA_NORMAL,
+  FLYSKY_CHANNEL_DATA_FAILSAFE
+};
+
+enum FlySkyPulseModeValue_E { PWM_IBUS, PWM_SBUS, PPM_IBUS, PPM_SBUS };
+
+enum FlySkyPulseProtocol_E {
+  FLYSKY_AFHDS1,
+  FLYSKY_AFHDS2,
+  FLYSKY_AFHDS2A,
+};
+
+enum FlySkyModuleState_E {
+  STATE_SET_TX_POWER = 0,
+  STATE_INIT = 1,
+  STATE_GET_FW_VERSION_INIT = 2,
+  STATE_BIND = 3,
+  STATE_SET_RECEIVER_ID = 4,
+  STATE_SET_RX_PWM_PPM = 5,
+  STATE_SET_RX_IBUS_SBUS = 6,
+  STATE_SET_RX_FREQUENCY = 7,
+  STATE_UPDATE_RF_FIRMWARE = 8,
+  STATE_UPDATE_RX_FIRMWARE = 9,
+  STATE_UPDATE_HALL_FIRMWARE = 10,
+  STATE_UPDATE_RF_PROTOCOL = 11,
+  STATE_GET_RECEIVER_CONFIG = 12,
+  STATE_GET_RX_VERSION_INFO = 13,
+  STATE_GET_RF_VERSION_INFO = 14,
+  STATE_SET_RANGE_TEST = 15,
+  STATE_RANGE_TEST_RUNNING = 16,
+  STATE_IDLE = 17,
+  STATE_DISCONNECT = 18,
+  STATE_SEND_CHANNELS = 19,
+};
+
+#define AfhdsPwmMode (gRomData.mode < 2 ? FLYSKY_PWM : FLYSKY_PPM)
+#define AfhdsIbusMode (gRomData.mode & 1 ? FLYSKY_SBUS : FLYSKY_IBUS)
+
+struct rf_info_t {
+  uint8_t bind_power;
+  uint8_t protocol;
+  // 0: normal, COMMAND_ID0C_UPDATE_RF_FIRMWARE or CMD_UPDATE_FIRMWARE_END
+  uint8_t fw_state;
+};
+
+// static STRUCT_HALL rxBuffer = {0};
+static uint32_t rfRxCount = 0;
+static uint8_t lastState = STATE_IDLE;
+static uint32_t set_loop_cnt = 0;
+
+uint32_t NV14internalModuleFwVersion;
+
+static rf_info_t rf_info = {
+    .bind_power = BIND_LOW_POWER,
+    .protocol = FLYSKY_AFHDS2A,  // never updated currently
+    .fw_state = 0,
+};
+
+typedef struct __attribute__((packed)) rxParams {
+  uint8_t outputMode;
+  uint8_t outputSerialMode;
+  uint16_t outputFreq;
+} rxParams;
+
+typedef struct __attribute__((packed)) fwVersion {
+  uint8_t target;
+  uint32_t version;
+} fwVersion;
+
+struct __attribute__((packed)) afhds2Resp {
+  uint8_t startByte;
+  uint8_t frame_number;
+  uint8_t frame_type;
+  uint8_t command_id;
+  uint8_t value;
+};
+
+uint8_t emptyRxID[] = {0, 0, 0, 0};
+
+void setFlyskyState(uint8_t state);
+void onFlySkyBindReceiver();
+void onFlySkyModuleSetPower(bool isPowerOn);
+void afhds2Command(uint8_t type, uint8_t cmd);
+void intmoduleSendBufferDMA(uint8_t* data, uint16_t size);
+void onFlySkyGetVersionInfoStart(uint8_t isRfTransfer);
+// void usbDownloadTransmit(uint8_t *buffer, uint32_t size);
+
+// extern uint32_t NV14internalModuleFwVersion;
+
+#define END 0xC0
+#define ESC 0xDB
+#define ESC_END 0xDC
+#define ESC_ESC 0xDD
+
+#define FRAME_TYPE_REQUEST_ACK 0x01
+#define FRAME_TYPE_REQUEST_NACK 0x02
+#define FRAME_TYPE_ANSWER 0x10
+
+enum FlySkyModuleCommandID {
+  CMD_NONE=0,
+  CMD_RF_INIT=1,
+  CMD_BIND=2,
+  CMD_SET_RECEIVER_ID=3,
+  CMD_RF_GET_CONFIG=4,
+  CMD_SEND_CHANNEL_DATA=5,
+  CMD_RX_SENSOR_DATA=6,
+  CMD_SET_RX_PWM_PPM=7,
+  CMD_SET_RX_SERVO_FREQ=8,
+  CMD_GET_VERSION_INFO=9,
+  CMD_SET_RX_IBUS_SBUS=0xA,
+  CMD_SET_RX_IBUS_SERVO_EXT=0xB,
+  CMD_UPDATE_RF_FIRMWARE = 0x0C,
+  CMD_SET_TX_POWER = 0x0D,
+  CMD_SET_RF_PROTOCOL,
+  CMD_TEST_RANGE,
+  CMD_TEST_RF_RESERVED,
+  CMD_UPDATE_RX_FIRMWARE = 0x20,
+  CMD_LAST
+};
+
+bool isFlySkyUsbDownload(void) { return rf_info.fw_state != 0; }
+
+// void usbSetFrameTransmit(uint8_t packetID, uint8_t *dataBuf, uint32_t nBytes)
+// {
+//     // send to host via usb
+//     uint8_t *pt = (uint8_t*)&rxBuffer;
+//    // rxBuffer.head = HALL_PROTOLO_HEAD;
+//     rxBuffer.hallID.hall_Id.packetID = packetID;//0x08;
+//     rxBuffer.hallID.hall_Id.senderID = 0x03;
+//     rxBuffer.hallID.hall_Id.receiverID = 0x02;
+
+//     if ( packetID == 0x08 ) {
+//       uint8_t fwVerision[40];
+//       for(uint32_t idx = 40; idx > 0; idx--)
+//       {
+//           if ( idx <= nBytes ) {
+//               fwVerision[idx-1] = dataBuf[idx-1];
+//           }
+//           else fwVerision[idx-1] = 0;
+//       }
+//       dataBuf = fwVerision;
+//       nBytes = 40;
+//     }
+
+//     rxBuffer.length = nBytes;
+
+//     TRACE_NOCRLF("\r\nToUSB: 55 %02X %02X ", rxBuffer.hallID.ID, nBytes);
+//     for ( uint32_t idx = 0; idx < nBytes; idx++ )
+//     {
+//         rxBuffer.data[idx] = dataBuf[idx];
+//         TRACE_NOCRLF("%02X ", rxBuffer.data[idx]);
+//     }
+// #if !defined(SIMU)
+//     uint16_t checkSum = calc_crc16(pt, rxBuffer.length+3);
+//     TRACE(" CRC:%04X;", checkSum);
+
+//     pt[rxBuffer.length + 3] = checkSum & 0xFF;
+//     pt[rxBuffer.length + 4] = checkSum >> 8;
+
+//     usbDownloadTransmit(pt, rxBuffer.length + 5);
+// #endif
+// }
+
+void onFlySkyModuleSetPower(bool isPowerOn)
+{
+  if (isPowerOn) {
+    INTERNAL_MODULE_ON();
+    resetPulsesAFHDS2();
+  } else {
+    moduleState[INTERNAL_MODULE].mode = MODULE_MODE_NORMAL;
+    INTERNAL_MODULE_OFF();
+  }
+}
+
+void setFlyskyState(uint8_t state) { intmodulePulsesData.flysky.state = state; }
+
+void onFlySkyUsbDownloadStart(uint8_t fw_state) { rf_info.fw_state = fw_state; }
+
+void onFlySkyGetVersionInfoStart(uint8_t isRfTransfer)
+{
+  lastState = intmodulePulsesData.flysky.state;
+  setFlyskyState(isRfTransfer ? STATE_GET_RF_VERSION_INFO
+                              : STATE_GET_RX_VERSION_INFO);
+}
+
+inline void initFlySkyArray()
+{
+  intmodulePulsesData.flysky.ptr = intmodulePulsesData.flysky.pulses;
+  intmodulePulsesData.flysky.crc = 0;
+}
+
+inline void putFlySkyByte(uint8_t byte)
+{
+  if (END == byte) {
+    *intmodulePulsesData.flysky.ptr++ = ESC;
+    *intmodulePulsesData.flysky.ptr++ = ESC_END;
+  } else if (ESC == byte) {
+    *intmodulePulsesData.flysky.ptr++ = ESC;
+    *intmodulePulsesData.flysky.ptr++ = ESC_ESC;
+  } else {
+    *intmodulePulsesData.flysky.ptr++ = byte;
+  }
+}
+
+inline void putFlySkyFrameByte(uint8_t byte)
+{
+  intmodulePulsesData.flysky.crc += byte;
+  putFlySkyByte(byte);
+}
+
+inline void putFlySkyFrameCmd(uint8_t type, uint8_t cmd)
+{
+  intmodulePulsesData.flysky.crc += type + cmd;
+  putFlySkyByte(type);
+  putFlySkyByte(cmd);
+}
+
+inline void putFlySkyFrameBytes(uint8_t* data, int length)
+{
+  for (int i = 0; i < length; i++) {
+    intmodulePulsesData.flysky.crc += data[i];
+    putFlySkyByte(data[i]);
+  }
+}
+
+inline void putFlySkyFrameHeader()
+{
+  initFlySkyArray();
+  *intmodulePulsesData.flysky.ptr++ = END;
+  putFlySkyFrameByte(intmodulePulsesData.flysky.frame_index);
+}
+
+inline void putFlySkyFrameFooter()
+{
+  if (++intmodulePulsesData.flysky.frame_index == 0) {
+    intmodulePulsesData.flysky.frame_index = 1;
+  }
+  putFlySkyByte(intmodulePulsesData.flysky.crc ^ 0xff);
+  *intmodulePulsesData.flysky.ptr++ = END;
+}
+
+void afhds2Command(uint8_t type, uint8_t cmd)
+{
+  putFlySkyFrameHeader();
+  putFlySkyFrameCmd(type, cmd);
+  putFlySkyFrameFooter();
+}
+
+inline void putFlySkySendChannelData()
+{
+  uint16_t pulseValue = 0;
+  uint8_t channels_start = g_model.moduleData[INTERNAL_MODULE].channelsStart;
+  uint8_t channels_last =
+      channels_start + 8 + g_model.moduleData[INTERNAL_MODULE].channelsCount;
+  putFlySkyFrameCmd(FRAME_TYPE_REQUEST_NACK, CMD_SEND_CHANNEL_DATA);
+
+  if (moduleState[INTERNAL_MODULE].counter-- == 0) {
+    moduleState[INTERNAL_MODULE].counter = FAILSAVE_SEND_COUNTER_MAX;
+    putFlySkyFrameByte(FLYSKY_CHANNEL_DATA_FAILSAFE);
+    putFlySkyFrameByte(channels_last - channels_start);
+    for (uint8_t channel = channels_start; channel < channels_last; channel++) {
+      if (g_model.moduleData[INTERNAL_MODULE].failsafeMode == FAILSAFE_CUSTOM) {
+        int16_t failsafeValue = g_model.failsafeChannels[channel];
+        pulseValue =
+            limit<uint16_t>(0, 988 + ((failsafeValue + 1024) / 2), 0xfff);
+      } else if (g_model.moduleData[INTERNAL_MODULE].failsafeMode ==
+                 FAILSAFE_HOLD) {
+        // protocol uses hold by default
+        pulseValue = 0xfff;
+      } else {
+        int16_t failsafeValue =
+            -1024 + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
+        pulseValue =
+            limit<uint16_t>(0, 988 + ((failsafeValue + 1024) / 2), 0xfff);
+      }
+      putFlySkyFrameByte(pulseValue & 0xff);
+      putFlySkyFrameByte(pulseValue >> 8);
+    }
+    if (DEBUG_RF_FRAME_PRINT & RF_FRAME_ONLY) {
+      TRACE("------FAILSAFE------");
+    }
+  } else {
+    putFlySkyFrameByte(FLYSKY_CHANNEL_DATA_NORMAL);
+    putFlySkyFrameByte(channels_last - channels_start);
+    for (uint8_t channel = channels_start; channel < channels_last; channel++) {
+      int channelValue =
+          channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
+      pulseValue = limit<uint16_t>(0, 988 + ((channelValue + 1024) / 2), 0xfff);
+      putFlySkyFrameByte(pulseValue & 0xff);
+      putFlySkyFrameByte(pulseValue >> 8);
+    }
+  }
+}
+
+void putFlySkyUpdateFirmwareStart(uint8_t cmd)
+{
+  putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, cmd);
+}
+
+bool checkFlySkyFrameCrc(const uint8_t* ptr, uint8_t size)
+{
+  uint8_t crc = 0;
+
+  for (uint8_t i = 0; i < size; i++) {
+    crc += ptr[i];
+  }
+
+  if (DEBUG_RF_FRAME_PRINT & RF_FRAME_ONLY) {
+    if (ptr[2] != 0x06 || (set_loop_cnt++ % 50 == 0)) {
+      TRACE_NOCRLF("RF(%0d): C0", INTMODULE_USART_AFHDS2_BAUDRATE);
+      for (int idx = 0; idx <= size; idx++) {
+        TRACE_NOCRLF(" %02X", ptr[idx]);
+      }
+      TRACE_NOCRLF(" C0;" CRLF);
+
+      if ((crc ^ 0xff) != ptr[size]) {
+        TRACE("ErrorCRC %02X especting %02X", crc ^ 0xFF, ptr[size]);
+      }
+    }
+  }
+
+  return (crc ^ 0xff) == ptr[size];
+}
+
+inline void debugFrame(const uint8_t* rxBuffer, uint8_t rxBufferCount)
+{
+  // debug print the content of the packet
+  char buffer[64 * 3];
+  char* pos = buffer;
+  if (rxBufferCount * 3 > sizeof(buffer)) rxBufferCount = 64;
+  for (int i = 0; i < rxBufferCount; i++) {
+    pos += snprintf(pos, buffer + sizeof(buffer) - pos, "%02X ", rxBuffer[i]);
+  }
+  (*pos) = 0;
+  TRACE("count [%d] data: %s", rxBufferCount, buffer);
+}
+
+inline void parseResponse()
+{
+  const afhds2Resp* resp =
+      reinterpret_cast<afhds2Resp*>(intmodulePulsesData.flysky.telemetry);
+  uint8_t dataLen = intmodulePulsesData.flysky.telemetry_index;
+
+  if (resp->startByte != END || dataLen < 2) return;
+
+  dataLen -= 2;
+  if (!checkFlySkyFrameCrc(intmodulePulsesData.flysky.telemetry + 1, dataLen)) {
+    return;
+  }
+
+  if ((moduleState[INTERNAL_MODULE].mode != MODULE_MODE_BIND) &&
+      (resp->frame_type == FRAME_TYPE_ANSWER) &&
+      ((intmodulePulsesData.flysky.frame_index - 1) != resp->frame_number)) {
+    return;
+  } else if (resp->frame_type == FRAME_TYPE_REQUEST_ACK) {
+    intmodulePulsesData.flysky.frame_index = resp->frame_number;
+  }
+
+  debugFrame(&resp->command_id, dataLen - 2);
+  switch (resp->command_id) {
+    default:
+      if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_NORMAL &&
+          intmodulePulsesData.flysky.state >= STATE_IDLE) {
+        setFlyskyState(STATE_SEND_CHANNELS);
+        if (DEBUG_RF_FRAME_PRINT & RF_FRAME_ONLY)
+          TRACE("State back to channel data");
+      }
+      break;
+    case CMD_RF_INIT:
+      if (resp->value == 0x01) {  // action only RF ready
+        if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_BIND)
+          setFlyskyState(STATE_BIND);
+        else
+          setFlyskyState(STATE_GET_FW_VERSION_INIT);  // get version first
+      } else {
+        // Try one more time;
+        resetPulsesAFHDS2();
+        setFlyskyState(STATE_INIT);
+      }
+      break;
+    case CMD_BIND: {
+      if (resp->frame_type != FRAME_TYPE_ANSWER) {
+        setFlyskyState(STATE_IDLE);
+        return;
+      }
+      if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_BIND)
+        moduleState[INTERNAL_MODULE].mode = MODULE_MODE_NORMAL;
+      *((uint32_t*)gRomData.rx_id) =
+          *(reinterpret_cast<const uint32_t*>(&resp->value));
+      g_model.header.modelId[INTERNAL_MODULE] = gRomData.rx_id[3];
+      if (DEBUG_RF_FRAME_PRINT & RF_FRAME_ONLY)
+        TRACE("New Rx ID: %02X %02X %02X %02X", gRomData.rx_id[0],
+              gRomData.rx_id[1], gRomData.rx_id[2], gRomData.rx_id[3]);
+      SET_DIRTY();
+      resetPulsesAFHDS2();
+      setFlyskyState(STATE_INIT);
+      break;
+    }
+    case CMD_RF_GET_CONFIG:
+      setFlyskyState(STATE_GET_RECEIVER_CONFIG);
+      intmodulePulsesData.flysky.timeout = FLYSKY_MODULE_TIMEOUT;
+      break;
+    case CMD_RX_SENSOR_DATA:
+      flySkyNv14ProcessTelemetryPacket(&resp->value, dataLen - 3);
+      if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_NORMAL &&
+          intmodulePulsesData.flysky.state >= STATE_IDLE) {
+        setFlyskyState(STATE_SEND_CHANNELS);
+      }
+      break;
+    case CMD_SET_RECEIVER_ID: {
+      // range check seems to be not working
+      // it disconnects receiver
+      // if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_RANGECHECK) {
+      //  setFlyskyState(STATE_SET_RANGE_TEST);
+      //}
+      // else
+      setFlyskyState(STATE_SEND_CHANNELS);
+      return;
+    }
+    case CMD_TEST_RANGE:
+      if (moduleState[INTERNAL_MODULE].mode != MODULE_MODE_RANGECHECK)
+        resetPulsesAFHDS2();
+      else
+        setFlyskyState(STATE_RANGE_TEST_RUNNING);
+      break;
+    case CMD_SET_TX_POWER:
+      setFlyskyState(STATE_INIT);
+      break;
+    case CMD_SET_RX_PWM_PPM:
+      setFlyskyState(STATE_SET_RX_IBUS_SBUS);
+      break;
+    case CMD_SET_RX_IBUS_SBUS:
+      setFlyskyState(STATE_SET_RX_FREQUENCY);
+      break;
+    case CMD_SET_RX_SERVO_FREQ:
+      setFlyskyState(STATE_SEND_CHANNELS);
+      break;
+    case CMD_UPDATE_RF_FIRMWARE: {
+      rf_info.fw_state = STATE_UPDATE_RF_FIRMWARE;
+      setFlyskyState(STATE_IDLE);
+      break;
+    }
+    case CMD_GET_VERSION_INFO: {
+      if (intmodulePulsesData.flysky.state == STATE_GET_FW_VERSION_INIT) {
+        memcpy(&NV14internalModuleFwVersion, &resp->value + 1,
+               sizeof(NV14internalModuleFwVersion));
+        setFlyskyState(STATE_SET_RECEIVER_ID);
+        break;
+      }
+
+      // if (dataLen > 4) {
+      //   usbSetFrameTransmit(0x08, (uint8_t*) &resp->value + 1, dataLen - 4);
+      // }
+
+      if (lastState == STATE_GET_RF_VERSION_INFO ||
+          lastState == STATE_GET_RX_VERSION_INFO) {
+        lastState = STATE_INIT;
+      }
+      setFlyskyState(lastState);
+      break;
+    }
+  }
+}
+
+bool isrxBufferMsgOK(void)
+{
+  bool isMsgOK = (0 != rfRxCount);
+  rfRxCount = 0;
+  return isMsgOK && isFlySkyUsbDownload();
+}
+
+void processInternalFlySkyTelemetryData(uint8_t byte)
+{
+  // #if !defined(SIMU)
+  //   parseFlyskyData(&rxBuffer, byte);
+  // #endif
+  // if (rxBuffer.valid) {
+  //   rfRxCount++;
+  //   rxBuffer.valid = 0;
+  //   uint8_t *pt = (uint8_t*)&rxBuffer;
+  //   //rxBuffer.head = HALL_PROTOLO_HEAD;
+  //   pt[rxBuffer.length + 3] = rxBuffer.checkSum & 0xFF;
+  //   pt[rxBuffer.length + 4] = rxBuffer.checkSum >> 8;
+
+  //   if((DEBUG_RF_FRAME_PRINT & RF_FRAME_ONLY)) {
+  //       TRACE("RF: %02X %02X %02X ...%04X; CRC:%04X", pt[0], pt[1], pt[2],
+  //       rxBuffer.checkSum, calc_crc16(pt, rxBuffer.length+3));
+  //   }
+
+  //   //FW UPDATE done
+  //   if (0x01 == rxBuffer.length && (0x05 == rxBuffer.data[0] || 0x06 ==
+  //   rxBuffer.data[0])) {
+  //       setFlyskyState(STATE_INIT);
+  //       rf_info.fw_state = 0;
+  //   }
+  //   usbDownloadTransmit(pt, rxBuffer.length + 5);
+  // }
+  if (byte == END && intmodulePulsesData.flysky.telemetry_index > 0) {
+    parseResponse();
+    intmodulePulsesData.flysky.telemetry_index = 0;
+  } else {
+    if (byte == ESC) {
+      intmodulePulsesData.flysky.esc_state = 1;
+    } else {
+      if (intmodulePulsesData.flysky.esc_state) {
+        intmodulePulsesData.flysky.esc_state = 0;
+        if (byte == ESC_END)
+          byte = END;
+        else if (byte == ESC_ESC)
+          byte = ESC;
+      }
+      intmodulePulsesData.flysky
+          .telemetry[intmodulePulsesData.flysky.telemetry_index++] = byte;
+      if (intmodulePulsesData.flysky.telemetry_index >=
+          sizeof(intmodulePulsesData.flysky.telemetry)) {
+        // TODO buffer is full, log an error?
+        intmodulePulsesData.flysky.telemetry_index = 0;
+      }
+    }
+  }
+}
+
+void resetPulsesAFHDS2()
+{
+  NV14internalModuleFwVersion = 0;
+  intmodulePulsesData.flysky.frame_index = 1;
+  setFlyskyState(STATE_SET_TX_POWER);
+  intmodulePulsesData.flysky.timeout = 0;
+  intmodulePulsesData.flysky.esc_state = 0;
+  uint16_t rx_freq = gRomData.rx_freq[0];
+  rx_freq += (gRomData.rx_freq[1] * 256);
+  if (50 > rx_freq || 400 < rx_freq) {
+    gRomData.rx_freq[0] = 50;
+  }
+}
+
+void setupPulsesAFHDS2()
+{
+  putFlySkyFrameHeader();
+  if (intmodulePulsesData.flysky.state == STATE_DISCONNECT) {
+    TRACE("STATE_DISCONNECT");
+    putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_SET_RECEIVER_ID);
+    putFlySkyFrameBytes(emptyRxID, 4);
+  } else if (intmodulePulsesData.flysky.state < STATE_SEND_CHANNELS) {
+    if (++intmodulePulsesData.flysky.timeout >=
+        FLYSKY_MODULE_TIMEOUT / FLYSKY_PERIOD) {
+      intmodulePulsesData.flysky.timeout = 0;
+      switch (intmodulePulsesData.flysky.state) {
+        case STATE_INIT: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_RF_INIT);
+        } break;
+        case STATE_BIND: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_BIND);
+          putFlySkyFrameByte(rf_info.bind_power);
+          uint32_t txID =
+              0;  // internal module firmware does not allow changing TX id
+          putFlySkyFrameBytes((uint8_t*)(&txID), 4);
+        } break;
+        case STATE_SET_RECEIVER_ID: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_SET_RECEIVER_ID);
+          putFlySkyFrameBytes(gRomData.rx_id, 4);
+        } break;
+        case STATE_GET_RECEIVER_CONFIG: {
+          putFlySkyFrameCmd(FRAME_TYPE_ANSWER, CMD_RF_GET_CONFIG);
+          putFlySkyFrameByte(AfhdsPwmMode);   // 00:PWM, 01:PPM
+          putFlySkyFrameByte(AfhdsIbusMode);  // 00:I-BUS, 01:S-BUS
+          putFlySkyFrameByte(
+              gRomData.rx_freq[0] < 50
+                  ? 50
+                  : gRomData.rx_freq[0]);  // receiver servo freq bit[7:0]
+          putFlySkyFrameByte(
+              gRomData.rx_freq[1]);  // receiver servo freq bit[15:8]
+          setFlyskyState(STATE_INIT);
+        } break;
+        case STATE_SET_TX_POWER: {
+          uint8_t power =
+              moduleState[INTERNAL_MODULE].mode == MODULE_MODE_RANGECHECK ? 0
+              : gRomData.rfPower                                          ? 170
+                                                                          : 90;
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_SET_TX_POWER);
+          putFlySkyFrameByte(power);
+        } break;
+        case STATE_SET_RANGE_TEST: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_TEST_RANGE);
+          putFlySkyFrameByte(moduleState[INTERNAL_MODULE].mode ==
+                             MODULE_MODE_RANGECHECK);
+        } break;
+        case STATE_RANGE_TEST_RUNNING: {
+          if (moduleState[INTERNAL_MODULE].mode != MODULE_MODE_RANGECHECK) {
+            // this will send stop command
+            setFlyskyState(STATE_SET_RANGE_TEST);
+          }
+        } break;
+        case STATE_SET_RX_PWM_PPM: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_SET_RX_PWM_PPM);
+          putFlySkyFrameByte(AfhdsPwmMode);  // 00:PWM, 01:PPM
+        } break;
+        case STATE_SET_RX_IBUS_SBUS: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_SET_RX_IBUS_SBUS);
+          putFlySkyFrameByte(AfhdsIbusMode);  // 0x00:I-BUS, 0x01:S-BUS
+        } break;
+        case STATE_SET_RX_FREQUENCY: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_SET_RX_SERVO_FREQ);
+          putFlySkyFrameBytes(gRomData.rx_freq, 2);
+        } break;
+        case STATE_UPDATE_RF_PROTOCOL: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_SET_RF_PROTOCOL);
+          putFlySkyFrameByte(
+              rf_info.protocol);  // 0x00:AFHDS1 0x01:AFHDS2 0x02:AFHDS2A
+        } break;
+        case STATE_UPDATE_RX_FIRMWARE: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_UPDATE_RX_FIRMWARE);
+        } break;
+        case STATE_UPDATE_RF_FIRMWARE: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_UPDATE_RF_FIRMWARE);
+        } break;
+        case STATE_GET_RX_VERSION_INFO: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_GET_VERSION_INFO);
+          putFlySkyFrameByte(FLYSKY_RX_FIRMWARE);
+        } break;
+        case STATE_GET_FW_VERSION_INIT:
+        case STATE_GET_RF_VERSION_INFO: {
+          putFlySkyFrameCmd(FRAME_TYPE_REQUEST_ACK, CMD_GET_VERSION_INFO);
+          putFlySkyFrameByte(FLYSKY_RF_FIRMWARE);
+        } break;
+        case STATE_IDLE:
+          initFlySkyArray();
+          break;
+        default:
+          setFlyskyState(STATE_INIT);
+          initFlySkyArray();
+          if ((DEBUG_RF_FRAME_PRINT & TX_FRAME_ONLY)) {
+            TRACE("State back to INIT\r\n");
+          }
+          return;
+      }
+    } else {
+      initFlySkyArray();
+      return;
+    }
+  } else {
+    if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_BIND)
+      moduleState[INTERNAL_MODULE].mode = MODULE_MODE_NORMAL;
+    putFlySkySendChannelData();
+  }
+
+  putFlySkyFrameFooter();
+
+  if (intmodulePulsesData.flysky.state < STATE_SEND_CHANNELS) {
+    // uint8_t size = intmodulePulsesData.flysky.ptr -
+    // intmodulePulsesData.flysky.pulses;
+    // debugFrame(intmodulePulsesData.flysky.pulses, size);
+  }
+  if ((DEBUG_RF_FRAME_PRINT & TX_FRAME_ONLY)) {
+    /* print each command, except channel data by interval */
+    uint8_t* data = intmodulePulsesData.flysky.pulses;
+    if (data[3] != CMD_SEND_CHANNEL_DATA || (set_loop_cnt++ % 100 == 0)) {
+      uint8_t size = intmodulePulsesData.flysky.ptr - data;
+      TRACE_NOCRLF("TX(State%0d)%0dB:", intmodulePulsesData.flysky.state, size);
+      for (int idx = 0; idx < size; idx++) {
+        TRACE_NOCRLF(" %02X", data[idx]);
+      }
+      TRACE_NOCRLF(";" CRLF);
+    }
+  }
+}
+
+// void usbDownloadTransmit(uint8_t *buffer, uint32_t size)
+// {
+//     if (USB_SERIAL_MODE != getSelectedUsbMode()) return;
+//     buffer[0] = HALL_PROTOLO_HEAD;
+//     for (uint32_t idx = 0; idx < size; idx++)
+//     {
+// #if !defined(SIMU)
+//         usbSerialPutc(buffer[idx]);
+// #endif
+//     }
+// }
diff --git a/radio/src/pulses/flysky.h b/radio/src/pulses/flysky.h
new file mode 100644
index 000000000..0c35130cf
--- /dev/null
+++ b/radio/src/pulses/flysky.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) OpenTX
+ *
+ * Dedicate for FlySky NV14 board.
+ *
+ * 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.
+ */
+
+#pragma once
+
+#define INTMODULE_USART_AFHDS2_BAUDRATE 921600
+
+void setupPulsesAFHDS2();
+void resetPulsesAFHDS2();
+
+void processInternalFlySkyTelemetryData(uint8_t byte);
diff --git a/radio/src/pulses/modules_helpers.h b/radio/src/pulses/modules_helpers.h
index 89c36e4f4..50eba4a49 100644
--- a/radio/src/pulses/modules_helpers.h
+++ b/radio/src/pulses/modules_helpers.h
@@ -529,8 +529,8 @@ inline bool isModuleFailsafeAvailable(uint8_t moduleIdx)
   }
 #endif
 
-#if defined(AFHDS3)
-  if (isModuleAFHDS3(moduleIdx))
+#if defined(AFHDS3) || defined(AFHDS2)
+  if (isModuleFlySky(moduleIdx))
     return true;
 #endif
 
@@ -542,12 +542,16 @@ inline bool isModuleFailsafeAvailable(uint8_t moduleIdx)
 
 inline bool isModuleBindRangeAvailable(uint8_t moduleIdx)
 {
-  return isModulePXX2(moduleIdx) || isModulePXX1(moduleIdx) || isModuleDSM2(moduleIdx) || isModuleMultimodule(moduleIdx) || isModuleAFHDS3(moduleIdx);
+  return isModulePXX2(moduleIdx) || isModulePXX1(moduleIdx) ||
+         isModuleDSM2(moduleIdx) || isModuleMultimodule(moduleIdx) ||
+         isModuleFlySky(moduleIdx);
 }
 
 inline bool isModuleRangeAvailable(uint8_t moduleIdx)
 {
-  return isModuleBindRangeAvailable(moduleIdx) && !IS_RX_MULTI(moduleIdx);
+  return isModuleBindRangeAvailable(moduleIdx)
+    && !IS_RX_MULTI(moduleIdx)
+    && !isModuleAFHDS3(moduleIdx);
 }
 
 constexpr uint8_t MAX_RXNUM = 63;
diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp
index b1e28f7cf..ea6f59d8d 100755
--- a/radio/src/pulses/pulses.cpp
+++ b/radio/src/pulses/pulses.cpp
@@ -19,10 +19,12 @@
  */
 
 #include "opentx.h"
-#include "io/frsky_pxx2.h"
-#include "pulses/pxx2.h"
 #include "mixer_scheduler.h"
+
+#include "io/frsky_pxx2.h"
 #include "io/multi_protolist.h"
+#include "pulses/pxx2.h"
+#include "pulses/flysky.h"
 
 uint8_t s_pulses_paused = 0;
 ModuleState moduleState[NUM_MODULES];
@@ -489,9 +491,11 @@ static void enablePulsesInternalModule(uint8_t protocol)
 
 #if defined(AFHDS2)
     case PROTOCOL_CHANNELS_AFHDS2A:
-/*      extmodulePulsesData.afhds2.init(INTERNAL_MODULE);
-      extmoduleSerialStart();*/
-      mixerSchedulerSetPeriod(INTERNAL_MODULE, 2 * 1000 /* us */);
+      resetPulsesAFHDS2();
+      intmoduleSerialStart(INTMODULE_USART_AFHDS2_BAUDRATE, true,
+                           USART_Parity_No, USART_StopBits_1,
+                           USART_WordLength_8b);
+      mixerSchedulerSetPeriod(INTERNAL_MODULE, AFHDS2_PERIOD);
       break;
 #endif
 
@@ -544,6 +548,19 @@ bool setupPulsesInternalModule(uint8_t protocol)
       return true;
 #endif
 
+#if defined(AFHDS2)
+    case PROTOCOL_CHANNELS_AFHDS2A:
+    { 
+      ModuleSyncStatus& status = getModuleSyncStatus(INTERNAL_MODULE);
+      mixerSchedulerSetPeriod(
+          INTERNAL_MODULE,
+          status.isValid() ? status.getAdjustedRefreshRate() : AFHDS2_PERIOD);
+      status.invalidate();
+      setupPulsesAFHDS2();
+      return true;
+    }
+#endif
+
     default:
       return false;
   }
diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h
index 76d489737..d89cf724c 100644
--- a/radio/src/pulses/pulses.h
+++ b/radio/src/pulses/pulses.h
@@ -28,6 +28,7 @@
 #include "pxx2.h"
 #include "multi.h"
 #include "afhds3.h"
+#include "afhds2.h"
 #include "modules_helpers.h"
 #include "ff.h"
 
@@ -266,6 +267,10 @@ union InternalModulePulsesData {
   Pxx2Pulses pxx2;
 #endif
 
+#if defined(INTMODULE_USART) && defined(AFHDS2)
+  FlySkySerialPulsesData flysky;
+#endif
+
 #if defined(MULTIMODULE) //&& defined(INTMODULE_USART)
   UartMultiPulses multi;
 #endif
diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt
index df77ab664..3cca9e1ed 100644
--- a/radio/src/targets/common/arm/CMakeLists.txt
+++ b/radio/src/targets/common/arm/CMakeLists.txt
@@ -159,9 +159,11 @@ endif()
 if(MULTIMODULE OR AFHDS3)
   set(SRC ${SRC} telemetry/flysky_ibus.cpp)
 endif()
+
 if(AFHDS2)
   add_definitions(-DAFHDS2)
   set(SRC ${SRC} telemetry/flysky_nv14.cpp)
+  set(PULSES_SRC ${PULSES_SRC} flysky.cpp)
 endif()
 
 if(AFHDS3)
diff --git a/radio/src/telemetry/flysky_nv14.cpp b/radio/src/telemetry/flysky_nv14.cpp
index 059abbbaf..ba0f8b920 100644
--- a/radio/src/telemetry/flysky_nv14.cpp
+++ b/radio/src/telemetry/flysky_nv14.cpp
@@ -64,8 +64,8 @@ const FlyskyNv14Sensor Nv14Sensor[]=
     {FLYSKY_SENSOR_TEMP,       0, STR_SENSOR_TEMP1,       UNIT_CELSIUS,       1, 0, 2, true},
     {FLYSKY_SENSOR_EXT_VOLTAGE,0, STR_SENSOR_A3,          UNIT_VOLTS,         2, 0, 2, false},
     {FLYSKY_SENSOR_MOTO_RPM,   0, STR_SENSOR_RPM,         UNIT_RPMS,          0, 0, 2, false},
-    {FLYSKY_SENSOR_PRESURRE,   0, STR_SENSOR_PRES,        UNIT_RAW,           1, 0, 2, false},
-    {FLYSKY_SENSOR_PRESURRE,   1, STR_SENSOR_ALT,         UNIT_METERS,        0, 0, 2, true},
+    {FLYSKY_SENSOR_PRESSURE,   0, STR_SENSOR_PRES,        UNIT_RAW,           1, 0, 2, false},
+    {FLYSKY_SENSOR_PRESSURE,   1, STR_SENSOR_ALT,         UNIT_METERS,        0, 0, 2, true},
     {FLYSKY_SENSOR_GPS,        1, STR_SENSOR_SATELLITES,  UNIT_RAW,           0, 0, 1, false},
     {FLYSKY_SENSOR_GPS,        2, STR_SENSOR_GPS,         UNIT_GPS_LATITUDE,  0, 1, 4, true},
     {FLYSKY_SENSOR_GPS,        3, STR_SENSOR_GPS,         UNIT_GPS_LONGITUDE, 0, 5, 4, true},
@@ -77,25 +77,32 @@ const FlyskyNv14Sensor Nv14Sensor[]=
 
 FlyskyNv14Sensor defaultNv14Sensor = {0, 0, "UNKNOWN", UNIT_RAW, 0, 0, 2, false};
 
-const signed short tAltitude[225]=
-{
-    20558, 20357, 20158, 19962, 19768, 19576, 19387, 19200, 19015,  18831, 18650, 18471, 18294, 18119, 17946, 17774,
-    17604, 17436, 17269, 17105, 16941, 16780, 16619, 16461, 16304,  16148, 15993, 15841, 15689, 15539, 15390, 15242,
-    15096, 14950, 14806, 14664, 14522, 14381, 14242, 14104, 13966,  13830, 13695, 13561, 13428, 13296, 13165, 13035,
-    12906, 12777, 12650, 12524, 12398, 12273, 12150, 12027, 11904,  11783, 11663, 11543, 11424, 11306, 11189, 11072,
-    10956, 10841, 10726, 10613, 10500, 10387, 10276, 10165, 10054,   9945,  9836,  9727,  9620,  9512,  9406,  9300,
-    9195,  9090,  8986,  8882,  8779,  8677,   8575,  8474,  8373,   8273,  8173,  8074,  7975,  7877,  7779,  7682,
-    7585,  7489,  7394,  7298,  7204,  7109,   7015,  6922,  6829,   6737,  6645,  6553,  6462,  6371,  6281,  6191,
-    6102,  6012,  5924,  5836,  5748,  5660,   5573,  5487,  5400,   5314,  5229,  5144,  5059,  4974,  4890,  4807,
-    4723,  4640,  4557,  4475,  4393,  4312,   4230,  4149,  4069,   3988,  3908,  3829,  3749,  3670,  3591,  3513,
-    3435,  3357,  3280,  3202,  3125,  3049,   2972,  2896,  2821,   2745,  2670,  2595,  2520,  2446,  2372,  2298,
-    2224,  2151,  2078,  2005,  1933,   1861,  1789,  1717,  1645,   1574,  1503,  1432,  1362,  1292,  1222,  1152,
-    1082,  1013,   944,   875,   806,   738,    670,   602,   534,    467,   399,   332,   265,   199,   132,    66,
-     0,     -66,  -131,  -197,  -262,  -327,   -392,  -456,  -521,   -585,  -649,  -713,  -776,  -840,  -903,  -966,
-    -1029,-1091, -1154, -1216, -1278, -1340,  -1402, -1463,  -1525, -1586, -1647, -1708, -1769, -1829, -1889, -1950,
-    -2010
+const signed short tAltitude[225] = {
+    20558, 20357, 20158, 19962, 19768, 19576, 19387, 19200, 19015, 18831, 18650,
+    18471, 18294, 18119, 17946, 17774, 17604, 17436, 17269, 17105, 16941, 16780,
+    16619, 16461, 16304, 16148, 15993, 15841, 15689, 15539, 15390, 15242, 15096,
+    14950, 14806, 14664, 14522, 14381, 14242, 14104, 13966, 13830, 13695, 13561,
+    13428, 13296, 13165, 13035, 12906, 12777, 12650, 12524, 12398, 12273, 12150,
+    12027, 11904, 11783, 11663, 11543, 11424, 11306, 11189, 11072, 10956, 10841,
+    10726, 10613, 10500, 10387, 10276, 10165, 10054, 9945,  9836,  9727,  9620,
+    9512,  9406,  9300,  9195,  9090,  8986,  8882,  8779,  8677,  8575,  8474,
+    8373,  8273,  8173,  8074,  7975,  7877,  7779,  7682,  7585,  7489,  7394,
+    7298,  7204,  7109,  7015,  6922,  6829,  6737,  6645,  6553,  6462,  6371,
+    6281,  6191,  6102,  6012,  5924,  5836,  5748,  5660,  5573,  5487,  5400,
+    5314,  5229,  5144,  5059,  4974,  4890,  4807,  4723,  4640,  4557,  4475,
+    4393,  4312,  4230,  4149,  4069,  3988,  3908,  3829,  3749,  3670,  3591,
+    3513,  3435,  3357,  3280,  3202,  3125,  3049,  2972,  2896,  2821,  2745,
+    2670,  2595,  2520,  2446,  2372,  2298,  2224,  2151,  2078,  2005,  1933,
+    1861,  1789,  1717,  1645,  1574,  1503,  1432,  1362,  1292,  1222,  1152,
+    1082,  1013,  944,   875,   806,   738,   670,   602,   534,   467,   399,
+    332,   265,   199,   132,   66,    0,     -66,   -131,  -197,  -262,  -327,
+    -392,  -456,  -521,  -585,  -649,  -713,  -776,  -840,  -903,  -966,  -1029,
+    -1091, -1154, -1216, -1278, -1340, -1402, -1463, -1525, -1586, -1647, -1708,
+    -1769, -1829, -1889, -1950, -2010
 };
-signed short CalculateAltitude( unsigned int Pressure, unsigned int SeaLevelPressure )
+
+signed short CalculateAltitude(unsigned int Pressure,
+                               unsigned int SeaLevelPressure)
 {
   unsigned int Index;
   signed int Altitude1;
@@ -103,91 +110,94 @@ signed short CalculateAltitude( unsigned int Pressure, unsigned int SeaLevelPres
   unsigned int Decimal;
   unsigned long Ratio;
 
-  Ratio = ( ( ( unsigned long long ) Pressure << 16 ) + ( SeaLevelPressure / 2 ) ) / SeaLevelPressure;
-  if( Ratio < ( ( 1 << 16 ) * 250 / 1000 ) )// 0.250 inclusive
-  {
-      Ratio = ( 1 << 16 ) * 250 / 1000;
-  }
-  else if( Ratio > ( 1 << 16 ) * 1125 / 1000 - 1 ) // 1.125 non-inclusive
-  {
-      Ratio = ( 1 << 16 ) * 1125 / 1000 - 1;
+  Ratio = (((unsigned long long)Pressure << 16) + (SeaLevelPressure / 2)) /
+          SeaLevelPressure;
+
+  if (Ratio < ((1 << 16) * 250 / 1000)) {  // 0.250 inclusive
+    Ratio = (1 << 16) * 250 / 1000;
+  } else if (Ratio > (1 << 16) * 1125 / 1000 - 1) {  // 1.125 non-inclusive
+    Ratio = (1 << 16) * 1125 / 1000 - 1;
   }
 
-  Ratio -= ( 1 << 16 ) * 250 / 1000; // from 0.000 (inclusive) to 0.875 (non-inclusive)
+  // from 0.000 (inclusive) to 0.875 (non-inclusive)
+  Ratio -= (1 << 16) * 250 / 1000;
   Index = Ratio >> 8;
-  Decimal = Ratio & ( ( 1 << 8 ) - 1 );
+  Decimal = Ratio & ((1 << 8) - 1);
   Altitude1 = tAltitude[Index];
   Altitude2 = Altitude1 - tAltitude[Index + 1];
-  Altitude1 = Altitude1 - ( Altitude2 * Decimal + ( 1 << 7 ) ) / ( 1 << 8 );
-  if( Altitude1 >= 0 )
-  {
-      return( ( Altitude1 + 1 ) / 2 );
-  }
-  else
-  {
-      return( ( Altitude1 - 1 ) / 2 );
+  Altitude1 = Altitude1 - (Altitude2 * Decimal + (1 << 7)) / (1 << 8);
+  if (Altitude1 >= 0) {
+    return ((Altitude1 + 1) / 2);
+  } else {
+    return ((Altitude1 - 1) / 2);
   }
 }
 
 const FlyskyNv14Sensor* getFlyskyNv14Sensor(uint16_t id, uint8_t subId)
 {
-  for(unsigned index = 0; index < *(&Nv14Sensor + 1) - Nv14Sensor; index++ ){
+  for (unsigned index = 0; index < *(&Nv14Sensor + 1) - Nv14Sensor; index++) {
     if (Nv14Sensor[index].id == id && Nv14Sensor[index].subId == subId) {
       return &Nv14Sensor[index];
     }
   }
-    return &defaultNv14Sensor;
+  return &defaultNv14Sensor;
 }
 
-void flySkyNv14SetDefault(int index, uint8_t id, uint8_t subId, uint8_t instance)
+void flySkyNv14SetDefault(int index, uint8_t id, uint8_t subId,
+                          uint8_t instance)
 {
-    TelemetrySensor & telemetrySensor = g_model.telemetrySensors[index];
-    telemetrySensor.id = id;
-    telemetrySensor.subId = subId;
-    telemetrySensor.instance = instance;
-    const FlyskyNv14Sensor* sensor = getFlyskyNv14Sensor(id, subId);
-    telemetrySensor.init(sensor->name, sensor->unit, sensor->precision);
-    storageDirty(EE_MODEL);
+  TelemetrySensor& telemetrySensor = g_model.telemetrySensors[index];
+  telemetrySensor.id = id;
+  telemetrySensor.subId = subId;
+  telemetrySensor.instance = instance;
+  const FlyskyNv14Sensor* sensor = getFlyskyNv14Sensor(id, subId);
+  telemetrySensor.init(sensor->name, sensor->unit, sensor->precision);
+  storageDirty(EE_MODEL);
 }
 
-
-
-int32_t GetSensorValueFlySkyNv14(const FlyskyNv14Sensor* sensor, const uint8_t * data){
+int32_t GetSensorValueFlySkyNv14(const FlyskyNv14Sensor* sensor,
+                                 const uint8_t* data)
+{
   int32_t value = 0;
-  const nv14SensorData* sensorData = reinterpret_cast<const nv14SensorData*>(data + sensor->offset);
-  if(sensor->bytes == 1) value = sensorData->UINT8;
-  else if(sensor->bytes == 2) value = sensor->issigned ? sensorData->INT16 : sensorData->UINT16;
-  else if(sensor->bytes == 4) value = sensorData->UINT32;
+  const nv14SensorData* sensorData =
+      reinterpret_cast<const nv14SensorData*>(data + sensor->offset);
+  if (sensor->bytes == 1)
+    value = sensorData->UINT8;
+  else if (sensor->bytes == 2)
+    value = sensor->issigned ? sensorData->INT16 : sensorData->UINT16;
+  else if (sensor->bytes == 4)
+    value = sensorData->UINT32;
 
-  if(sensor->id == FLYSKY_SENSOR_RX_RSSI) {
-    if(value < -200) value = -200;
-#if defined (PCBNV14)
-    if(!g_model.rssiAlarms.flysky_telemetry)
+  if (sensor->id == FLYSKY_SENSOR_RX_RSSI) {
+    if (value < -200) value = -200;
+#if defined(PCBNV14)
+    if (!g_model.rssiAlarms.flysky_telemetry)
 #endif
     {
-        value += 200;
-        value /= 2;
+      value += 200;
+      value /= 2;
     }
+    TRACE("RSSI: %d", value);
     telemetryData.rssi.set(value);
   }
-  if(sensor->id == FLYSKY_SENSOR_PRESURRE && sensor->subId != 0){
+  if (sensor->id == FLYSKY_SENSOR_PRESSURE && sensor->subId != 0) {
     value = CalculateAltitude(value, 101325);
-    value = (value+500)/1000;
+    value = (value + 500) / 1000;
   }
   return value;
 }
 
-void flySkyNv14ProcessTelemetryPacket(const uint8_t * ptr, uint8_t sensorID )
+void flySkyNv14ProcessTelemetryPacket(const uint8_t* ptr, uint8_t sensorID)
 {
-  uint8_t instnace = *ptr++;
-  if(sensorID == FLYSKY_SENSOR_RX_VOLTAGE) sensorID = FLYSKY_FIXED_RX_VOLTAGE;
+  uint8_t instance = *ptr++;
+  if (sensorID == FLYSKY_SENSOR_RX_VOLTAGE) sensorID = FLYSKY_FIXED_RX_VOLTAGE;
   for (const FlyskyNv14Sensor sensor : Nv14Sensor) {
-  if (sensor.id == sensorID) {
-  int32_t value = GetSensorValueFlySkyNv14(&sensor, ptr);
-  setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_NV14, sensor.id, sensor.subId, instnace, value, sensor.unit, sensor.precision);
-}
+    if (sensor.id == sensorID) {
+      int32_t value = GetSensorValueFlySkyNv14(&sensor, ptr);
+      setTelemetryValue(PROTOCOL_TELEMETRY_FLYSKY_NV14, sensor.id, sensor.subId,
+                        instance, value, sensor.unit, sensor.precision);
     }
+  }
 
-    telemetryStreaming = TELEMETRY_TIMEOUT10ms;
+  telemetryStreaming = TELEMETRY_TIMEOUT10ms;
 }
-
diff --git a/radio/src/telemetry/flysky_nv14.h b/radio/src/telemetry/flysky_nv14.h
index f338e5e3e..554a8fa49 100644
--- a/radio/src/telemetry/flysky_nv14.h
+++ b/radio/src/telemetry/flysky_nv14.h
@@ -30,7 +30,7 @@ enum FlySkySensorType_E {
   FLYSKY_SENSOR_TEMP,
   FLYSKY_SENSOR_EXT_VOLTAGE,
   FLYSKY_SENSOR_MOTO_RPM,
-  FLYSKY_SENSOR_PRESURRE,
+  FLYSKY_SENSOR_PRESSURE,
   FLYSKY_SENSOR_GPS
 };
 typedef struct FLYSKY_GPS_INFO_S {
diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp
index 3487f5c14..d01fa8d7e 100644
--- a/radio/src/telemetry/telemetry.cpp
+++ b/radio/src/telemetry/telemetry.cpp
@@ -21,6 +21,7 @@
 #include "opentx.h"
 #include "multi.h"
 #include "pulses/afhds3.h"
+#include "pulses/flysky.h"
 #include "mixer_scheduler.h"
 #include "io/multi_protolist.h"
 
@@ -145,6 +146,18 @@ void telemetryWakeup()
 #endif
 #endif
 
+#if defined(PCBNV14)
+  if (//!moduleUpdateActive(INTERNAL_MODULE) &&
+      moduleState[INTERNAL_MODULE].protocol == PROTOCOL_CHANNELS_AFHDS2A &&
+      intmoduleFifo.pop(data)) {
+    LOG_TELEMETRY_WRITE_START();
+    do {
+      processInternalFlySkyTelemetryData(data);
+      LOG_TELEMETRY_WRITE_BYTE(data);
+    } while (intmoduleFifo.pop(data));
+  }
+#endif
+
 #if defined(STM32)
   if (telemetryGetByte(&data)) {
     LOG_TELEMETRY_WRITE_START();
@@ -432,6 +445,11 @@ void ModuleSyncStatus::update(uint16_t newRefreshRate, int16_t newInputLag)
   TRACE("[SYNC] update rate = %dus; lag = %dus",refreshRate,currentLag);
 }
 
+void ModuleSyncStatus::invalidate() {
+  //make invalid after use
+  currentLag = 0;
+}
+
 uint16_t ModuleSyncStatus::getAdjustedRefreshRate()
 {
   int16_t lag = currentLag;
diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h
index 0cecee0c9..beb2c19c6 100644
--- a/radio/src/telemetry/telemetry.h
+++ b/radio/src/telemetry/telemetry.h
@@ -316,6 +316,9 @@ struct ModuleSyncStatus
   // Set feedback from RF module
   void update(uint16_t newRefreshRate, int16_t newInputLag);
 
+  //mark as timeouted
+  void invalidate();
+
   // Get computed settings for scheduler
   uint16_t getAdjustedRefreshRate();