From 240a40e6167455e7d6ea3bfe6c728439696f1c92 Mon Sep 17 00:00:00 2001
From: Petri Mattila <petri.t.j.mattila@gmail.com>
Date: Tue, 23 Jul 2024 11:46:58 +0100
Subject: [PATCH 1/2] Add pinswap option to serial ports

This commit adds CLI options:
  - serialrx_pinswap
  - tlm_pinswap
  - vtx_pinswap
  - esc_sensor_pinswap

Setting the pinswap to TRUE swaps the Tx and Rx pins around,
vs. the selected pins in the resources.

If the resource pins are already swapped, then pinswap will
swap them again, effectively to unswapped.
---
 src/main/cli/settings.c                  | 14 +++++++-
 src/main/drivers/serial.h                | 46 +++++++++++++++---------
 src/main/drivers/serial_softserial.c     | 15 ++++++--
 src/main/drivers/serial_uart.c           |  2 ++
 src/main/drivers/serial_uart_hal.c       | 42 ++++++++++++++++------
 src/main/drivers/serial_uart_impl.h      |  6 +++-
 src/main/drivers/serial_uart_pinconfig.c | 15 ++++----
 src/main/drivers/serial_uart_stdperiph.c | 12 +++++++
 src/main/io/vtx_smartaudio.c             |  8 +++--
 src/main/io/vtx_tramp.c                  |  6 ++--
 src/main/msp/msp.c                       | 17 +++++++++
 src/main/pg/esc_sensor.c                 |  1 +
 src/main/pg/esc_sensor.h                 |  1 +
 src/main/pg/rx.c                         |  1 +
 src/main/pg/rx.h                         |  1 +
 src/main/pg/telemetry.c                  |  1 +
 src/main/pg/telemetry.h                  |  1 +
 src/main/pg/vtx.c                        |  3 +-
 src/main/pg/vtx.h                        |  1 +
 src/main/rx/fport.c                      |  5 ++-
 src/main/rx/ibus.c                       |  4 ++-
 src/main/rx/sbus.c                       |  5 ++-
 src/main/rx/spektrum.c                   |  5 +--
 src/main/rx/srxl2.c                      | 11 +++---
 src/main/rx/sumd.c                       |  4 ++-
 src/main/rx/xbus.c                       |  4 ++-
 src/main/sensors/esc_sensor.c            |  8 +++--
 src/main/target/common_pre.h             |  4 +++
 src/main/telemetry/hott.c                | 17 ++++-----
 src/main/telemetry/smartport.c           |  5 ++-
 30 files changed, 192 insertions(+), 73 deletions(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 311fffbfe4..d2fa2e1a8a 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -715,6 +715,10 @@ const clivalue_t valueTable[] = {
 #ifdef USE_SERIAL_RX
     { PARAM_NAME_SERIAL_RX_PROVIDER, VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) },
     { "serialrx_inverted",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_inverted) },
+    { "serialrx_halfduplex",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, halfDuplex) },
+#ifdef USE_SERIAL_PINSWAP
+    { "serialrx_pinswap",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, pinSwap) },
+#endif
 #endif
 #ifdef USE_SPEKTRUM_BIND
     { "spektrum_sat_bind",           VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) },
@@ -734,7 +738,6 @@ const clivalue_t valueTable[] = {
     { "crsf_use_negotiated_baud",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, crsf_use_negotiated_baud) },
 #endif
 #endif
-    { "serialrx_halfduplex",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, halfDuplex) },
 #ifdef USE_RX_SPI
     { "rx_spi_protocol",            VAR_UINT8  | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_SPI }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, rx_spi_protocol) },
     { "rx_spi_bus",                 VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, spibus) },
@@ -1173,6 +1176,9 @@ const clivalue_t valueTable[] = {
 #ifdef USE_TELEMETRY
     { "tlm_inverted",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) },
     { "tlm_halfduplex",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, halfDuplex) },
+#ifdef USE_SERIAL_PINSWAP
+    { "tlm_pinswap",                VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pinSwap) },
+#endif
 #if defined(USE_TELEMETRY_FRSKY_HUB)
 #if defined(USE_GPS)
     { "frsky_default_lat",          VAR_INT16  | MASTER_VALUE, .config.minmax = { -9000, 9000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) },
@@ -1485,6 +1491,9 @@ const clivalue_t valueTable[] = {
 // PG_VTX_CONFIG
 #if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON)
     { "vtx_halfduplex",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, halfDuplex) },
+#ifdef USE_SERIAL_PINSWAP
+    { "vtx_pinswap",                VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, pinSwap) },
+#endif
 #endif
 
 // PG_VTX_IO
@@ -1529,6 +1538,9 @@ const clivalue_t valueTable[] = {
 #ifdef USE_ESC_SENSOR
     { "esc_sensor_protocol",            VAR_UINT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ESC_SENSOR_PROTO }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, protocol) },
     { "esc_sensor_halfduplex",          VAR_UINT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) },
+#ifdef USE_SERIAL_PINSWAP
+    { "esc_sensor_pinswap",             VAR_UINT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, pinSwap) },
+#endif
     { "esc_sensor_update_hz",           VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 500 }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, update_hz) },
     { "esc_sensor_current_offset",      VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 16000 }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, current_offset) },
     { "esc_sensor_hw4_current_offset",  VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, hw4_current_offset) },
diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h
index a6fca676cb..faf040762f 100644
--- a/src/main/drivers/serial.h
+++ b/src/main/drivers/serial.h
@@ -27,21 +27,33 @@
 #include "pg/serial_pinconfig.h"
 
 typedef enum {
-    MODE_RX = 1 << 0,
-    MODE_TX = 1 << 1,
-    MODE_RXTX = MODE_RX | MODE_TX
+    MODE_RX         = BIT(0),
+    MODE_TX         = BIT(1),
+    MODE_RXTX       = MODE_RX | MODE_TX
 } portMode_e;
 
 typedef enum {
-    SERIAL_NOT_INVERTED  = 0 << 0,
-    SERIAL_INVERTED      = 1 << 0,
-    SERIAL_STOPBITS_1    = 0 << 1,
-    SERIAL_STOPBITS_2    = 1 << 1,
-    SERIAL_PARITY_NO     = 0 << 2,
-    SERIAL_PARITY_EVEN   = 1 << 2,
-    SERIAL_UNIDIR        = 0 << 3,
-    SERIAL_BIDIR         = 1 << 3,
+    SERIAL_PINSWAP_BIT          = 0,
+    SERIAL_INVERTED_BIT         = 1,
+    SERIAL_STOPBITS_BIT         = 2,
+    SERIAL_PARITY_BIT           = 3,
+    SERIAL_BIDIR_BIT            = 4,
+    SERIAL_BIDIR_OD_BIT         = 5,
+    SERIAL_BIDIR_NOPULL_BIT     = 6,
+    SERIAL_BIDIR_PULLDOWN_BIT   = 7,
+} portOptionBits_e;
 
+typedef enum {
+    SERIAL_NOSWAP               = 0,
+    SERIAL_PINSWAP              = BIT(SERIAL_PINSWAP_BIT),
+    SERIAL_NOT_INVERTED         = 0,
+    SERIAL_INVERTED             = BIT(SERIAL_INVERTED_BIT),
+    SERIAL_STOPBITS_1           = 0,
+    SERIAL_STOPBITS_2           = BIT(SERIAL_STOPBITS_BIT),
+    SERIAL_PARITY_NO            = 0,
+    SERIAL_PARITY_EVEN          = BIT( SERIAL_PARITY_BIT),
+    SERIAL_UNIDIR               = 0,
+    SERIAL_BIDIR                = BIT(SERIAL_BIDIR_BIT),
     /*
      * Note on SERIAL_BIDIR_PP
      * With SERIAL_BIDIR_PP, the very first start bit of back-to-back bytes
@@ -49,15 +61,15 @@ typedef enum {
      * To ensure the first start bit to be sent, prepend a zero byte (0x00)
      * to actual data bytes.
      */
-    SERIAL_BIDIR_OD        = 0 << 4,
-    SERIAL_BIDIR_PP        = 1 << 4,
-    SERIAL_BIDIR_NOPULL    = 1 << 5, // disable pulls in BIDIR RX mode
-    SERIAL_BIDIR_PP_PD     = 1 << 6, // PP mode, normall inverted, but with PullDowns, to fix SA after bidir issue fixed (#10220)
+    SERIAL_BIDIR_OD             = 0,
+    SERIAL_BIDIR_PP             = BIT(SERIAL_BIDIR_OD_BIT),
+    SERIAL_BIDIR_NOPULL         = BIT(SERIAL_BIDIR_NOPULL_BIT),     // disable pulls in BIDIR RX mode
+    SERIAL_BIDIR_PP_PD          = BIT(SERIAL_BIDIR_PULLDOWN_BIT),   // PP mode, normall inverted, but with PullDowns, to fix SA after bidir issue fixed (#10220)
 } portOptions_e;
 
 // Define known line control states which may be passed up by underlying serial driver callback
-#define CTRL_LINE_STATE_DTR (1 << 0)
-#define CTRL_LINE_STATE_RTS (1 << 1)
+#define CTRL_LINE_STATE_DTR     BIT(0)
+#define CTRL_LINE_STATE_RTS     BIT(1)
 
 typedef void (*serialReceiveCallbackPtr)(uint16_t data, void *rxCallbackData);   // used by serial drivers to return frames to app
 typedef void (*serialIdleCallbackPtr)();
diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c
index cbe734589e..4ad2545e53 100644
--- a/src/main/drivers/serial_softserial.c
+++ b/src/main/drivers/serial_softserial.c
@@ -221,8 +221,19 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb
 
     int pinCfgIndex = portIndex + RESOURCE_SOFT_OFFSET;
 
-    ioTag_t tagRx = serialPinConfig()->ioTagRx[pinCfgIndex];
-    ioTag_t tagTx = serialPinConfig()->ioTagTx[pinCfgIndex];
+    ioTag_t tagRx, tagTx;
+#ifdef USE_SERIAL_PINSWAP
+    if (options & SERIAL_PINSWAP)
+    {
+        tagTx = serialPinConfig()->ioTagRx[pinCfgIndex];
+        tagRx = serialPinConfig()->ioTagTx[pinCfgIndex];
+    }
+    else
+#endif
+    {
+        tagRx = serialPinConfig()->ioTagRx[pinCfgIndex];
+        tagTx = serialPinConfig()->ioTagTx[pinCfgIndex];
+    }
 
     const timerHardware_t *timerTx = timerAllocate(tagTx, OWNER_SERIAL_TX, RESOURCE_INDEX(portIndex + RESOURCE_SOFT_OFFSET));
     const timerHardware_t *timerRx = (tagTx == tagRx) ? timerTx : timerAllocate(tagRx, OWNER_SERIAL_RX, RESOURCE_INDEX(portIndex + RESOURCE_SOFT_OFFSET));
diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index b03b80a0ee..32ba234c08 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -117,6 +117,8 @@ LPUART_BUFFERS(1);
 
 serialPort_t *uartOpen(UARTDevice_e device, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options)
 {
+    uartSelectPins(device, options);
+
     uartPort_t *uartPort = serialUART(device, baudRate, mode, options);
 
     if (!uartPort)
diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c
index 7d3c6c765d..1c8e082ae0 100644
--- a/src/main/drivers/serial_uart_hal.c
+++ b/src/main/drivers/serial_uart_hal.c
@@ -64,7 +64,7 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) {
     }
 }
 
-static uartDevice_t *uartFindDevice(uartPort_t *uartPort)
+uartDevice_t *uartFindDevice(uartPort_t *uartPort)
 {
     for (uint32_t i = 0; i < UARTDEV_COUNT_MAX; i++) {
         uartDevice_t *candidate = uartDevmap[i];
@@ -76,17 +76,39 @@ static uartDevice_t *uartFindDevice(uartPort_t *uartPort)
     return NULL;
 }
 
-#if !(defined(STM32F4))
-static void uartConfigurePinSwap(uartPort_t *uartPort)
+void uartSelectPins(UARTDevice_e device, portOptions_e options)
 {
-    uartDevice_t *uartDevice = uartFindDevice(uartPort);
-    if (!uartDevice) {
-        return;
+    UNUSED(options);
+
+    uartDevice_t *uartDevice = uartDevmap[device];
+
+    if (uartDevice) {
+#ifdef USE_SERIAL_PINSWAP
+        bool swapOption = (options & SERIAL_PINSWAP);
+
+        if (uartDevice->pinSwap ^ swapOption) {
+            uartDevice->tx = uartDevice->rxPin;
+            uartDevice->rx = uartDevice->txPin;
+        }
+        else
+#endif
+        {
+            uartDevice->rx = uartDevice->rxPin;
+            uartDevice->tx = uartDevice->txPin;
+        }
     }
+}
 
-    if (uartDevice->pinSwap) {
-        uartDevice->port.Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT;
-        uartDevice->port.Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE;
+#ifdef USE_SERIAL_PINSWAP
+static void uartConfigurePinSwap(uartPort_t *uartPort)
+{
+    uartDevice_t *uartDevice = uartFindDevice(uartPort);
+    if (uartDevice) {
+        bool swapOption = (uartPort->port.options & SERIAL_PINSWAP);
+        if (uartDevice->pinSwap ^ swapOption) {
+            uartDevice->port.Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT;
+            uartDevice->port.Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE;
+        }
     }
 }
 #endif
@@ -118,7 +140,7 @@ void uartReconfigure(uartPort_t *uartPort)
 
 
     usartConfigurePinInversion(uartPort);
-#if !(defined(STM32F1) || defined(STM32F4))
+#ifdef USE_SERIAL_PINSWAP
     uartConfigurePinSwap(uartPort);
 #endif
 
diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h
index 87e55230c7..d3286e3a84 100644
--- a/src/main/drivers/serial_uart_impl.h
+++ b/src/main/drivers/serial_uart_impl.h
@@ -200,11 +200,13 @@ extern const uartHardware_t uartHardware[];
 typedef struct uartDevice_s {
     uartPort_t port;
     const uartHardware_t *hardware;
+    uartPinDef_t rxPin;
+    uartPinDef_t txPin;
     uartPinDef_t rx;
     uartPinDef_t tx;
     volatile uint8_t *rxBuffer;
     volatile uint8_t *txBuffer;
-#if !defined(STM32F4) // Don't support pin swap.
+#if defined(USE_SERIAL_PINSWAP)
     bool pinSwap;
 #endif
 } uartDevice_t;
@@ -219,6 +221,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode,
 
 void uartIrqHandler(uartPort_t *s);
 
+void uartSelectPins(UARTDevice_e device, portOptions_e options);
+
 void uartReconfigure(uartPort_t *uartPort);
 
 void uartConfigureDma(uartDevice_t *uartdev);
diff --git a/src/main/drivers/serial_uart_pinconfig.c b/src/main/drivers/serial_uart_pinconfig.c
index 7a8a56b895..5afe9aa2c6 100644
--- a/src/main/drivers/serial_uart_pinconfig.c
+++ b/src/main/drivers/serial_uart_pinconfig.c
@@ -53,34 +53,33 @@ void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig)
         const uartHardware_t *hardware = &uartHardware[hindex];
         const UARTDevice_e device = hardware->device;
 
-#if !defined(STM32F4) // Don't support pin swap.
+#ifdef USE_SERIAL_PINSWAP
         uartdev->pinSwap = false;
 #endif
         for (int pindex = 0 ; pindex < UARTHARDWARE_MAX_PINS ; pindex++) {
             if (pSerialPinConfig->ioTagRx[device] && (pSerialPinConfig->ioTagRx[device] == hardware->rxPins[pindex].pin)) {
-                uartdev->rx = hardware->rxPins[pindex];
+                uartdev->rxPin = hardware->rxPins[pindex];
             }
 
             if (pSerialPinConfig->ioTagTx[device] && (pSerialPinConfig->ioTagTx[device] == hardware->txPins[pindex].pin)) {
-                uartdev->tx = hardware->txPins[pindex];
+                uartdev->txPin = hardware->txPins[pindex];
             }
 
-
-#if !defined(STM32F4)
+#ifdef USE_SERIAL_PINSWAP
             // Check for swapped pins
             if (pSerialPinConfig->ioTagTx[device] && (pSerialPinConfig->ioTagTx[device] == hardware->rxPins[pindex].pin)) {
-                uartdev->tx = hardware->rxPins[pindex];
+                uartdev->rxPin = hardware->rxPins[pindex];
                 uartdev->pinSwap = true;
             }
 
             if (pSerialPinConfig->ioTagRx[device] && (pSerialPinConfig->ioTagRx[device] == hardware->txPins[pindex].pin)) {
-                uartdev->rx = hardware->txPins[pindex];
+                uartdev->txPin = hardware->txPins[pindex];
                 uartdev->pinSwap = true;
             }
 #endif
         }
 
-        if (uartdev->rx.pin || uartdev->tx.pin) {
+        if (uartdev->rxPin.pin || uartdev->txPin.pin) {
             uartdev->hardware = hardware;
             uartDevmap[device] = uartdev++;
         }
diff --git a/src/main/drivers/serial_uart_stdperiph.c b/src/main/drivers/serial_uart_stdperiph.c
index a77d03d3e2..c90d79337d 100644
--- a/src/main/drivers/serial_uart_stdperiph.c
+++ b/src/main/drivers/serial_uart_stdperiph.c
@@ -63,6 +63,18 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) {
 #endif
 }
 
+void uartSelectPins(UARTDevice_e device, portOptions_e options)
+{
+    UNUSED(options);
+
+    uartDevice_t *uartDevice = uartDevmap[device];
+
+    if (uartDevice) {
+        uartDevice->rx = uartDevice->rxPin;
+        uartDevice->tx = uartDevice->txPin;
+    }
+}
+
 void uartReconfigure(uartPort_t *uartPort)
 {
     USART_InitTypeDef USART_InitStructure;
diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c
index fe54b453b8..6750d222b2 100644
--- a/src/main/io/vtx_smartaudio.c
+++ b/src/main/io/vtx_smartaudio.c
@@ -700,15 +700,17 @@ bool vtxSmartAudioInit(void)
     dprintf(("smartAudioInit: OK\r\n"));
 #endif
 
-    // Note, for SA, which uses bidirectional mode, would normally require pullups. 
+    // Note, for SA, which uses bidirectional mode, would normally require pullups.
     // the SA protocol instead requires pulldowns, and therefore uses SERIAL_BIDIR_PP_PD instead of SERIAL_BIDIR_PP
     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO);
     if (portConfig) {
         portOptions_e portOptions = SERIAL_STOPBITS_2 | SERIAL_BIDIR_NOPULL;
 #if defined(USE_VTX_COMMON)
-        portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP_PD : SERIAL_UNIDIR);
+        portOptions |=
+            (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP_PD : SERIAL_UNIDIR) |
+            (vtxConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP);
 #else
-        portOptions = SERIAL_BIDIR;
+        portOptions |= SERIAL_BIDIR;
 #endif
 
         smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions);
diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c
index 562eb30319..988c5d1bf7 100644
--- a/src/main/io/vtx_tramp.c
+++ b/src/main/io/vtx_tramp.c
@@ -654,9 +654,11 @@ bool vtxTrampInit(void)
     if (portConfig) {
         portOptions_e portOptions = 0;
 #if defined(USE_VTX_COMMON)
-        portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR);
+        portOptions |=
+            (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+            (vtxConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP);
 #else
-        portOptions = SERIAL_BIDIR;
+        portOptions |= SERIAL_BIDIR;
 #endif
 
         trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions);
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index 55c3ae03bc..ad1dc4e1ee 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -803,6 +803,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
         sbufWriteU16(dst, escSensorConfig()->hw4_current_offset);
         sbufWriteU8(dst, escSensorConfig()->hw4_current_gain);
         sbufWriteU8(dst, escSensorConfig()->hw4_voltage_gain);
+        sbufWriteU8(dst, escSensorConfig()->pinSwap);
         break;
 
     case MSP_ESC_PARAMETERS:
@@ -1535,6 +1536,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         sbufWriteU32(dst, 0);
         sbufWriteU8(dst, 0);
 #endif
+        sbufWriteU8(dst, rxConfig()->pinSwap);
         break;
 
     case MSP_FAILSAFE_CONFIG:
@@ -1578,6 +1580,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, telemetryConfig()->telemetry_inverted);
         sbufWriteU8(dst, telemetryConfig()->halfDuplex);
         sbufWriteU32(dst, telemetryConfig()->enableSensors);
+        sbufWriteU8(dst, telemetryConfig()->pinSwap);
         break;
 
     case MSP_SERIAL_CONFIG:
@@ -2650,6 +2653,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         escSensorConfigMutable()->hw4_current_offset = sbufReadU16(src);
         escSensorConfigMutable()->hw4_current_gain = sbufReadU8(src);
         escSensorConfigMutable()->hw4_voltage_gain = sbufReadU8(src);
+        if (sbufBytesRemaining(src) >= 1) {
+            escSensorConfigMutable()->pinSwap = sbufReadU8(src);
+        }
         break;
 
     case MSP_SET_ESC_PARAMETERS:
@@ -3016,6 +3022,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
             rxConfigMutable()->halfDuplex = sbufReadU8(src);
             rxConfigMutable()->rx_pulse_min = sbufReadU16(src);
             rxConfigMutable()->rx_pulse_max = sbufReadU16(src);
+            if (sbufBytesRemaining(src) >= 6) {
+                sbufReadU8(src);
+                sbufReadU32(src);
+                sbufReadU8(src);
+            }
+            if (sbufBytesRemaining(src) >= 1) {
+                rxConfigMutable()->pinSwap = sbufReadU8(src);
+            }
         }
     #ifdef USE_RX_SPI
         if (sbufBytesRemaining(src) >= 6) {
@@ -3072,6 +3086,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         telemetryConfigMutable()->telemetry_inverted = sbufReadU8(src);
         telemetryConfigMutable()->halfDuplex = sbufReadU8(src);
         telemetryConfigMutable()->enableSensors = sbufReadU32(src);
+        if (sbufBytesRemaining(src) >= 1) {
+            telemetryConfigMutable()->pinSwap = sbufReadU8(src);
+        }
         break;
 
     case MSP_SET_SERIAL_CONFIG:
diff --git a/src/main/pg/esc_sensor.c b/src/main/pg/esc_sensor.c
index 4e88a95941..275753bb23 100644
--- a/src/main/pg/esc_sensor.c
+++ b/src/main/pg/esc_sensor.c
@@ -27,6 +27,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSO
 PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig,
         .protocol = ESC_SENSOR_PROTO_NONE,
         .halfDuplex = 0,
+        .pinSwap = 0,
         .update_hz = ESC_SENSOR_TASK_FREQ_HZ,
         .current_offset = 0,
         .hw4_current_offset = 0,
diff --git a/src/main/pg/esc_sensor.h b/src/main/pg/esc_sensor.h
index 60f98316c6..0ceb9f031b 100644
--- a/src/main/pg/esc_sensor.h
+++ b/src/main/pg/esc_sensor.h
@@ -43,6 +43,7 @@ enum {
 typedef struct {
     uint8_t     protocol;               // ESC telemetry protocol
     uint8_t     halfDuplex;             // Set to false to listen on the TX pin for telemetry data
+    uint8_t     pinSwap;                // Swap rx and tx pins around compared to the resource settings
     uint16_t    update_hz;              // Update frequency
     uint16_t    current_offset;         // Offset (extra current) consumed by the VTX / cam (mA)
     uint16_t    hw4_current_offset;     // HobbyWing V4 raw current offset
diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c
index 41a2f0d2d1..a4bff7fdc3 100644
--- a/src/main/pg/rx.c
+++ b/src/main/pg/rx.c
@@ -40,6 +40,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
         .serialrx_provider = SERIALRX_PROVIDER,
         .serialrx_inverted = 0,
         .halfDuplex = 0,
+        .pinSwap = 0,
         .rx_pulse_min = RX_PWM_PULSE_MIN,
         .rx_pulse_max = RX_PWM_PULSE_MAX,
         .rssi_src_frame_errors = false,
diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h
index f7d3a6a2d7..39a9ab4be1 100644
--- a/src/main/pg/rx.h
+++ b/src/main/pg/rx.h
@@ -31,6 +31,7 @@ typedef struct rxConfig_s {
     uint8_t serialrx_provider;                 // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first.
     uint8_t serialrx_inverted;                 // invert the serial RX protocol compared to it's default setting
     uint8_t halfDuplex;                        // allow rx to operate in half duplex mode on F4, ignored for F1 and F3.
+    uint8_t pinSwap;                           // swap rx and tx pins around compared to the resource settings
     uint16_t rx_pulse_min;                     // Absolute minimum pulse accepted
     uint16_t rx_pulse_max;                     // Absolute maximum pulse accepted
     uint8_t rssi_channel;
diff --git a/src/main/pg/telemetry.c b/src/main/pg/telemetry.c
index c960383f57..de13595eeb 100644
--- a/src/main/pg/telemetry.c
+++ b/src/main/pg/telemetry.c
@@ -34,6 +34,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_inverted = false,
     .halfDuplex = 1,
+    .pinSwap = 0,
     .gpsNoFixLatitude = 0,
     .gpsNoFixLongitude = 0,
     .frsky_coordinate_format = FRSKY_FORMAT_DMS,
diff --git a/src/main/pg/telemetry.h b/src/main/pg/telemetry.h
index f08fbd351a..87d91baa8d 100644
--- a/src/main/pg/telemetry.h
+++ b/src/main/pg/telemetry.h
@@ -64,6 +64,7 @@ typedef struct {
     int16_t gpsNoFixLongitude;
     uint8_t telemetry_inverted;
     uint8_t halfDuplex;
+    uint8_t pinSwap;
     uint8_t frsky_coordinate_format;
     uint8_t frsky_unit;
     uint8_t frsky_vfas_precision;
diff --git a/src/main/pg/vtx.c b/src/main/pg/vtx.c
index 291091e3fb..52ef7a671f 100644
--- a/src/main/pg/vtx.c
+++ b/src/main/pg/vtx.c
@@ -30,7 +30,8 @@ PG_REGISTER_WITH_RESET_FN(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTING
 PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1);
 
 PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
-    .halfDuplex = true
+    .halfDuplex = 1,
+    .pinSwap = 0
 );
 
 #endif
\ No newline at end of file
diff --git a/src/main/pg/vtx.h b/src/main/pg/vtx.h
index 873216f97a..3d1c9ccf2a 100644
--- a/src/main/pg/vtx.h
+++ b/src/main/pg/vtx.h
@@ -49,6 +49,7 @@ typedef struct {
 typedef struct {
     vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT];
     uint8_t halfDuplex;
+    uint8_t pinSwap;
 } vtxConfig_t;
 
 PG_DECLARE(vtxConfig_t, vtxConfig);
diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c
index 4bd0e8e472..9d0e37dcc7 100644
--- a/src/main/rx/fport.c
+++ b/src/main/rx/fport.c
@@ -408,7 +408,10 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         NULL,
         FPORT_BAUDRATE,
         MODE_RXTX,
-        FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+        FPORT_PORT_OPTIONS |
+        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) |
+        (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP)
     );
 
     if (fportPort) {
diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c
index 753e25b8d5..5f4b3eba26 100644
--- a/src/main/rx/ibus.c
+++ b/src/main/rx/ibus.c
@@ -232,7 +232,9 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         NULL,
         IBUS_BAUDRATE,
         portShared ? MODE_RXTX : MODE_RX,
-        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0)
+        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) |
+        (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP)
         );
 
 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c
index a63889376f..d0512dcf8c 100644
--- a/src/main/rx/sbus.c
+++ b/src/main/rx/sbus.c
@@ -198,7 +198,10 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         &sbusFrameData,
         sbusBaudRate,
         portShared ? MODE_RXTX : MODE_RX,
-        SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+        SBUS_PORT_OPTIONS |
+        (rxConfig->serialrx_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED) |
+        (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP)
         );
 
     if (rxConfig->rssi_src_frame_errors) {
diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c
index 8333b2ecce..248ed53f78 100644
--- a/src/main/rx/spektrum.c
+++ b/src/main/rx/spektrum.c
@@ -400,8 +400,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         rxRuntimeState,
         SPEKTRUM_BAUDRATE,
         portShared || srxlEnabled ? MODE_RXTX : MODE_RX,
-        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) |
-        ((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
+        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) |
+        ((rxConfig->halfDuplex || srxlEnabled) ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP)
         );
 
 #if defined(USE_TELEMETRY_SRXL)
diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c
index 9406e1e2c3..979bede0bd 100644
--- a/src/main/rx/srxl2.c
+++ b/src/main/rx/srxl2.c
@@ -500,13 +500,10 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         return false;
     }
 
-    portOptions_e options = SRXL2_PORT_OPTIONS;
-    if (rxConfig->serialrx_inverted) {
-        options |= SERIAL_INVERTED;
-    }
-    if (rxConfig->halfDuplex) {
-        options |= SERIAL_BIDIR;
-    }
+    portOptions_e options = SRXL2_PORT_OPTIONS |
+        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) |
+        (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP);
 
     serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive,
         NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options);
diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c
index 76a60b4496..daf92b2481 100644
--- a/src/main/rx/sumd.c
+++ b/src/main/rx/sumd.c
@@ -195,7 +195,9 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         NULL,
         SUMD_BAUDRATE,
         portShared ? MODE_RXTX : MODE_RX,
-        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) |
+        (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP)
         );
 
 #ifdef USE_TELEMETRY
diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c
index 3981bd4c4a..0fb64d4d3f 100644
--- a/src/main/rx/xbus.c
+++ b/src/main/rx/xbus.c
@@ -314,7 +314,9 @@ bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         NULL,
         baudRate,
         portShared ? MODE_RXTX : MODE_RX,
-        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+        (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) |
+        (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP)
         );
 
 #ifdef USE_TELEMETRY
diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c
index f0edb88062..b4678980d6 100644
--- a/src/main/sensors/esc_sensor.c
+++ b/src/main/sensors/esc_sensor.c
@@ -1541,8 +1541,8 @@ static bool pl5ParamCommit(uint8_t cmd)
             return false;
 
         // params dirty?
-        if (memcmp(paramPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH, 
-            paramUpdPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH, 
+        if (memcmp(paramPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH,
+            paramUpdPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH,
             PL5_RESP_GETPARAMS_PAYLOAD_LENGTH) != 0) {
             // set dirty flag, will schedule read
             pl5DirtyParams = true;
@@ -2803,7 +2803,9 @@ bool INIT_CODE escSensorInit(void)
         return false;
     }
 
-    options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED | (escHalfDuplex ? SERIAL_BIDIR : 0);
+    options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED |
+        (escHalfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (escSensorConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP);
 
     switch (escSensorConfig()->protocol) {
         case ESC_SENSOR_PROTO_BLHELI32:
diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h
index 31ad214b98..b3fab1ea04 100644
--- a/src/main/target/common_pre.h
+++ b/src/main/target/common_pre.h
@@ -224,6 +224,10 @@ extern uint8_t _dmaram_end__;
 #define USE_DMA
 #define USE_TIMER
 
+#ifndef STM32F4
+#define USE_SERIAL_PINSWAP
+#endif
+
 #define USE_CLI
 #define USE_SERIAL_PASSTHROUGH
 #define USE_GYRO_REGISTER_DUMP  // Adds gyroregisters command to cli to dump configured register values
diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c
index 61c7fcef30..d2a034f7fe 100644
--- a/src/main/telemetry/hott.c
+++ b/src/main/telemetry/hott.c
@@ -371,11 +371,10 @@ static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e
 {
     closeSerialPort(hottPort);
 
-    portOptions_e portOptions = telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED;
-
-    if (telemetryConfig()->halfDuplex) {
-        portOptions |= SERIAL_BIDIR;
-    }
+    portOptions_e portOptions =
+        (telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) |
+        (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (telemetryConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP);
 
     hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, mode, portOptions);
 }
@@ -416,11 +415,9 @@ void configureHoTTTelemetryPort(void)
         return;
     }
 
-    portOptions_e portOptions = SERIAL_NOT_INVERTED;
-
-    if (telemetryConfig()->halfDuplex) {
-        portOptions |= SERIAL_BIDIR;
-    }
+    portOptions_e portOptions = SERIAL_NOT_INVERTED |
+        (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+        (telemetryConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP);
 
     hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_PORT_MODE, portOptions);
 
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 82f16cc4cf..3871beb7dd 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -502,7 +502,10 @@ static void freeSmartPortTelemetryPort(void)
 static void configureSmartPortTelemetryPort(void)
 {
     if (portConfig) {
-        portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
+        portOptions_e portOptions =
+            (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED) |
+            (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) |
+            (telemetryConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP);
 
         smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
     }

From 7e973c2635d682000e5568fea0b81ca7817a0338 Mon Sep 17 00:00:00 2001
From: Petri Mattila <petri.t.j.mattila@gmail.com>
Date: Tue, 4 Jun 2024 11:51:44 +0100
Subject: [PATCH 2/2] Refactor CRSF telemetry

This commit introduces Custom CRSF Telemetry.

It is using a "custom" CRSF frame (0x88) for passing the telemetry
data to the Tx. On the Tx side, a LUA script is parsing these frames
and injecting the sensors into the native sensor framework.

The FC has over hundred telemetry sensors to choose from.
Up to 40 can be enabled at the same time. Depending on the link
bandwidth (up to 20kbit/s with ELRS), sensor update rates up to 50Hz
are achievable.

This framework is also fully extensible in the future. For adding
new sensors, changes in the firmware and the LUA script is needed.
The Receiver or the Transmitter are not involved in the process.
---
 make/source.mk                         |    1 +
 src/main/cli/settings.c                |   41 +-
 src/main/cli/settings.h                |    5 +-
 src/main/fc/tasks.c                    |    4 +-
 src/main/msp/msp.c                     |   14 +
 src/main/pg/telemetry.c                |   22 +-
 src/main/pg/telemetry.h                |   75 +-
 src/main/rx/crsf.c                     |   31 +-
 src/main/rx/crsf.h                     |    4 +-
 src/main/rx/crsf_protocol.h            |    5 +
 src/main/rx/rx.c                       |    2 +
 src/main/sensors/battery.c             |    6 +
 src/main/sensors/battery.h             |    1 +
 src/main/target/STM32_UNIFIED/target.h |    2 +
 src/main/telemetry/crsf.c              | 1692 +++++++++++++-----------
 src/main/telemetry/crsf.h              |   96 +-
 src/main/telemetry/sensors.c           |  511 +++++++
 src/main/telemetry/sensors.h           |  198 +++
 src/main/telemetry/telemetry.c         |  228 ++--
 src/main/telemetry/telemetry.h         |   55 +-
 20 files changed, 1908 insertions(+), 1085 deletions(-)
 create mode 100644 src/main/telemetry/sensors.c
 create mode 100644 src/main/telemetry/sensors.h

diff --git a/make/source.mk b/make/source.mk
index 57196f5b05..c3d9cbf864 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -191,6 +191,7 @@ COMMON_SRC = \
             telemetry/msp_shared.c \
             telemetry/ibus.c \
             telemetry/ibus_shared.c \
+            telemetry/sensors.c \
             sensors/esc_sensor.c \
             io/vtx.c \
             io/vtx_rtc6705.c \
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index d2fa2e1a8a..d552efa9dc 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -484,26 +484,14 @@ const char * const lookupTableSwashType[] = {
     "NONE", "PASSTHROUGH", "CP120", "CP135", "CP140", "FP90L", "FP90V",
 };
 
-const char * const lookupTableCrsfFmReuse[] = {
-    "NONE", "GOVERNOR", "HEADSPEED", "THROTTLE", "ESC_TEMP", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE", "ADJFUNC", "GOV_ADJFUNC",
-};
-
-const char * const lookupTableCrsfAttReuse[] = {
-    "NONE", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE",
-};
-
-const char * const lookupTableCrsfGpsReuse[] = {
-    "NONE", "HEADSPEED", "THROTTLE", "ESC_TEMP", "ESC_PWM", "ESC_THROTTLE", "ESC_BEC_VOLTAGE", "ESC_BEC_CURRENT", "ESC_BEC_TEMP", "ESC_STATUS", "ESC_STATUS2", "MCU_TEMP", "MCU_LOAD", "SYS_LOAD", "RT_LOAD", "BEC_VOLTAGE", "BUS_VOLTAGE", "MCU_VOLTAGE",
-};
-
-const char * const lookupTableCrsfGpsSatsReuse[] = {
-    "NONE", "ESC_TEMP", "MCU_TEMP", "PROFILE", "RATE_PROFILE", "LED_PROFILE", "MODEL_ID",
-};
-
 const char * const lookupTableDtermMode[] = {
     "GYRO", "ERROR",
 };
 
+const char * const lookupTableTelemMode[] = {
+    "NATIVE", "CUSTOM",
+};
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -613,11 +601,8 @@ const lookupTableEntry_t lookupTables[] = {
 
     LOOKUP_TABLE_ENTRY(lookupTableRescueMode),
     LOOKUP_TABLE_ENTRY(lookupTableSwashType),
-    LOOKUP_TABLE_ENTRY(lookupTableCrsfFmReuse),
-    LOOKUP_TABLE_ENTRY(lookupTableCrsfAttReuse),
-    LOOKUP_TABLE_ENTRY(lookupTableCrsfGpsReuse),
-    LOOKUP_TABLE_ENTRY(lookupTableCrsfGpsSatsReuse),
     LOOKUP_TABLE_ENTRY(lookupTableDtermMode),
+    LOOKUP_TABLE_ENTRY(lookupTableTelemMode),
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -1200,15 +1185,11 @@ const clivalue_t valueTable[] = {
     // Set to $size_of_battery to get a percentage of battery used.
     { "mavlink_mah_as_heading_divisor", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 30000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, mavlink_mah_as_heading_divisor) },
 #endif
-    { "crsf_flight_mode_reuse",      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_FM_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_flight_mode_reuse) },
-    { "crsf_att_pitch_reuse",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_ATT_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_att_pitch_reuse) },
-    { "crsf_att_roll_reuse",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_ATT_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_att_roll_reuse) },
-    { "crsf_att_yaw_reuse",          VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_ATT_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_att_yaw_reuse) },
-
-    { "crsf_gps_heading_reuse",      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_heading_reuse) },
-    { "crsf_gps_ground_speed_reuse", VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_ground_speed_reuse) },
-    { "crsf_gps_altitude_reuse",     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_altitude_reuse) },
-    { "crsf_gps_sats_reuse",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_CRSF_GPS_SATS_REUSE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_gps_sats_reuse) },
+    { "crsf_telemetry_mode",         VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_TELEM_MODE }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_mode) },
+    { "crsf_telemetry_link_rate",    VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_link_rate) },
+    { "crsf_telemetry_link_ratio",   VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_link_ratio) },
+    { "crsf_telemetry_sensors",      VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = TELEM_SENSOR_SLOT_COUNT, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_sensors)},
+    { "crsf_telemetry_interval",     VAR_UINT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = TELEM_SENSOR_SLOT_COUNT, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, crsf_telemetry_interval)},
 
 #ifdef USE_TELEMETRY_ENABLE_SENSORS
     { "telemetry_enable_voltage",         VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_VOLTAGE),         PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
@@ -1235,7 +1216,7 @@ const clivalue_t valueTable[] = {
     { "telemetry_enable_adjustment",      VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_ADJUSTMENT),      PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
     { "telemetry_enable_gov_mode",        VAR_UINT32  | MASTER_VALUE | MODE_BITSET, .config.bitpos = LOG2(SENSOR_GOV_MODE),        PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
 #else
-    { "telemetry_enable_sensors", VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
+    { "telemetry_enable_sensors",         VAR_UINT32 | MASTER_VALUE, .config.u32Max = SENSOR_ALL, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, enableSensors)},
 #endif
 #endif // USE_TELEMETRY
 
diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h
index 0e9777c5d9..da4a0d42e5 100644
--- a/src/main/cli/settings.h
+++ b/src/main/cli/settings.h
@@ -127,11 +127,8 @@ typedef enum {
 #endif
     TABLE_RESCUE_MODE,
     TABLE_SWASH_TYPE,
-    TABLE_CRSF_FM_REUSE,
-    TABLE_CRSF_ATT_REUSE,
-    TABLE_CRSF_GPS_REUSE,
-    TABLE_CRSF_GPS_SATS_REUSE,
     TABLE_DTERM_MODE,
+    TABLE_TELEM_MODE,
 
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c
index 9e779bd3d9..4546ddeee8 100644
--- a/src/main/fc/tasks.c
+++ b/src/main/fc/tasks.c
@@ -503,8 +503,8 @@ void tasksInit(void)
             // Reschedule telemetry to 500hz for Jeti Exbus
             rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
         } else if (rxRuntimeState.serialrxProvider == SERIALRX_CRSF) {
-            // Reschedule telemetry to 500hz, 2ms for CRSF
-            rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
+            // Reschedule telemetry to 1000hz, 1ms for CRSF
+            rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(1000));
         }
     }
 #endif
diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c
index ad1dc4e1ee..e08321b8f7 100644
--- a/src/main/msp/msp.c
+++ b/src/main/msp/msp.c
@@ -1581,6 +1581,12 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, telemetryConfig()->halfDuplex);
         sbufWriteU32(dst, telemetryConfig()->enableSensors);
         sbufWriteU8(dst, telemetryConfig()->pinSwap);
+        sbufWriteU8(dst, telemetryConfig()->crsf_telemetry_mode);
+        sbufWriteU16(dst, telemetryConfig()->crsf_telemetry_link_rate);
+        sbufWriteU16(dst, telemetryConfig()->crsf_telemetry_link_ratio);
+        for (int i = 0; i < TELEM_SENSOR_SLOT_COUNT; i++) {
+            sbufWriteU8(dst, telemetryConfig()->crsf_telemetry_sensors[i]);
+        }
         break;
 
     case MSP_SERIAL_CONFIG:
@@ -3089,6 +3095,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
         if (sbufBytesRemaining(src) >= 1) {
             telemetryConfigMutable()->pinSwap = sbufReadU8(src);
         }
+        if (sbufBytesRemaining(src) >= 5 + TELEM_SENSOR_SLOT_COUNT) {
+            telemetryConfigMutable()->crsf_telemetry_mode = sbufReadU8(src);
+            telemetryConfigMutable()->crsf_telemetry_link_rate = sbufReadU16(src);
+            telemetryConfigMutable()->crsf_telemetry_link_ratio = sbufReadU16(src);
+            for (int i = 0; i < TELEM_SENSOR_SLOT_COUNT; i++) {
+                telemetryConfigMutable()->crsf_telemetry_sensors[i] = sbufReadU8(src);
+            }
+        }
         break;
 
     case MSP_SET_SERIAL_CONFIG:
diff --git a/src/main/pg/telemetry.c b/src/main/pg/telemetry.c
index de13595eeb..b2e4eb7317 100644
--- a/src/main/pg/telemetry.c
+++ b/src/main/pg/telemetry.c
@@ -22,14 +22,11 @@
 
 #include "common/unit.h"
 
-#include "config/config.h"
-
+#include "pg/pg_ids.h"
 #include "pg/telemetry.h"
 
-#include "telemetry/crsf.h"
-
 
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 5);
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 6);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .telemetry_inverted = false,
@@ -44,9 +41,9 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .pidValuesAsTelemetry = 0,
     .report_cell_voltage = false,
     .flysky_sensors = {
-            IBUS_SENSOR_TYPE_TEMPERATURE,
-            IBUS_SENSOR_TYPE_RPM_FLYSKY,
-            IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE
+        IBUS_SENSOR_TYPE_TEMPERATURE,
+        IBUS_SENSOR_TYPE_RPM_FLYSKY,
+        IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE
     },
     .enableSensors =
         SENSOR_VOLTAGE |
@@ -57,10 +54,11 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
         ESC_SENSOR_RPM |
         ESC_SENSOR_TEMPERATURE,
     .mavlink_mah_as_heading_divisor = 0,
-    .crsf_flight_mode_reuse = CRSF_FM_REUSE_NONE,
-    .crsf_att_pitch_reuse = CRSF_ATT_REUSE_NONE,
-    .crsf_att_roll_reuse = CRSF_ATT_REUSE_NONE,
-    .crsf_att_yaw_reuse = CRSF_ATT_REUSE_NONE,
+    .crsf_telemetry_mode = CRSF_TELEMETRY_MODE_NATIVE,
+    .crsf_telemetry_sensors = INIT_ZERO,
+    .crsf_telemetry_interval = INIT_ZERO,
+    .crsf_telemetry_link_rate = 250,
+    .crsf_telemetry_link_ratio = 8,
 );
 
 #endif
diff --git a/src/main/pg/telemetry.h b/src/main/pg/telemetry.h
index 87d91baa8d..a41233860e 100644
--- a/src/main/pg/telemetry.h
+++ b/src/main/pg/telemetry.h
@@ -23,35 +23,31 @@
 
 #include "pg/pg.h"
 
+
 typedef enum {
-    SENSOR_VOLTAGE         = 1 << 0,
-    SENSOR_CURRENT         = 1 << 1,
-    SENSOR_FUEL            = 1 << 2,
-    SENSOR_MODE            = 1 << 3,
-    SENSOR_ACC_X           = 1 << 4,
-    SENSOR_ACC_Y           = 1 << 5,
-    SENSOR_ACC_Z           = 1 << 6,
-    SENSOR_PITCH           = 1 << 7,
-    SENSOR_ROLL            = 1 << 8,
-    SENSOR_HEADING         = 1 << 9,
-    SENSOR_ALTITUDE        = 1 << 10,
-    SENSOR_VARIO           = 1 << 11,
-    SENSOR_LAT_LONG        = 1 << 12,
-    SENSOR_GROUND_SPEED    = 1 << 13,
-    SENSOR_DISTANCE        = 1 << 14,
-    ESC_SENSOR_CURRENT     = 1 << 15,
-    ESC_SENSOR_VOLTAGE     = 1 << 16,
-    ESC_SENSOR_RPM         = 1 << 17,
-    ESC_SENSOR_TEMPERATURE = 1 << 18,
-    ESC_SENSOR_ALL         = ESC_SENSOR_CURRENT \
-                            | ESC_SENSOR_VOLTAGE \
-                            | ESC_SENSOR_RPM \
-                            | ESC_SENSOR_TEMPERATURE,
-    SENSOR_TEMPERATURE     = 1 << 19,
-    SENSOR_CAP_USED        = 1 << 20,
-    SENSOR_ADJUSTMENT      = 1 << 21,
-    SENSOR_GOV_MODE        = 1 << 22,
-    SENSOR_ALL             = (1 << 23) - 1,
+    SENSOR_VOLTAGE         = BIT(0),
+    SENSOR_CURRENT         = BIT(1),
+    SENSOR_FUEL            = BIT(2),
+    SENSOR_MODE            = BIT(3),
+    SENSOR_ACC_X           = BIT(4),
+    SENSOR_ACC_Y           = BIT(5),
+    SENSOR_ACC_Z           = BIT(6),
+    SENSOR_PITCH           = BIT(7),
+    SENSOR_ROLL            = BIT(8),
+    SENSOR_HEADING         = BIT(9),
+    SENSOR_ALTITUDE        = BIT(10),
+    SENSOR_VARIO           = BIT(11),
+    SENSOR_LAT_LONG        = BIT(12),
+    SENSOR_GROUND_SPEED    = BIT(13),
+    SENSOR_DISTANCE        = BIT(14),
+    ESC_SENSOR_CURRENT     = BIT(15),
+    ESC_SENSOR_VOLTAGE     = BIT(16),
+    ESC_SENSOR_RPM         = BIT(17),
+    ESC_SENSOR_TEMPERATURE = BIT(18),
+    SENSOR_TEMPERATURE     = BIT(19),
+    SENSOR_CAP_USED        = BIT(20),
+    SENSOR_ADJUSTMENT      = BIT(21),
+    SENSOR_GOV_MODE        = BIT(22),
 } sensor_e;
 
 typedef enum {
@@ -59,7 +55,14 @@ typedef enum {
     FRSKY_FORMAT_NMEA
 } frskyGpsCoordFormat_e;
 
-typedef struct {
+enum {
+    CRSF_TELEMETRY_MODE_NATIVE = 0,
+    CRSF_TELEMETRY_MODE_CUSTOM,
+};
+
+#define TELEM_SENSOR_SLOT_COUNT 40
+
+typedef struct telemetryConfig_s {
     int16_t gpsNoFixLatitude;
     int16_t gpsNoFixLongitude;
     uint8_t telemetry_inverted;
@@ -73,15 +76,13 @@ typedef struct {
     uint8_t report_cell_voltage;
     uint8_t flysky_sensors[IBUS_SENSOR_COUNT];
     uint16_t mavlink_mah_as_heading_divisor;
-    uint8_t crsf_flight_mode_reuse;
-    uint8_t crsf_att_pitch_reuse;
-    uint8_t crsf_att_roll_reuse;
-    uint8_t crsf_att_yaw_reuse;
-    uint8_t crsf_gps_heading_reuse;
-    uint8_t crsf_gps_ground_speed_reuse;
-    uint8_t crsf_gps_altitude_reuse;
-    uint8_t crsf_gps_sats_reuse;
     uint32_t enableSensors;
+    uint8_t crsf_telemetry_mode;
+    uint16_t crsf_telemetry_sensors[TELEM_SENSOR_SLOT_COUNT];
+    uint16_t crsf_telemetry_interval[TELEM_SENSOR_SLOT_COUNT];
+    uint16_t crsf_telemetry_link_rate;
+    uint16_t crsf_telemetry_link_ratio;
 } telemetryConfig_t;
 
 PG_DECLARE(telemetryConfig_t, telemetryConfig);
+
diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c
index 348c6d08b6..b7437de345 100644
--- a/src/main/rx/crsf.c
+++ b/src/main/rx/crsf.c
@@ -70,8 +70,6 @@ STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
 
 static serialPort_t *serialPort;
 static timeUs_t crsfFrameStartAtUs = 0;
-static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
-static uint8_t telemetryBufLen = 0;
 static float channelScale = CRSF_RC_CHANNEL_SCALE_LEGACY;
 
 #ifdef USE_RX_LINK_UPLINK_POWER
@@ -337,6 +335,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
     return crc;
 }
 
+#if defined(USE_CRSF_V3)
 STATIC_UNIT_TESTED uint8_t crsfFrameCmdCRC(void)
 {
     // CRC includes type and payload
@@ -346,6 +345,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCmdCRC(void)
     }
     return crc;
 }
+#endif
 
 // Receive ISR callback, called back from serial port
 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
@@ -412,9 +412,6 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
                 }
 #endif
 #if defined(USE_CRSF_CMS_TELEMETRY)
-                case CRSF_FRAMETYPE_DEVICE_PING:
-                    crsfScheduleDeviceInfoResponse();
-                    break;
                 case CRSF_FRAMETYPE_DISPLAYPORT_CMD: {
                     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
                     crsfProcessDisplayPortCmd(frameStart);
@@ -422,7 +419,6 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
                 }
 #endif
 #if defined(USE_CRSF_LINK_STATISTICS)
-
                 case CRSF_FRAMETYPE_LINK_STATISTICS: {
                     // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
                     if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
@@ -456,6 +452,9 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
                     }
                     break;
 #endif
+                case CRSF_FRAMETYPE_DEVICE_PING:
+                    crsfScheduleDeviceInfoResponse();
+                    break;
                 default:
                     break;
                 }
@@ -598,27 +597,13 @@ STATIC_UNIT_TESTED float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, u
     }
 }
 
-void crsfRxWriteTelemetryData(const void *data, int len)
-{
-    len = MIN(len, (int)sizeof(telemetryBuf));
-    memcpy(telemetryBuf, data, len);
-    telemetryBufLen = len;
-}
-
-void crsfRxSendTelemetryData(void)
+void crsfRxTransmitTelemetryData(const void *data, int len)
 {
-    // if there is telemetry data to write
-    if (telemetryBufLen > 0) {
-        serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
-        telemetryBufLen = 0; // reset telemetry buffer
+    if (len > 0 && len <= CRSF_FRAME_SIZE_MAX) {
+        serialWriteBuf(serialPort, data, len);
     }
 }
 
-bool crsfRxIsTelemetryBufEmpty(void)
-{
-    return telemetryBufLen == 0;
-}
-
 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 {
     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h
index a1ae3c80be..038735aee4 100644
--- a/src/main/rx/crsf.h
+++ b/src/main/rx/crsf.h
@@ -78,9 +78,7 @@ typedef union crsfFrame_u {
     crsfFrameDef_t frame;
 } crsfFrame_t;
 
-void crsfRxWriteTelemetryData(const void *data, int len);
-void crsfRxSendTelemetryData(void);
-bool crsfRxIsTelemetryBufEmpty(void); // check this function before using crsfRxWriteTelemetryData()
+void crsfRxTransmitTelemetryData(const void *data, int len);
 
 struct rxConfig_s;
 struct rxRuntimeState_s;
diff --git a/src/main/rx/crsf_protocol.h b/src/main/rx/crsf_protocol.h
index 99fef0d1a1..439bace241 100644
--- a/src/main/rx/crsf_protocol.h
+++ b/src/main/rx/crsf_protocol.h
@@ -36,7 +36,9 @@ enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 };
 
 typedef enum {
     CRSF_FRAMETYPE_GPS = 0x02,
+    CRSF_FRAMETYPE_VARIO_SENSOR = 0x07,
     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
+    CRSF_FRAMETYPE_ALTITUDE_SENSOR = 0x09,
     CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
@@ -57,6 +59,7 @@ typedef enum {
     CRSF_FRAMETYPE_MSP_RESP = 0x7B,  // reply with 58 byte chunked binary
     CRSF_FRAMETYPE_MSP_WRITE = 0x7C,  // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
     CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7D, // displayport control command
+    CRSF_FRAMETYPE_CUSTOM_TELEM = 0x88, // custom telemetry
 } crsfFrameType_e;
 
 enum {
@@ -83,7 +86,9 @@ enum {
 
 enum {
     CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
+    CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2,
     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
+    CRSF_FRAME_ALTITUDE_SENSOR_PAYLOAD_SIZE = 4,
     CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE = 2,
     CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
     CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE = 6,
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 040b469af0..031cfafcb2 100644
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -54,6 +54,8 @@
 #include "pg/pg_ids.h"
 #include "pg/rx.h"
 
+#include "telemetry/sensors.h"
+
 #include "rx/rx.h"
 #include "rx/pwm.h"
 #include "rx/fport.h"
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index f22abc592a..32a22e1371 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -139,6 +139,12 @@ uint8_t getBatteryCellCount(void)
     return batteryCellCount;
 }
 
+uint16_t getBatteryCellVoltage(uint8_t cell)
+{
+    UNUSED(cell);
+    return getBatteryAverageCellVoltage(); // Not implemented
+}
+
 uint16_t getBatteryAverageCellVoltage(void)
 {
     return (batteryCellCount > 0) ? getBatteryVoltage() / batteryCellCount : 0;
diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h
index 35d857e9f8..00aa51eea8 100644
--- a/src/main/sensors/battery.h
+++ b/src/main/sensors/battery.h
@@ -63,6 +63,7 @@ const char * getBatteryStateString(void);
 
 bool isBatteryVoltageConfigured(void);
 uint8_t getBatteryCellCount(void);
+uint16_t getBatteryCellVoltage(uint8_t cell);
 uint16_t getBatteryVoltage(void);
 uint16_t getBatteryVoltageSample(void);
 uint16_t getLegacyBatteryVoltage(void);
diff --git a/src/main/target/STM32_UNIFIED/target.h b/src/main/target/STM32_UNIFIED/target.h
index e616b044b7..9987d45b77 100644
--- a/src/main/target/STM32_UNIFIED/target.h
+++ b/src/main/target/STM32_UNIFIED/target.h
@@ -385,6 +385,8 @@
 
 #define USE_CMS
 
+#undef USE_CRSF_V3
+
 #undef USE_OSD
 #undef USE_MAX7456
 #undef USE_RCDEVICE
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index b26f72230c..e1eff2cf90 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -1,21 +1,18 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
  *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
  *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight 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.
  *
  * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see <http://www.gnu.org/licenses/>.
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
  */
 
 #include <stdbool.h>
@@ -46,11 +43,11 @@
 #include "drivers/persistent.h"
 
 #include "fc/rc_modes.h"
-#include "fc/runtime_config.h"
 #include "fc/rc_adjustments.h"
+#include "fc/runtime_config.h"
 
 #include "flight/imu.h"
-#include "flight/motors.h"
+#include "flight/mixer.h"
 #include "flight/position.h"
 #include "flight/governor.h"
 
@@ -65,12 +62,11 @@
 #include "rx/crsf.h"
 #include "rx/crsf_protocol.h"
 
-#include "scheduler/scheduler.h"
-
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
-#include "sensors/adcinternal.h"
-#include "sensors/esc_sensor.h"
+#include "sensors/acceleration.h"
+
+#include "scheduler/scheduler.h"
 
 #include "telemetry/telemetry.h"
 #include "telemetry/msp_shared.h"
@@ -78,39 +74,138 @@
 #include "crsf.h"
 
 
-#define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
 #define CRSF_DEVICEINFO_VERSION             0x01
 #define CRSF_DEVICEINFO_PARAMETER_COUNT     0
 
-#define CRSF_MSP_BUFFER_SIZE 96
-#define CRSF_MSP_LENGTH_OFFSET 1
+#define CRSF_MSP_BUFFER_SIZE                128
+#define CRSF_MSP_LENGTH_OFFSET              1
 
-static bool crsfTelemetryEnabled;
-static bool deviceInfoReplyPending;
-static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
+#define CRSF_HEARTBEAT_RATE                 10
+#define CRSF_HEARTBEAT_PERIOD               (1000000 / CRSF_HEARTBEAT_RATE)
 
-#if defined(USE_MSP_OVER_TELEMETRY)
-typedef struct mspBuffer_s {
-    uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
-    int len;
-} mspBuffer_t;
+enum {
+    TELEMETRY_STATE_OFF = 0,
+    TELEMETRY_STATE_NATIVE,
+    TELEMETRY_STATE_CUSTOM,
+    TELEMETRY_STATE_POPULATE,
+};
+
+static uint8_t crsfTelemetryState;
+
+#if defined(USE_CRSF_V3)
+static float crsfHeartBeatRateBucket;
+#endif
+
+static float crsfTelemetryRateBucket;
+static float crsfTelemetryRateQuanta;
+
+static uint8_t crsfTelemetryFrameId;
+static timeUs_t crsfTelemetryUpdateTime;
+
+static bool deviceInfoReplyPending = false;
+
+static sbuf_t crsfSbuf;
+static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX + 32];
+
+
+bool checkCrsfTelemetryState(void)
+{
+    return (crsfTelemetryState != TELEMETRY_STATE_OFF);
+}
+
+static inline size_t crsfLinkFrameSlots(size_t bytes)
+{
+    // Telemetry data is send in 5 byte slots, with 1 slot overhead
+    return (bytes + 9) / 5;
+}
+
+static inline void crsfTelemetryRateConsume(size_t slots)
+{
+    crsfTelemetryRateBucket -= slots;
+#if defined(USE_CRSF_V3)
+    crsfHeartBeatRateBucket -= 1;
+#endif
+}
 
-static mspBuffer_t mspRxBuffer;
+static void crsfTelemetryRateUpdate(timeUs_t currentTimeUs)
+{
+    timeDelta_t delta = cmpTimeUs(currentTimeUs, crsfTelemetryUpdateTime);
 
 #if defined(USE_CRSF_V3)
+    crsfHeartBeatRateBucket += (1e-6f * CRSF_HEARTBEAT_RATE) * delta;
+    crsfHeartBeatRateBucket = constrainf(crsfHeartBeatRateBucket, -2, 1);
+#endif
+    crsfTelemetryRateBucket += crsfTelemetryRateQuanta * delta;
+    crsfTelemetryRateBucket = constrainf(crsfTelemetryRateBucket, -25, 1);
+
+    crsfTelemetryUpdateTime = currentTimeUs;
+
+    telemetryScheduleUpdate(currentTimeUs);
+}
+
+static bool crsfCanTransmitTelemetry(void)
+{
+    return (crsfTelemetryRateBucket >= 0) &&
+          !(getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME);
+}
+
+static size_t crsfSbufLen(sbuf_t *buf)
+{
+    return buf->ptr - crsfFrame;
+}
+
+static sbuf_t * crsfInitializeSbuf(void)
+{
+    sbuf_t * dst = &crsfSbuf;
+
+    dst->ptr = crsfFrame;
+    dst->end = crsfFrame + CRSF_FRAME_SIZE_MAX;
+
+    sbufWriteU8(dst, CRSF_SYNC_BYTE);
+    sbufWriteU8(dst, CRSF_FRAME_LENGTH_TYPE_CRC);  // placeholder
+
+    return dst;
+}
+
+static size_t crsfTransmitSbuf(sbuf_t *dst)
+{
+    // Frame length including CRC
+    const size_t frameLength = crsfSbufLen(dst) + 1;
+
+    if (frameLength <= CRSF_FRAME_SIZE_MAX)
+    {
+        // Set frame length (without device address and frame length) into the placeholder
+        crsfFrame[1] = frameLength - 2;
+
+        // CRC does not include device address and frame length
+        crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]);
+
+        // Transmit the telemetry frame to the receiver
+        crsfRxTransmitTelemetryData(crsfFrame, frameLength);
+
+        // Consume telemetry rate
+        crsfTelemetryRateConsume(crsfLinkFrameSlots(frameLength));
+
+        return frameLength;
+    }
 
-#define CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US 20000 // 20ms
+    return 0;
+}
+
+#if defined(USE_CRSF_V3)
 
 static bool isCrsfV3Running = false;
+
 typedef struct {
-    uint8_t hasPendingReply:1;
-    uint8_t isNewSpeedValid:1;
+    bool hasPendingReply;
+    bool isNewSpeedValid;
     uint8_t portID:3;
     uint8_t index;
-    uint32_t confirmationTime;
-} crsfSpeedControl_s;
+    timeUs_t confirmationTime;
+} crsfSpeedControl_t;
+
+static crsfSpeedControl_t crsfSpeed = INIT_ZERO;
 
-static crsfSpeedControl_s crsfSpeed = {0};
 
 uint32_t getCrsfCachedBaudrate(void)
 {
@@ -126,7 +221,7 @@ uint32_t getCrsfCachedBaudrate(void)
 
 bool checkCrsfCustomizedSpeed(void)
 {
-    return crsfSpeed.index < BAUD_COUNT ? true : false;
+    return crsfSpeed.index < BAUD_COUNT;
 }
 
 uint32_t getCrsfDesiredSpeed(void)
@@ -148,380 +243,196 @@ bool crsfBaudNegotiationInProgress(void)
 {
     return crsfSpeed.hasPendingReply || crsfSpeed.isNewSpeedValid;
 }
-#endif
 
-void initCrsfMspBuffer(void)
+#endif /* USE_CRSF_V3 */
+
+
+#if defined(USE_MSP_OVER_TELEMETRY)
+
+static bool mspRespPending = false;
+
+static uint8_t mspRequestOriginID = 0;
+
+static uint8_t mspRequestDataBuffer[CRSF_MSP_BUFFER_SIZE];
+
+static int mspRequestDataLength = 0;
+
+
+static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
+{
+    sbuf_t *dst = crsfInitializeSbuf();
+    sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
+    sbufWriteU8(dst, mspRequestOriginID);
+    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+    sbufWriteData(dst, payload, payloadSize);
+    crsfTransmitSbuf(dst);
+    crsfTelemetryRateConsume(2);
+}
+
+void crsfScheduleMspResponse(uint8_t requestOriginID)
 {
-    mspRxBuffer.len = 0;
+    mspRequestOriginID = requestOriginID;
 }
 
 bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
 {
-    if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) {
-        return false;
-    } else {
-        uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len;
+    if (mspRequestDataLength + CRSF_MSP_LENGTH_OFFSET + frameLength <= CRSF_MSP_BUFFER_SIZE) {
+        uint8_t *p = mspRequestDataBuffer + mspRequestDataLength;
         *p++ = frameLength;
         memcpy(p, frameStart, frameLength);
-        mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
+        mspRequestDataLength += CRSF_MSP_LENGTH_OFFSET + frameLength;
         return true;
     }
+    return false;
 }
 
 bool handleCrsfMspFrameBuffer(mspResponseFnPtr responseFn)
 {
-    static bool replyPending = false;
-    if (replyPending) {
-        if (crsfRxIsTelemetryBufEmpty()) {
-            replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
-        }
-        return replyPending;
-    }
-    if (!mspRxBuffer.len) {
-        return false;
-    }
-    int pos = 0;
-    while (true) {
-        const uint8_t mspFrameLength = mspRxBuffer.bytes[pos];
-        if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
-            if (crsfRxIsTelemetryBufEmpty()) {
-                replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
-            } else {
-                replyPending = true;
+    if (!mspRespPending) {
+        bool loop = true;
+        int pos = 0;
+
+        do {
+            ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
+                loop = (pos < mspRequestDataLength);
+            }
+            if (loop) {
+                const uint8_t mspFrameLength = mspRequestDataBuffer[pos];
+                mspRespPending = handleMspFrame(&mspRequestDataBuffer[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL);
+                pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
             }
         }
-        pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
+        while (loop && !mspRespPending);
+
         ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
-            if (pos >= mspRxBuffer.len) {
-                mspRxBuffer.len = 0;
-                return replyPending;
-            }
+            mspRequestDataLength = 0;
         }
     }
-    return replyPending;
-}
-#endif
 
-static void crsfInitializeFrame(sbuf_t *dst)
-{
-    dst->ptr = crsfFrame;
-    dst->end = ARRAYEND(crsfFrame);
+    if (mspRespPending) {
+        mspRespPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
+        return true;
+    }
 
-    sbufWriteU8(dst, CRSF_SYNC_BYTE);
+    return false;
 }
+#endif /* USE_MSP_OVER_TELEMETRY */
 
-static void crsfFinalize(sbuf_t *dst)
-{
-    crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
-    sbufSwitchToReader(dst, crsfFrame);
-    // write the telemetry frame to the receiver.
-    crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
-}
 
 /*
-CRSF frame has the structure:
-<Device address> <Frame length> <Type> <Payload> <CRC>
-Device address: (uint8_t)
-Frame length:   length in  bytes including Type (uint8_t)
-Type:           (uint8_t)
-CRC:            (uint8_t), crc of <Type> and <Payload>
-*/
+ * CRSF frame has the structure:
+ * <Device address> <Frame length> <Type> <Payload> <CRC>
+ * Device address: (uint8_t)
+ * Frame length:   length in  bytes including Type (uint8_t)
+ * Type:           (uint8_t)
+ * CRC:            (uint8_t), crc of <Type> and <Payload>
+ */
 
 /*
-0x02 GPS
-Payload:
-int32_t     Latitude ( degree / 10`000`000 )
-int32_t     Longitude (degree / 10`000`000 )
-uint16_t    Groundspeed ( km/h / 10 )
-uint16_t    GPS heading ( degree / 100 )
-uint16      Altitude ( meter ­1000m offset )
-uint8_t     Satellites in use ( counter )
-*/
-
-static int getVoltageMeter(voltageMeterId_e id)
-{
-    voltageMeter_t meter;
-
-    voltageMeterRead(id, &meter);
-
-    // Use ratio 200 in EdgeTx 2.9.3 and 20 in earlier versions
-    // Max voltage 25.5V
-    return meter.voltage * 255 / 200;
-}
-
-static int16_t crsfGpsReuse(uint8_t reuse, int16_t value)
-{
-    escSensorData_t *escData;
-
-    switch (reuse) {
-        case CRSF_GPS_REUSE_NONE:
-            return value;
-        case CRSF_GPS_REUSE_HEADSPEED:
-            return getHeadSpeed();
-        case CRSF_GPS_REUSE_THROTTLE:
-            return getGovernorOutput() * 1000;
-        case CRSF_GPS_REUSE_ESC_TEMP:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->temperature : 0;
-        case CRSF_GPS_REUSE_ESC_PWM:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->pwm : 0;
-        case CRSF_GPS_REUSE_ESC_THROTTLE:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->throttle : 0;
-        case CRSF_GPS_REUSE_ESC_BEC_VOLTAGE:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->bec_voltage : 0;
-        case CRSF_GPS_REUSE_ESC_BEC_CURRENT:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->bec_current : 0;
-        case CRSF_GPS_REUSE_ESC_BEC_TEMP:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->temperature2 : 0;
-        case CRSF_GPS_REUSE_ESC_STATUS:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? (escData->status & 0xFFFF) : 0;
-        case CRSF_GPS_REUSE_ESC_STATUS2:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? (escData->status >> 16) : 0;
-        case CRSF_GPS_REUSE_MCU_TEMP:
-            return getCoreTemperatureCelsius() * 10;
-        case CRSF_GPS_REUSE_MCU_LOAD:
-            return getAverageCPULoad();
-        case CRSF_GPS_REUSE_SYS_LOAD:
-            return getAverageSystemLoad();
-        case CRSF_GPS_REUSE_RT_LOAD:
-            return getMaxRealTimeLoad();
-        case CRSF_GPS_REUSE_BEC_VOLTAGE:
-            return getVoltageMeter(VOLTAGE_METER_ID_BEC);
-        case CRSF_GPS_REUSE_BUS_VOLTAGE:
-            return getVoltageMeter(VOLTAGE_METER_ID_BUS);
-        case CRSF_GPS_REUSE_MCU_VOLTAGE:
-            return getVoltageMeter(VOLTAGE_METER_ID_MCU);
-    }
-
-    return 0;
-}
-
-static int16_t crsfGpsAltitudeReuse(uint8_t reuse, int32_t altitude)
-{
-    escSensorData_t *escData;
-
-    switch (reuse) {
-        case CRSF_GPS_REUSE_NONE:
-            return constrain(altitude / 100, -1000, 5000);   // constrain altitude from -1000 to 5,000m
-        case CRSF_GPS_REUSE_HEADSPEED:
-            return getHeadSpeed();
-        case CRSF_GPS_REUSE_THROTTLE:
-            return getGovernorOutput() * 100;
-        case CRSF_GPS_REUSE_ESC_TEMP:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->temperature / 10 : 0;
-        case CRSF_GPS_REUSE_MCU_TEMP:
-            return getCoreTemperatureCelsius();
-        case CRSF_GPS_REUSE_MCU_LOAD:
-            return getAverageCPULoadPercent();
-        case CRSF_GPS_REUSE_SYS_LOAD:
-            return getAverageSystemLoadPercent();
-        case CRSF_GPS_REUSE_RT_LOAD:
-            return getMaxRealTimeLoadPercent();
-    }
-
-    return 0;
+ * 0x02 GPS
+ * Payload:
+ * int32_t     Latitude ( degree / 10`000`000 )
+ * int32_t     Longitude (degree / 10`000`000 )
+ * uint16_t    Groundspeed ( km/h / 10 )
+ * uint16_t    GPS heading ( degree / 100 )
+ * uint16      Altitude ( meter ­1000m offset )
+ * uint8_t     Satellites in use ( counter )
+ */
+static void crsfFrameGps(sbuf_t *dst)
+{
+    sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
+    sbufWriteS32BE(dst, gpsSol.llh.lat);
+    sbufWriteS32BE(dst, gpsSol.llh.lon);
+    sbufWriteU16BE(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // cm/s
+    sbufWriteU16BE(dst, gpsSol.groundCourse * 10); // degrees * 10
+    sbufWriteU16BE(dst, getEstimatedAltitudeCm() / 100 + 1000);
+    sbufWriteU8(dst, gpsSol.numSat);
 }
 
-static int8_t crsfGpsSatsReuse(uint8_t reuse, int8_t value)
-{
-    escSensorData_t *escData;
-
-    switch (reuse) {
-        case CRSF_GPS_SATS_REUSE_NONE:
-            return value;
-        case CRSF_GPS_SATS_REUSE_ESC_TEMP:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? MAX(escData->temperature, 0) / 10 : 0;
-        case CRSF_GPS_SATS_REUSE_MCU_TEMP:
-            return MAX(getCoreTemperatureCelsius(), 0);
-        case CRSF_GPS_SATS_REUSE_PROFILE:
-            return getCurrentPidProfileIndex() + 1;
-        case CRSF_GPS_SATS_REUSE_RATE_PROFILE:
-            return getCurrentControlRateProfileIndex() + 1;
-        case CRSF_GPS_SATS_REUSE_MODEL_ID:
-            return pilotConfig()->modelId;
-        case CRSF_GPS_SATS_REUSE_LED_PROFILE:
-#ifdef USE_LED_STRIP
-            return getLedProfile() + 1;
-#else
-            return 0;
-#endif
-    }
-
-    return 0;
+/*
+ * 0x07 Variometer sensor
+ * Payload:
+ * int16_t     Variometer
+*/
+static void crsfFrameVarioSensor(sbuf_t *dst)
+{
+    sbufWriteU8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
+    sbufWriteS16BE(dst, getEstimatedVarioCms());
 }
 
-void crsfFrameGps(sbuf_t *dst)
+/*
+ * 0x08 Battery sensor
+ * Payload:
+ * uint16_t    Voltage (100mV steps)
+ * uint16_t    Current (100mA steps)
+ * uint24_t    Fuel (drawn mAh)
+ * uint8_t     Battery remaining (percent)
+*/
+static void crsfFrameBatterySensor(sbuf_t *dst)
 {
-    // use sbufWrite since CRC does not include frame length
-    sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-    sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
-    sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
-    sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
-    sbufWriteU16BigEndian(dst, crsfGpsReuse(telemetryConfig()->crsf_gps_ground_speed_reuse,
-        (gpsSol.groundSpeed * 36 + 50) / 100)); // gpsSol.groundSpeed is in cm/s
-    sbufWriteU16BigEndian(dst, crsfGpsReuse(telemetryConfig()->crsf_gps_heading_reuse,
-        gpsSol.groundCourse * 10)); // gpsSol.groundCourse is degrees * 10
-    sbufWriteU16BigEndian(dst, crsfGpsAltitudeReuse(telemetryConfig()->crsf_gps_altitude_reuse,
-        getEstimatedAltitudeCm()) + 1000);
-    sbufWriteU8(dst, crsfGpsSatsReuse(telemetryConfig()->crsf_gps_sats_reuse, gpsSol.numSat));
+    sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
+    sbufWriteU16BE(dst, getLegacyBatteryVoltage());
+    sbufWriteU16BE(dst, getLegacyBatteryCurrent());
+    sbufWriteU24BE(dst, getBatteryCapacityUsed());
+    sbufWriteU8(dst, calculateBatteryPercentageRemaining());
 }
 
 /*
-0x08 Battery sensor
-Payload:
-uint16_t    Voltage ( mV * 100 )
-uint16_t    Current ( mA * 100 )
-uint24_t    Fuel ( drawn mAh )
-uint8_t     Battery remaining ( percent )
+ * 0x09 Baro altitude sensor
+ * Payload:
+ * uint16_t    Altitude (dm)
+ * int16_t     Variometer
 */
-void crsfFrameBatterySensor(sbuf_t *dst)
+static void crsfFrameAltitudeSensor(sbuf_t *dst)
 {
-    // use sbufWrite since CRC does not include frame length
-    sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
-    sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
-    if (telemetryConfig()->report_cell_voltage) {
-        sbufWriteU16BigEndian(dst, (getBatteryAverageCellVoltage() + 5) / 10); // vbat is in units of 0.01V
-    } else {
-        sbufWriteU16BigEndian(dst, getLegacyBatteryVoltage());
-    }
-    sbufWriteU16BigEndian(dst, getLegacyBatteryCurrent());
-    const uint32_t mAhDrawn = getBatteryCapacityUsed();
-    const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining();
-    sbufWriteU8(dst, (mAhDrawn >> 16));
-    sbufWriteU8(dst, (mAhDrawn >> 8));
-    sbufWriteU8(dst, (uint8_t)mAhDrawn);
-    sbufWriteU8(dst, batteryRemainingPercentage);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_ALTITUDE_SENSOR);
+    sbufWriteU16BE(dst, getEstimatedAltitudeCm() / 10 + 10000);
+    sbufWriteS16BE(dst, getEstimatedVarioCms());
 }
 
 /*
-0x0B Heartbeat
-Payload:
-int16_t    origin_add ( Origin Device address )
+ * 0x0B Heartbeat
+ * Payload:
+ * int16_t    Origin Device address
 */
-void crsfFrameHeartbeat(sbuf_t *dst)
+static void crsfFrameHeartbeat(sbuf_t *dst)
 {
-    sbufWriteU8(dst, CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
     sbufWriteU8(dst, CRSF_FRAMETYPE_HEARTBEAT);
-    sbufWriteU16BigEndian(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
-}
-
-typedef enum {
-    CRSF_ACTIVE_ANTENNA1 = 0,
-    CRSF_ACTIVE_ANTENNA2 = 1
-} crsfActiveAntenna_e;
-
-typedef enum {
-    CRSF_RF_MODE_4_HZ = 0,
-    CRSF_RF_MODE_50_HZ = 1,
-    CRSF_RF_MODE_150_HZ = 2
-} crsrRfMode_e;
-
-typedef enum {
-    CRSF_RF_POWER_0_mW = 0,
-    CRSF_RF_POWER_10_mW = 1,
-    CRSF_RF_POWER_25_mW = 2,
-    CRSF_RF_POWER_100_mW = 3,
-    CRSF_RF_POWER_500_mW = 4,
-    CRSF_RF_POWER_1000_mW = 5,
-    CRSF_RF_POWER_2000_mW = 6,
-    CRSF_RF_POWER_250_mW = 7,
-    CRSF_RF_POWER_50_mW = 8
-} crsrRfPower_e;
+    sbufWriteU16BE(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+}
 
 /*
-0x1E Attitude
-Payload:
-int16_t     Pitch angle ( rad / 10000 )
-int16_t     Roll angle ( rad / 10000 )
-int16_t     Yaw angle ( rad / 10000 )
-*/
+ * 0x1E Attitude
+ * Payload:
+ * int16_t     Pitch angle (rad / 10000)
+ * int16_t     Roll angle (rad / 10000)
+ * int16_t     Yaw angle (rad / 10000)
+ */
 
-// convert andgle in decidegree to radians/10000 with reducing angle to +/-180 degree range
+// convert angle in decidegree to radians/10000 with +/-180 degree range
 static int16_t decidegrees2Radians10000(int16_t angle_decidegree)
 {
-    while (angle_decidegree > 1800) {
+    while (angle_decidegree > 1800)
         angle_decidegree -= 3600;
-    }
-    while (angle_decidegree < -1800) {
+    while (angle_decidegree < -1800)
         angle_decidegree += 3600;
-    }
-    return (int16_t)(RAD * 1000.0f * angle_decidegree);
-}
-
-static int16_t crsfAttitudeReuse(uint8_t reuse, int attitude)
-{
-    escSensorData_t *escData;
-
-    switch (reuse) {
-        case CRSF_ATT_REUSE_NONE:
-            return decidegrees2Radians10000(attitude);
-        case CRSF_ATT_REUSE_THROTTLE:
-            return getGovernorOutput() * 10000;
-        case CRSF_ATT_REUSE_ESC_TEMP:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->temperature * 10 : 0;
-        case CRSF_ATT_REUSE_ESC_PWM:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->pwm : 0;
-        case CRSF_ATT_REUSE_ESC_BEC_VOLTAGE:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->bec_voltage : 0;
-        case CRSF_ATT_REUSE_ESC_BEC_CURRENT:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->bec_current : 0;
-        case CRSF_ATT_REUSE_ESC_BEC_TEMP:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? escData->temperature2 * 10 : 0;
-        case CRSF_ATT_REUSE_ESC_STATUS:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? (escData->status & 0xFFFF) : 0;
-        case CRSF_ATT_REUSE_ESC_STATUS2:
-            escData = getEscSensorData(ESC_SENSOR_COMBINED);
-            return (escData) ? (escData->status >> 16) : 0;
-        case CRSF_ATT_REUSE_MCU_TEMP:
-            return getCoreTemperatureCelsius() * 100;
-        case CRSF_ATT_REUSE_MCU_LOAD:
-            return getAverageCPULoad() * 10;
-        case CRSF_ATT_REUSE_SYS_LOAD:
-            return getAverageSystemLoad() * 10;
-        case CRSF_ATT_REUSE_RT_LOAD:
-            return getMaxRealTimeLoad() * 10;
-        case CRSF_ATT_REUSE_BEC_VOLTAGE:
-            return getVoltageMeter(VOLTAGE_METER_ID_BEC);
-        case CRSF_ATT_REUSE_BUS_VOLTAGE:
-            return getVoltageMeter(VOLTAGE_METER_ID_BUS);
-        case CRSF_ATT_REUSE_MCU_VOLTAGE:
-            return getVoltageMeter(VOLTAGE_METER_ID_MCU);
-    }
-
-    return 0;
+    return RAD * 1000 * angle_decidegree;
 }
 
 // fill dst buffer with crsf-attitude telemetry frame
 void crsfFrameAttitude(sbuf_t *dst)
 {
-    sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
     sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE);
-    sbufWriteU16BigEndian(dst, crsfAttitudeReuse(telemetryConfig()->crsf_att_pitch_reuse, attitude.values.pitch));
-    sbufWriteU16BigEndian(dst, crsfAttitudeReuse(telemetryConfig()->crsf_att_roll_reuse, attitude.values.roll));
-    sbufWriteU16BigEndian(dst, crsfAttitudeReuse(telemetryConfig()->crsf_att_yaw_reuse, attitude.values.yaw));
+    sbufWriteS16BE(dst, decidegrees2Radians10000(attitude.values.pitch));
+    sbufWriteS16BE(dst, decidegrees2Radians10000(attitude.values.roll));
+    sbufWriteS16BE(dst, decidegrees2Radians10000(attitude.values.yaw));
 }
 
 /*
-0x21 Flight mode text based
-Payload:
-char[]      Flight mode ( Null terminated string )
-*/
-
+ * 0x21 Flight mode text based
+ * Payload:
+ * char[]      Flight mode (Null terminated string)
+ */
 static void crsfFlightModeInfo(char *buf)
 {
     const char *flightMode = "-";
@@ -567,7 +478,7 @@ static const char * govStateNames[] = {
     "BAILOUT",
 };
 
-static void crsfGovernorInfo(char *buf)
+void crsfGovernorInfo(char *buf)
 {
     // Modes that are only relevant when disarmed
     if (!ARMING_FLAG(ARMED)) {
@@ -581,175 +492,357 @@ static void crsfGovernorInfo(char *buf)
     }
 }
 
-static void crsfHeadspeedInfo(char *buf)
+void crsfFrameFlightMode(sbuf_t *dst)
 {
-    int val = getHeadSpeed();
-    tfp_sprintf(buf, "%d", val);
+    char buff[32] = INIT_ZERO;
+
+    crsfFlightModeInfo(buff);
+    //crsfGovernorInfo(buff);
+
+    sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
+    sbufWriteStringWithZeroTerminator(dst, buff);
 }
 
-static void crsfThrottleInfo(char *buf)
+/*
+ * 0x29 Device Info
+ * Payload:
+ * uint8_t     Destination
+ * uint8_t     Origin
+ * char[]      Device Name (Null terminated string)
+ * uint32_t    Null Bytes
+ * uint32_t    Null Bytes
+ * uint32_t    Null Bytes
+ * uint8_t     255 (Max MSP Parameter)
+ * uint8_t     0x01 (Parameter version 1)
+ */
+static void crsfFrameDeviceInfo(sbuf_t *dst)
 {
-    int val = lrintf(getGovernorOutput() * 100);
-    tfp_sprintf(buf, "%d", val);
+    char buff[30];
+
+    tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
+
+    sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
+    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+    sbufWriteStringWithZeroTerminator(dst, buff);
+    sbufWriteU32(dst, 0);
+    sbufWriteU32(dst, 0);
+    sbufWriteU32(dst, 0);
+    sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
+    sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
 }
 
-static void crsfMCUTempInfo(char *buf)
+
+/*
+ * 0x88 Custom telemetry
+ * Payload:
+ * uint16_t    Sensor id
+ * <type>      Sensor data
+ * ...
+ */
+
+void crsfSensorEncodeNil(sbuf_t *buf, telemetrySensor_t *sensor)
 {
-    int val = getCoreTemperatureCelsius();
-    tfp_sprintf(buf, "%d", val);
+    UNUSED(buf);
+    UNUSED(sensor);
 }
 
-static void crsfMCULoadInfo(char *buf)
+void crsfSensorEncodeU8(sbuf_t *buf, telemetrySensor_t *sensor)
 {
-    int val = getAverageCPULoadPercent();
-    tfp_sprintf(buf, "%d", val);
+    sbufWriteU8(buf, constrain(sensor->value, 0, 0xFF));
 }
 
-static void crsfSysLoadInfo(char *buf)
+void crsfSensorEncodeS8(sbuf_t *buf, telemetrySensor_t *sensor)
 {
-    int val = getAverageSystemLoadPercent();
-    tfp_sprintf(buf, "%d", val);
+    sbufWriteS8(buf, constrain(sensor->value, -0x80, 0x7F));
 }
 
-static void crsfRTLoadInfo(char *buf)
+void crsfSensorEncodeU16(sbuf_t *buf, telemetrySensor_t *sensor)
 {
-    int val = getMaxRealTimeLoadPercent();
-    tfp_sprintf(buf, "%d", val);
+    sbufWriteU16BE(buf, constrain(sensor->value, 0, 0xFFFF));
 }
 
-static void crsfESCTempInfo(char *buf)
+void crsfSensorEncodeS16(sbuf_t *buf, telemetrySensor_t *sensor)
 {
-    escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
-    if (escData) {
-        int val = escData->temperature / 10;
-        tfp_sprintf(buf, "%d", val);
-    }
+    sbufWriteS16BE(buf, constrain(sensor->value, -0x8000, 0x7FFF));
 }
 
-static void crsfVoltageMetereInfo(char *buf, voltageMeterId_e id)
+void crsfSensorEncodeU24(sbuf_t *buf, telemetrySensor_t *sensor)
 {
-    voltageMeter_t meter;
+    sbufWriteU24BE(buf, constrain(sensor->value, 0, 0xFFFFFF));
+}
 
-    if (voltageMeterRead(id, &meter)) {
-        int val = meter.voltage / 10;
-        tfp_sprintf(buf, "%d.%02d", val / 100, val % 100);
-    }
+void crsfSensorEncodeS24(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    sbufWriteS24BE(buf, constrain(sensor->value, -0x800000, 0x7FFFFF));
 }
 
-static void crsfAdjFuncInfo(char *buf)
+void crsfSensorEncodeU32(sbuf_t *buf, telemetrySensor_t *sensor)
 {
-    if (getAdjustmentsRangeName()) {
-        int fun = getAdjustmentsRangeFunc();
-        int val = getAdjustmentsRangeValue();
-        tfp_sprintf(buf, "%d:%d", fun, val);
+    sbufWriteU32BE(buf, sensor->value);
+}
+
+void crsfSensorEncodeS32(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    sbufWriteS32BE(buf, sensor->value);
+}
+
+void crsfSensorEncodeCellVolt(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    const int volt = constrain(sensor->value, 200, 455) - 200;
+    sbufWriteU8(buf, volt);
+}
+
+void crsfSensorEncodeCells(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    UNUSED(sensor);
+    const int cells = MIN(getBatteryCellCount(), 16);
+    sbufWriteU8(buf, cells);
+    for (int i = 0; i < cells; i++) {
+        int volt = constrain(getBatteryCellVoltage(i), 200, 455) - 200;
+        sbufWriteU8(buf, volt);
     }
 }
 
-static void crsfGovAdjFuncInfo(char *buf)
+void crsfSensorEncodeControl(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    UNUSED(sensor);
+    const int p = lrintf(mixerGetInput(MIXER_IN_STABILIZED_PITCH) * 1200);
+    const int r = lrintf(mixerGetInput(MIXER_IN_STABILIZED_ROLL) * 1200);
+    const int y = lrintf(mixerGetInput(MIXER_IN_STABILIZED_YAW) * 2400);
+    const int c = lrintf(mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE) * 1200);
+    sbufWriteU8(buf, ((p >> 8) & 0x0F) | ((r >> 4) & 0xF0));
+    sbufWriteU8(buf, (p & 0xFF));
+    sbufWriteU8(buf, (r & 0xFF));
+    sbufWriteU8(buf, ((y >> 8) & 0x0F) | ((c >> 4) & 0xF0));
+    sbufWriteU8(buf, (y & 0xFF));
+    sbufWriteU8(buf, (c & 0xFF));
+}
+
+void crsfSensorEncodeAttitude(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    UNUSED(sensor);
+    sbufWriteS16BE(buf, attitude.values.pitch);
+    sbufWriteS16BE(buf, attitude.values.roll);
+    sbufWriteS16BE(buf, attitude.values.yaw);
+}
+
+void crsfSensorEncodeAccel(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    UNUSED(sensor);
+    sbufWriteS16BE(buf, acc.accADC[0] * acc.dev.acc_1G_rec * 100);
+    sbufWriteS16BE(buf, acc.accADC[1] * acc.dev.acc_1G_rec * 100);
+    sbufWriteS16BE(buf, acc.accADC[2] * acc.dev.acc_1G_rec * 100);
+}
+
+void crsfSensorEncodeLatLong(sbuf_t *buf, telemetrySensor_t *sensor)
 {
+    UNUSED(sensor);
+    sbufWriteS32BE(buf, gpsSol.llh.lat);
+    sbufWriteS32BE(buf, gpsSol.llh.lon);
+}
+
+void crsfSensorEncodeAdjFunc(sbuf_t *buf, telemetrySensor_t *sensor)
+{
+    UNUSED(sensor);
     if (getAdjustmentsRangeName()) {
-        int fun = getAdjustmentsRangeFunc();
-        int val = getAdjustmentsRangeValue();
-        tfp_sprintf(buf, "%d:%d", fun, val);
+        sbufWriteU16BE(buf, getAdjustmentsRangeFunc());
+        sbufWriteS32BE(buf, getAdjustmentsRangeValue());
     }
     else {
-        crsfGovernorInfo(buf);
+        sbufWriteU16BE(buf, 0);
+        sbufWriteS32BE(buf, 0);
     }
 }
 
-void crsfFrameFlightMode(sbuf_t *dst)
+static void crsfFrameCustomTelemetryHeader(sbuf_t *dst)
 {
-    char buff[32] = { 0, };
+    sbufWriteU8(dst, CRSF_FRAMETYPE_CUSTOM_TELEM);
+    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+    sbufWriteU8(dst, crsfTelemetryFrameId);
+}
 
-    switch (telemetryConfig()->crsf_flight_mode_reuse) {
-        case CRSF_FM_REUSE_GOV_STATE:
-            crsfGovernorInfo(buff);
-            break;
-        case CRSF_FM_REUSE_HEADSPEED:
-            crsfHeadspeedInfo(buff);
-            break;
-        case CRSF_FM_REUSE_THROTTLE:
-            crsfThrottleInfo(buff);
-            break;
-        case CRSF_FM_REUSE_ESC_TEMP:
-            crsfESCTempInfo(buff);
-            break;
-        case CRSF_FM_REUSE_MCU_TEMP:
-            crsfMCUTempInfo(buff);
-            break;
-        case CRSF_FM_REUSE_MCU_LOAD:
-            crsfMCULoadInfo(buff);
-            break;
-        case CRSF_FM_REUSE_SYS_LOAD:
-            crsfSysLoadInfo(buff);
-            break;
-        case CRSF_FM_REUSE_RT_LOAD:
-            crsfRTLoadInfo(buff);
-            break;
-        case CRSF_FM_REUSE_BEC_VOLTAGE:
-            crsfVoltageMetereInfo(buff, VOLTAGE_METER_ID_BEC);
-            break;
-        case CRSF_FM_REUSE_BUS_VOLTAGE:
-            crsfVoltageMetereInfo(buff, VOLTAGE_METER_ID_BUS);
-            break;
-        case CRSF_FM_REUSE_MCU_VOLTAGE:
-            crsfVoltageMetereInfo(buff, VOLTAGE_METER_ID_MCU);
-            break;
-        case CRSF_FM_REUSE_ADJFUNC:
-            crsfAdjFuncInfo(buff);
-            break;
-        case CRSF_FM_REUSE_GOV_ADJFUNC:
-            crsfGovAdjFuncInfo(buff);
-            break;
-        default:
-            crsfFlightModeInfo(buff);
-            break;
+static void crsfFrameCustomTelemetrySensor(sbuf_t *dst, telemetrySensor_t * sensor)
+{
+    sbufWriteU16BE(dst, sensor->tcode);
+    sensor->encode(dst, sensor);
+}
+
+
+#define TLM_SENSOR(NAME, CODE, MINI, MAXI, ENCODER) \
+    { \
+        .telid = TELEM_##NAME, \
+        .tcode = (CODE), \
+        .min_interval = (MINI), \
+        .max_interval = (MAXI), \
+        .bucket = 0, \
+        .value = 0, \
+        .update = 0, \
+        .active = false, \
+        .encode = crsfSensorEncode##ENCODER, \
     }
 
-    uint8_t *lengthPtr = sbufPtr(dst);
-    sbufWriteU8(dst, 0);
-    sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
-    sbufWriteStringWithZeroTerminator(dst, buff);
-    *lengthPtr = sbufPtr(dst) - lengthPtr;
-}
+#define LEGACY_FLIGHT_MODE      (SENSOR_MODE)
+#define LEGACY_BATTERY          (SENSOR_VOLTAGE | SENSOR_CURRENT | SENSOR_FUEL | SENSOR_CAP_USED)
+#define LEGACY_ATTITUDE         (SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)
+#define LEGACY_ALTITUDE         (SENSOR_ALTITUDE | SENSOR_VARIO)
+#define LEGACY_GPS              (SENSOR_LAT_LONG | SENSOR_GROUND_SPEED)
 
-/*
-0x29 Device Info
-Payload:
-uint8_t     Destination
-uint8_t     Origin
-char[]      Device Name ( Null terminated string )
-uint32_t    Null Bytes
-uint32_t    Null Bytes
-uint32_t    Null Bytes
-uint8_t     255 (Max MSP Parameter)
-uint8_t     0x01 (Parameter version 1)
-*/
-void crsfFrameDeviceInfo(sbuf_t *dst)
+static telemetrySensor_t crsfNativeTelemetrySensors[] =
 {
-    char buff[30];
-    tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
+    TLM_SENSOR(FLIGHT_MODE,     LEGACY_FLIGHT_MODE,   100,  100,  Nil),
+    TLM_SENSOR(BATTERY,         LEGACY_BATTERY,       100,  100,  Nil),
+    TLM_SENSOR(ATTITUDE,        LEGACY_ATTITUDE,      100,  100,  Nil),
+    TLM_SENSOR(ALTITUDE,        LEGACY_ALTITUDE,      100,  100,  Nil),
+    TLM_SENSOR(GPS,             LEGACY_GPS,           100,  100,  Nil),
+};
 
-    uint8_t *lengthPtr = sbufPtr(dst);
-    sbufWriteU8(dst, 0);
-    sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
-    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
-    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
-    sbufWriteStringWithZeroTerminator(dst, buff);
-    for (unsigned int ii = 0; ii < 12; ii++) {
-        sbufWriteU8(dst, 0x00);
+static telemetrySensor_t crsfCustomTelemetrySensors[] =
+{
+    TLM_SENSOR(NONE,                    0x0000,  1000,  1000,    Nil),
+    TLM_SENSOR(HEARTBEAT,               0x0001,  1000,  1000,    U16),
+
+    TLM_SENSOR(BATTERY_VOLTAGE,         0x0011,   200,  3000,    U16),
+    TLM_SENSOR(BATTERY_CURRENT,         0x0012,   200,  3000,    U16),
+    TLM_SENSOR(BATTERY_CONSUMPTION,     0x0013,   200,  3000,    U16),
+    TLM_SENSOR(BATTERY_CHARGE_LEVEL,    0x0014,   200,  3000,    U8),
+
+    TLM_SENSOR(BATTERY_CELL_COUNT,      0x0020,   200,  3000,    U8),
+    TLM_SENSOR(BATTERY_CELL_VOLTAGE,    0x0021,   200,  3000,    CellVolt),
+    TLM_SENSOR(BATTERY_CELL_VOLTAGES,   0x002F,   200,  3000,    Cells),
+
+    TLM_SENSOR(CONTROL,                 0x0030,   100,  3000,    Control),
+    TLM_SENSOR(PITCH_CONTROL,           0x0031,   200,  3000,    S16),
+    TLM_SENSOR(ROLL_CONTROL,            0x0032,   200,  3000,    S16),
+    TLM_SENSOR(YAW_CONTROL,             0x0033,   200,  3000,    S16),
+    TLM_SENSOR(COLLECTIVE_CONTROL,      0x0034,   200,  3000,    S16),
+    TLM_SENSOR(THROTTLE_CONTROL,        0x0035,   200,  3000,    S8),
+
+    TLM_SENSOR(ESC1_VOLTAGE,            0x0041,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_CURRENT,            0x0042,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_CAPACITY,           0x0043,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_ERPM,               0x0044,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_POWER,              0x0045,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_THROTTLE,           0x0046,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_TEMP1,              0x0047,   200,  3000,    U8),
+    TLM_SENSOR(ESC1_TEMP2,              0x0048,   200,  3000,    U8),
+    TLM_SENSOR(ESC1_BEC_VOLTAGE,        0x0049,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_BEC_CURRENT,        0x004A,   200,  3000,    U16),
+    TLM_SENSOR(ESC1_STATUS,             0x004E,   200,  3000,    U32),
+    TLM_SENSOR(ESC1_MODEL,              0x004F,   200,  3000,    U8),
+
+    TLM_SENSOR(ESC2_VOLTAGE,            0x0051,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_CURRENT,            0x0052,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_CAPACITY,           0x0053,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_ERPM,               0x0054,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_POWER,              0x0055,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_THROTTLE,           0x0056,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_TEMP1,              0x0057,   200,  3000,    U8),
+    TLM_SENSOR(ESC2_TEMP2,              0x0058,   200,  3000,    U8),
+    TLM_SENSOR(ESC2_BEC_VOLTAGE,        0x0059,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_BEC_CURRENT,        0x005A,   200,  3000,    U16),
+    TLM_SENSOR(ESC2_STATUS,             0x005E,   200,  3000,    U32),
+    TLM_SENSOR(ESC2_MODEL,              0x005F,   200,  3000,    U8),
+
+    TLM_SENSOR(ESC_VOLTAGE,             0x0080,   200,  3000,    U16),
+    TLM_SENSOR(BEC_VOLTAGE,             0x0081,   200,  3000,    U16),
+    TLM_SENSOR(BUS_VOLTAGE,             0x0082,   200,  3000,    U16),
+    TLM_SENSOR(MCU_VOLTAGE,             0x0083,   200,  3000,    U16),
+
+    TLM_SENSOR(ESC_CURRENT,             0x0090,   200,  3000,    U16),
+    TLM_SENSOR(BEC_CURRENT,             0x0091,   200,  3000,    U16),
+    TLM_SENSOR(BUS_CURRENT,             0x0092,   200,  3000,    U16),
+    TLM_SENSOR(MCU_CURRENT,             0x0093,   200,  3000,    U16),
+
+    TLM_SENSOR(ESC_TEMP,                0x00A0,   500,  3000,    U8),
+    TLM_SENSOR(BEC_TEMP,                0x00A1,   500,  3000,    U8),
+    TLM_SENSOR(MCU_TEMP,                0x00A3,   500,  3000,    U8),
+
+    TLM_SENSOR(HEADING,                 0x00B1,   200,  3000,    S16),
+    TLM_SENSOR(ALTITUDE,                0x00B2,   200,  3000,    S24),
+    TLM_SENSOR(VARIOMETER,              0x00B3,   200,  3000,    S16),
+
+    TLM_SENSOR(HEADSPEED,               0x00C0,   200,  3000,    U16),
+    TLM_SENSOR(TAILSPEED,               0x00C1,   200,  3000,    U16),
+
+    TLM_SENSOR(ATTITUDE,                0x0100,   100,  3000,    Attitude),
+    TLM_SENSOR(ATTITUDE_PITCH,          0x0101,   200,  3000,    S16),
+    TLM_SENSOR(ATTITUDE_ROLL,           0x0102,   200,  3000,    S16),
+    TLM_SENSOR(ATTITUDE_YAW,            0x0103,   200,  3000,    S16),
+
+    TLM_SENSOR(ACCEL,                   0x0110,   100,  3000,    Accel),
+    TLM_SENSOR(ACCEL_X,                 0x0111,   200,  3000,    S16),
+    TLM_SENSOR(ACCEL_Y,                 0x0112,   200,  3000,    S16),
+    TLM_SENSOR(ACCEL_Z,                 0x0113,   200,  3000,    S16),
+
+    TLM_SENSOR(GPS_SATS,                0x0121,   500,  3000,    U8),
+    TLM_SENSOR(GPS_PDOP,                0x0122,   500,  3000,    U8),
+    TLM_SENSOR(GPS_HDOP,                0x0123,   500,  3000,    U8),
+    TLM_SENSOR(GPS_VDOP,                0x0124,   500,  3000,    U8),
+    TLM_SENSOR(GPS_COORD,               0x0125,   200,  3000,    LatLong),
+    TLM_SENSOR(GPS_ALTITUDE,            0x0126,   200,  3000,    S16),
+    TLM_SENSOR(GPS_HEADING,             0x0127,   200,  3000,    S16),
+    TLM_SENSOR(GPS_GROUNDSPEED,         0x0128,   200,  3000,    U16),
+    TLM_SENSOR(GPS_HOME_DISTANCE,       0x0129,   200,  3000,    U16),
+    TLM_SENSOR(GPS_HOME_DIRECTION,      0x012A,   200,  3000,    S16),
+
+    TLM_SENSOR(CPU_LOAD,                0x0141,   500,  3000,    U8),
+    TLM_SENSOR(SYS_LOAD,                0x0142,   500,  3000,    U8),
+    TLM_SENSOR(RT_LOAD,                 0x0143,   500,  3000,    U8),
+
+    TLM_SENSOR(MODEL_ID,                0x0200,   200,  3000,    U8),
+    TLM_SENSOR(FLIGHT_MODE,             0x0201,   200,  3000,    U16),
+    TLM_SENSOR(ARMING_FLAGS,            0x0202,   200,  3000,    U8),
+    TLM_SENSOR(ARMING_DISABLE_FLAGS,    0x0203,   200,  3000,    U32),
+    TLM_SENSOR(RESCUE_STATE,            0x0204,   200,  3000,    U8),
+    TLM_SENSOR(GOVERNOR_STATE,          0x0205,   200,  3000,    U8),
+
+    TLM_SENSOR(PID_PROFILE,             0x0211,   200,  3000,    U8),
+    TLM_SENSOR(RATES_PROFILE,           0x0212,   200,  3000,    U8),
+    TLM_SENSOR(LED_PROFILE,             0x0213,   200,  3000,    U8),
+
+    TLM_SENSOR(ADJFUNC,                 0x0220,   200,  3000,    AdjFunc),
+
+    TLM_SENSOR(DEBUG_0,                 0xFE00,   100,  3000,    S32),
+    TLM_SENSOR(DEBUG_1,                 0xFE01,   100,  3000,    S32),
+    TLM_SENSOR(DEBUG_2,                 0xFE02,   100,  3000,    S32),
+    TLM_SENSOR(DEBUG_3,                 0xFE03,   100,  3000,    S32),
+    TLM_SENSOR(DEBUG_4,                 0xFE04,   100,  3000,    S32),
+    TLM_SENSOR(DEBUG_5,                 0xFE05,   100,  3000,    S32),
+    TLM_SENSOR(DEBUG_6,                 0xFE06,   100,  3000,    S32),
+    TLM_SENSOR(DEBUG_7,                 0xFE07,   100,  3000,    S32),
+};
+
+telemetrySensor_t * crsfGetNativeSensor(sensor_id_e id)
+{
+    for (size_t i = 0; i < ARRAYLEN(crsfNativeTelemetrySensors); i++) {
+        telemetrySensor_t * sensor = &crsfNativeTelemetrySensors[i];
+        if (sensor->telid == id)
+            return sensor;
     }
-    sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
-    sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
-    *lengthPtr = sbufPtr(dst) - lengthPtr;
+
+    return NULL;
+}
+
+telemetrySensor_t * crsfGetCustomSensor(sensor_id_e id)
+{
+    for (size_t i = 0; i < ARRAYLEN(crsfCustomTelemetrySensors); i++) {
+        telemetrySensor_t * sensor = &crsfCustomTelemetrySensors[i];
+        if (sensor->telid == id)
+            return sensor;
+    }
+
+    return NULL;
 }
 
 
 #if defined(USE_CRSF_V3)
-void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
+
+static void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
 {
-    uint8_t *lengthPtr = sbufPtr(dst);
-    sbufWriteU8(dst, 0);
+    uint8_t *start = sbufPtr(dst);
+
     sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND);
     sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER);
     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
@@ -757,64 +850,76 @@ void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
     sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_RESPONSE);
     sbufWriteU8(dst, crsfSpeed.portID);
     sbufWriteU8(dst, reply);
-    crc8_poly_0xba_sbuf_append(dst, &lengthPtr[1]);
-    *lengthPtr = sbufPtr(dst) - lengthPtr;
+
+    crc8_poly_0xba_sbuf_append(dst, start);
 }
 
 static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart)
 {
     uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5];
-    uint8_t ii = 0;
-    for (ii = 0; ii < BAUD_COUNT; ++ii) {
-        if (newBaudrate == baudRates[ii]) {
+    unsigned index = 0;
+    for (index = 0; index < BAUD_COUNT; index++) {
+        if (newBaudrate == baudRates[index])
             break;
-        }
     }
     crsfSpeed.portID = frameStart[1];
-    crsfSpeed.index = ii;
+    crsfSpeed.index = index;
 }
 
-void crsfScheduleSpeedNegotiationResponse(void)
+static void crsfScheduleSpeedNegotiationResponse(void)
 {
     crsfSpeed.hasPendingReply = true;
     crsfSpeed.isNewSpeedValid = false;
 }
 
+/* TASK_SPEED_NEGOTIATION @ 100Hz */
 void speedNegotiationProcess(timeUs_t currentTimeUs)
 {
     if (crsfSpeed.hasPendingReply) {
-        bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false;
-        sbuf_t crsfSpeedNegotiationBuf;
-        sbuf_t *dst = &crsfSpeedNegotiationBuf;
-        crsfInitializeFrame(dst);
+        bool found = (crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud();
+        sbuf_t *dst = crsfInitializeSbuf();
         crsfFrameSpeedNegotiationResponse(dst, found);
-        crsfRxSendTelemetryData(); // prevent overwriting previous data
-        crsfFinalize(dst);
-        crsfRxSendTelemetryData();
+        crsfTransmitSbuf(dst);
         crsfSpeed.hasPendingReply = false;
         crsfSpeed.isNewSpeedValid = found;
         crsfSpeed.confirmationTime = currentTimeUs;
-    } else if (crsfSpeed.isNewSpeedValid) {
+    }
+    else if (crsfSpeed.isNewSpeedValid) {
+        // delay 4ms before applying the new baudrate
         if (cmpTimeUs(currentTimeUs, crsfSpeed.confirmationTime) >= 4000) {
-            // delay 4ms before applying the new baudrate
+            crsfSpeed.confirmationTime = currentTimeUs;
             crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
             crsfSpeed.isNewSpeedValid = false;
             isCrsfV3Running = true;
         }
-    } else if (!featureIsEnabled(FEATURE_TELEMETRY) && crsfRxUseNegotiatedBaud()) {
+    }
+    else if (!featureIsEnabled(FEATURE_TELEMETRY) && crsfRxUseNegotiatedBaud()) {
         // Send heartbeat if telemetry is disabled to allow RX to detect baud rate mismatches
-        sbuf_t crsfPayloadBuf;
-        sbuf_t *dst = &crsfPayloadBuf;
-        crsfInitializeFrame(dst);
-        crsfFrameHeartbeat(dst);
-        crsfRxSendTelemetryData(); // prevent overwriting previous data
-        crsfFinalize(dst);
-        crsfRxSendTelemetryData();
+        if (cmpTimeUs(currentTimeUs, crsfSpeed.confirmationTime) >= CRSF_HEARTBEAT_PERIOD) {
+            crsfSpeed.confirmationTime = currentTimeUs;
+            sbuf_t *dst = crsfInitializeSbuf();
+            crsfFrameHeartbeat(dst);
+            crsfTransmitSbuf(dst);
+        }
+    }
+}
+
+void crsfProcessCommand(uint8_t *frameStart)
+{
+    uint8_t cmd = frameStart[0];
+    uint8_t sub = frameStart[1];
+
+    if (cmd == CRSF_COMMAND_SUBCMD_GENERAL && sub == CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL) {
+        crsfProcessSpeedNegotiationCmd(&frameStart[1]);
+        crsfScheduleSpeedNegotiationResponse();
     }
 }
+
 #endif
 
+
 #if defined(USE_CRSF_CMS_TELEMETRY)
+
 #define CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH   50
 #define CRSF_DISPLAYPORT_BATCH_MAX          0x3F
 #define CRSF_DISPLAYPORT_FIRST_CHUNK_MASK   0x80
@@ -862,7 +967,8 @@ static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen)
             } else {
                 break;
             }
-        } else if (destRemaining >= runLength) {
+        }
+        else if (destRemaining >= runLength) {
             sbufWriteU8(dest, c);
             sbufAdvance(source, runLength);
         }
@@ -871,379 +977,397 @@ static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen)
 
 static void crsfFrameDisplayPortChunk(sbuf_t *dst, sbuf_t *src, uint8_t batchId, uint8_t idx)
 {
-    uint8_t *lengthPtr = sbufPtr(dst);
-    sbufWriteU8(dst, 0);
     sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
     sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_UPDATE);
-    uint8_t *metaPtr = sbufPtr(dst);
+
+    uint8_t *ptr = sbufPtr(dst);
     sbufWriteU8(dst, batchId);
     sbufWriteU8(dst, idx);
     cRleEncodeStream(src, dst, CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH);
-    if (idx == 0) {
-        *metaPtr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK;
-    }
-    if (!sbufBytesRemaining(src)) {
-        *metaPtr |= CRSF_DISPLAYPORT_LAST_CHUNK_MASK;
-    }
-    *lengthPtr = sbufPtr(dst) - lengthPtr;
+
+    if (idx == 0)
+        *ptr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK;
+    if (sbufBytesRemaining(src) == 0)
+        *ptr |= CRSF_DISPLAYPORT_LAST_CHUNK_MASK;
 }
 
 static void crsfFrameDisplayPortClear(sbuf_t *dst)
 {
-    uint8_t *lengthPtr = sbufPtr(dst);
-    sbufWriteU8(dst, CRSF_DISPLAY_PORT_COLS_MAX + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
     sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
     sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_CLEAR);
-    *lengthPtr = sbufPtr(dst) - lengthPtr;
 }
 
-#endif
-
-// schedule array to decide how often each type of frame is sent
-typedef enum {
-    CRSF_FRAME_START_INDEX = 0,
-    CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX,
-    CRSF_FRAME_BATTERY_SENSOR_INDEX,
-    CRSF_FRAME_FLIGHT_MODE_INDEX,
-    CRSF_FRAME_GPS_INDEX,
-    CRSF_FRAME_HEARTBEAT_INDEX,
-    CRSF_SCHEDULE_COUNT_MAX
-} crsfFrameTypeIndex_e;
-
-static uint8_t crsfScheduleCount;
-static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
+static bool crsfSendDisplayPortData(void)
+{
+    static uint8_t displayPortBatchId = 0;
 
-#if defined(USE_MSP_OVER_TELEMETRY)
+    if (crsfDisplayPortScreen()->reset) {
+        crsfDisplayPortScreen()->reset = false;
+        sbuf_t *dst = crsfInitializeSbuf();
+        crsfFrameDisplayPortClear(dst);
+        crsfTransmitSbuf(dst);
+        return true;
+    }
 
-static bool mspReplyPending;
-static uint8_t mspRequestOriginID = 0; // origin ID of last msp-over-crsf request. Needed to send response to the origin.
+    if (crsfDisplayPortIsReady() && crsfDisplayPortScreen()->updated)
+    {
+        crsfDisplayPortScreen()->updated = false;
+        uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols;
+        uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer;
+        uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize);
+        sbuf_t displayPortSbuf;
+        sbuf_t *src = sbufInit(&displayPortSbuf, srcStart, srcEnd);
+        displayPortBatchId = (displayPortBatchId  + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
+        for (uint8_t i = 0; sbufBytesRemaining(src); i++) {
+            sbuf_t *dst = crsfInitializeSbuf();
+            crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i);
+            crsfTransmitSbuf(dst);
+        }
+        return true;
+    }
 
-void crsfScheduleMspResponse(uint8_t requestOriginID)
-{
-    mspReplyPending = true;
-    mspRequestOriginID = requestOriginID;
+    return false;
 }
 
-// sends MSP response chunk over CRSF. Must be of type mspResponseFnPtr
-static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
+void crsfProcessDisplayPortCmd(uint8_t *frameStart)
 {
-    sbuf_t crsfPayloadBuf;
-    sbuf_t *dst = &crsfPayloadBuf;
+    const uint8_t cmd = frameStart[0];
 
-    crsfInitializeFrame(dst);
-    sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
-    sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
-    sbufWriteU8(dst, mspRequestOriginID);   // response destination must be the same as request origin in order to response reach proper destination.
-    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
-    sbufWriteData(dst, payload, payloadSize);
-    crsfFinalize(dst);
+    switch (cmd) {
+        case CRSF_DISPLAYPORT_SUBCMD_OPEN:;
+            const uint8_t rows = frameStart[CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET];
+            const uint8_t cols = frameStart[CRSF_DISPLAYPORT_OPEN_COLS_OFFSET];
+            crsfDisplayPortSetDimensions(rows, cols);
+            crsfDisplayPortMenuOpen();
+            break;
+        case CRSF_DISPLAYPORT_SUBCMD_CLOSE:
+            crsfDisplayPortMenuExit();
+            break;
+        case CRSF_DISPLAYPORT_SUBCMD_POLL:
+            crsfDisplayPortRefresh();
+            break;
+        default:
+            break;
+    }
 }
-#endif
 
-static void processCrsf(void)
+#endif /* USE_CRSF_CMS_TELEMETRY */
+
+
+#if defined(USE_RX_EXPRESSLRS)
+
+static int crsfTransmitSbufBuf(sbuf_t *dst, uint8_t *frame)
 {
-    if (!crsfRxIsTelemetryBufEmpty()) {
-        return; // do nothing if telemetry ouptut buffer is not empty yet.
-    }
+    // frame size including CRC
+    const size_t frameLength = crsfSbufLen(dst) + 2;
 
-    static uint8_t crsfScheduleIndex = 0;
+    // Set frame length into the placeholder
+    crsfFrame[1] = frameSize - 3;
 
-    const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
+    // frame CRC
+    crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
 
-    sbuf_t crsfPayloadBuf;
-    sbuf_t *dst = &crsfPayloadBuf;
+    // Copy data to the frame
+    memcpy(frame, crsfFrame, frameSize);
 
-    if (currentSchedule & BIT(CRSF_FRAME_ATTITUDE_INDEX)) {
-        crsfInitializeFrame(dst);
-        crsfFrameAttitude(dst);
-        crsfFinalize(dst);
-    }
-    if (currentSchedule & BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX)) {
-        crsfInitializeFrame(dst);
-        crsfFrameBatterySensor(dst);
-        crsfFinalize(dst);
-    }
+    return frameSize;
+}
 
-    if (currentSchedule & BIT(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
-        crsfInitializeFrame(dst);
-        crsfFrameFlightMode(dst);
-        crsfFinalize(dst);
-    }
-#ifdef USE_GPS
-    if (currentSchedule & BIT(CRSF_FRAME_GPS_INDEX)) {
-        crsfInitializeFrame(dst);
-        crsfFrameGps(dst);
-        crsfFinalize(dst);
-    }
-#endif
+int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
+{
+    sbuf_t *dst = crsfInitializeSbuf();
 
-#if defined(USE_CRSF_V3)
-    if (currentSchedule & BIT(CRSF_FRAME_HEARTBEAT_INDEX)) {
-        crsfInitializeFrame(dst);
-        crsfFrameHeartbeat(dst);
-        crsfFinalize(dst);
-    }
+    switch (frameType) {
+        default:
+        case CRSF_FRAMETYPE_ATTITUDE:
+            crsfFrameAttitude(dst);
+            break;
+        case CRSF_FRAMETYPE_VARIO_SENSOR:
+            crsfFrameVarioSensor(dst);
+            break;
+        case CRSF_FRAMETYPE_ALTITUDE_SENSOR:
+            crsfFrameAltitudeSensor(dst);
+            break;
+        case CRSF_FRAMETYPE_BATTERY_SENSOR:
+            crsfFrameBatterySensor(dst);
+            break;
+        case CRSF_FRAMETYPE_FLIGHT_MODE:
+            crsfFrameFlightMode(dst);
+            break;
+#if defined(USE_GPS)
+        case CRSF_FRAMETYPE_GPS:
+            crsfFrameGps(dst);
+            break;
 #endif
+        case CRSF_FRAMETYPE_DEVICE_INFO:
+            crsfFrameDeviceInfo(dst);
+            break;
+    }
+
+    return crsfTransmitSbufBuf(dst, frame);
+}
 
-    crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
+#if defined(USE_MSP_OVER_TELEMETRY)
+int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize)
+{
+    sbuf_t *dst = crsfInitializeSbuf();
+
+    sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
+    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
+    sbufWriteData(dst, payload, payloadSize);
+
+    return crsfTransmitSbufBuf(dst, frame);
 }
+#endif /* USE_MSP_OVER_TELEMETRY */
+#endif /* USE_RX_EXPRESSLRS */
+
 
 void crsfScheduleDeviceInfoResponse(void)
 {
     deviceInfoReplyPending = true;
 }
 
-void initCrsfTelemetry(void)
+static bool crsfSendDeviceInfoData(void)
 {
-    // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
-    // and feature is enabled, if so, set CRSF telemetry enabled
-    crsfTelemetryEnabled = crsfRxIsActive();
-
-    if (!crsfTelemetryEnabled) {
-        return;
-    }
-
-    deviceInfoReplyPending = false;
-#if defined(USE_MSP_OVER_TELEMETRY)
-    mspReplyPending = false;
-#endif
-
-    int index = 0;
-    if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
-        crsfSchedule[index++] = BIT(CRSF_FRAME_ATTITUDE_INDEX);
-    }
-    if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
-        || (isBatteryCurrentConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
-        crsfSchedule[index++] = BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX);
-    }
-    if (telemetryIsSensorEnabled(SENSOR_MODE)) {
-        crsfSchedule[index++] = BIT(CRSF_FRAME_FLIGHT_MODE_INDEX);
-    }
-#ifdef USE_GPS
-    if ((featureIsEnabled(FEATURE_GPS)
-       && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING))
-       || telemetryConfig()->crsf_gps_ground_speed_reuse
-       || telemetryConfig()->crsf_gps_heading_reuse
-       || telemetryConfig()->crsf_gps_altitude_reuse
-       || telemetryConfig()->crsf_gps_sats_reuse) {
-        crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX);
+    if (deviceInfoReplyPending) {
+        deviceInfoReplyPending = false;
+        sbuf_t *dst = crsfInitializeSbuf();
+        crsfFrameDeviceInfo(dst);
+        crsfTransmitSbuf(dst);
+        return true;
     }
-#endif
+    return false;
+}
 
+static bool crsfSendHeartBeat(void)
+{
 #if defined(USE_CRSF_V3)
-    while (index < (CRSF_CYCLETIME_US / CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US) && index < CRSF_SCHEDULE_COUNT_MAX) {
-        // schedule heartbeat to ensure that telemetry/heartbeat frames are sent at minimum 50Hz
-        crsfSchedule[index++] = BIT(CRSF_FRAME_HEARTBEAT_INDEX);
+    if (crsfHeartBeatRateBucket >= 0) {
+        sbuf_t *dst = crsfInitializeSbuf();
+        crsfFrameHeartbeat(dst);
+        crsfTransmitSbuf(dst);
+        return true;
     }
 #endif
-
-    crsfScheduleCount = (uint8_t)index;
-
-#if defined(USE_CRSF_CMS_TELEMETRY)
-    crsfDisplayportRegister();
-#endif
+    return false;
 }
 
-bool checkCrsfTelemetryState(void)
+static bool crsfSendTelemetry(void)
 {
-    return crsfTelemetryEnabled;
+    if (crsfTelemetryState == TELEMETRY_STATE_NATIVE)
+    {
+        telemetrySensor_t *sensor = telemetryScheduleNext();
+
+        if (sensor) {
+            sbuf_t *dst = crsfInitializeSbuf();
+            switch (sensor->telid) {
+                case TELEM_ATTITUDE:
+                    crsfFrameAttitude(dst);
+                    break;
+                case TELEM_VARIOMETER:
+                    crsfFrameVarioSensor(dst);
+                    break;
+                case TELEM_ALTITUDE:
+                    crsfFrameAltitudeSensor(dst);
+                    break;
+                case TELEM_BATTERY:
+                    crsfFrameBatterySensor(dst);
+                    break;
+                case TELEM_FLIGHT_MODE:
+                    crsfFrameFlightMode(dst);
+                    break;
+    #ifdef USE_GPS
+                case TELEM_GPS:
+                    crsfFrameGps(dst);
+                    break;
+    #endif
+                default:
+                    crsfFrameHeartbeat(dst);
+                    break;
+            }
+            crsfTransmitSbuf(dst);
+            telemetryScheduleCommit(sensor);
+            return true;
+        }
+    }
+
+    return false;
 }
 
-#if defined(USE_CRSF_CMS_TELEMETRY)
-void crsfProcessDisplayPortCmd(uint8_t *frameStart)
+static bool crsfSendCustomTelemetry(void)
 {
-    uint8_t cmd = *frameStart;
-    switch (cmd) {
-    case CRSF_DISPLAYPORT_SUBCMD_OPEN: ;
-        const uint8_t rows = *(frameStart + CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET);
-        const uint8_t cols = *(frameStart + CRSF_DISPLAYPORT_OPEN_COLS_OFFSET);
-        crsfDisplayPortSetDimensions(rows, cols);
-        crsfDisplayPortMenuOpen();
-        break;
-    case CRSF_DISPLAYPORT_SUBCMD_CLOSE:
-        crsfDisplayPortMenuExit();
-        break;
-    case CRSF_DISPLAYPORT_SUBCMD_POLL:
-        crsfDisplayPortRefresh();
-        break;
-    default:
-        break;
+    if (crsfTelemetryState == TELEMETRY_STATE_CUSTOM)
+    {
+        size_t sensor_count = 0;
+        sbuf_t *dst = crsfInitializeSbuf();
+
+        crsfFrameCustomTelemetryHeader(dst);
+
+        while (sbufBytesRemaining(dst) > 6) {
+            telemetrySensor_t *sensor = telemetryScheduleNext();
+            if (sensor) {
+                uint8_t *ptr = sbufPtr(dst);
+                crsfFrameCustomTelemetrySensor(dst, sensor);
+                if (sbufBytesRemaining(dst) < 1) {
+                    sbufReset(dst, ptr);
+                    break;
+                }
+                telemetryScheduleCommit(sensor);
+                sensor_count++;
+            }
+            else {
+                break;
+            }
+        }
+
+        if (sensor_count) {
+            crsfTransmitSbuf(dst);
+            crsfTelemetryFrameId++;
+            return true;
+        }
     }
 
+    return false;
 }
 
-#endif
-
-#if defined(USE_CRSF_V3)
-void crsfProcessCommand(uint8_t *frameStart)
+static bool crsfPopulateCustomTelemetry(void)
 {
-    uint8_t cmd = *frameStart;
-    uint8_t subCmd = frameStart[1];
-    switch (cmd) {
-    case CRSF_COMMAND_SUBCMD_GENERAL:
-        switch (subCmd) {
-        case CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL:
-            crsfProcessSpeedNegotiationCmd(&frameStart[1]);
-            crsfScheduleSpeedNegotiationResponse();
-            break;
-        default:
-            break;
+    if (crsfTelemetryState == TELEMETRY_STATE_POPULATE)
+    {
+        static int slot = -10;
+
+        if (slot < 0) {
+            telemetrySensor_t * sensor = crsfGetCustomSensor(TELEM_NONE);
+            slot++;
+
+            if (sensor) {
+                sbuf_t *dst = crsfInitializeSbuf();
+                crsfFrameCustomTelemetryHeader(dst);
+                crsfFrameCustomTelemetrySensor(dst, sensor);
+                crsfTransmitSbuf(dst);
+                return true;
+            }
+            return false;
         }
-        break;
-    default:
-        break;
+
+        while (slot < TELEM_SENSOR_SLOT_COUNT) {
+            sensor_id_e id = telemetryConfig()->crsf_telemetry_sensors[slot];
+            slot++;
+
+            if (telemetrySensorActive(id)) {
+                telemetrySensor_t * sensor = crsfGetCustomSensor(id);
+                if (sensor) {
+                    sbuf_t *dst = crsfInitializeSbuf();
+                    crsfFrameCustomTelemetryHeader(dst);
+                    crsfFrameCustomTelemetrySensor(dst, sensor);
+                    crsfTransmitSbuf(dst);
+                    crsfTelemetryFrameId++;
+                    return true;
+                }
+            }
+        }
+
+        crsfTelemetryState = TELEMETRY_STATE_CUSTOM;
     }
+
+    return false;
 }
-#endif
 
-/*
- * Called periodically by the scheduler
- */
 void handleCrsfTelemetry(timeUs_t currentTimeUs)
 {
-    static uint32_t crsfLastCycleTime;
-
-    if (!crsfTelemetryEnabled) {
+    if (crsfTelemetryState == TELEMETRY_STATE_OFF)
         return;
-    }
 
 #if defined(USE_CRSF_V3)
-    if (crsfBaudNegotiationInProgress()) {
+    if (crsfBaudNegotiationInProgress())
         return;
-    }
 #endif
 
-    // Give the receiver a chance to send any outstanding telemetry data.
-    // This needs to be done at high frequency, to enable the RX to send the telemetry frame
-    // in between the RX frames.
-    crsfRxSendTelemetryData();
+    crsfTelemetryRateUpdate(currentTimeUs);
 
-    // Send ad-hoc response frames as soon as possible
+    if (crsfCanTransmitTelemetry())
+    {
+        bool __unused sent =
 #if defined(USE_MSP_OVER_TELEMETRY)
-    if (mspReplyPending) {
-        mspReplyPending = handleCrsfMspFrameBuffer(&crsfSendMspResponse);
-        crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
-        return;
-    }
+            handleCrsfMspFrameBuffer(&crsfSendMspResponse) ||
 #endif
-
-    if (deviceInfoReplyPending) {
-        sbuf_t crsfPayloadBuf;
-        sbuf_t *dst = &crsfPayloadBuf;
-        crsfInitializeFrame(dst);
-        crsfFrameDeviceInfo(dst);
-        crsfFinalize(dst);
-        deviceInfoReplyPending = false;
-        crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
-        return;
-    }
-
 #if defined(USE_CRSF_CMS_TELEMETRY)
-    if (crsfDisplayPortScreen()->reset) {
-        crsfDisplayPortScreen()->reset = false;
-        sbuf_t crsfDisplayPortBuf;
-        sbuf_t *dst = &crsfDisplayPortBuf;
-        crsfInitializeFrame(dst);
-        crsfFrameDisplayPortClear(dst);
-        crsfFinalize(dst);
-        crsfLastCycleTime = currentTimeUs;
-        return;
-    }
-    static uint8_t displayPortBatchId = 0;
-    if (crsfDisplayPortIsReady() && crsfDisplayPortScreen()->updated) {
-        crsfDisplayPortScreen()->updated = false;
-        uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols;
-        uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer;
-        uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize);
-        sbuf_t displayPortSbuf;
-        sbuf_t *src = sbufInit(&displayPortSbuf, srcStart, srcEnd);
-        sbuf_t crsfDisplayPortBuf;
-        sbuf_t *dst = &crsfDisplayPortBuf;
-        displayPortBatchId = (displayPortBatchId  + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
-        uint8_t i = 0;
-        while (sbufBytesRemaining(src)) {
-            crsfInitializeFrame(dst);
-            crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i);
-            crsfFinalize(dst);
-            crsfRxSendTelemetryData();
-            i++;
-        }
-        crsfLastCycleTime = currentTimeUs;
-        return;
-    }
+            crsfSendDisplayPortData() ||
 #endif
-
-    // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
-    // Spread out scheduled frames evenly so each frame is sent at the same frequency.
-    if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
-        crsfLastCycleTime = currentTimeUs;
-        processCrsf();
+            crsfSendDeviceInfoData() ||
+            crsfSendTelemetry() ||
+            crsfSendCustomTelemetry() ||
+            crsfPopulateCustomTelemetry() ||
+            crsfSendHeartBeat();
     }
 }
 
-#if defined(UNIT_TEST) || defined(USE_RX_EXPRESSLRS)
-static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
+static void INIT_CODE crsfInitNativeTelemetry(void)
 {
-    crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
-    sbufSwitchToReader(dst, crsfFrame);
-    const int frameSize = sbufBytesRemaining(dst);
-    for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
-        frame[ii] = sbufReadU8(dst);
+    telemetryScheduleInit(crsfNativeTelemetrySensors, ARRAYLEN(crsfNativeTelemetrySensors));
+
+    for (size_t i = 0; i < ARRAYLEN(crsfNativeTelemetrySensors); i++) {
+        telemetrySensor_t * sensor = &crsfNativeTelemetrySensors[i];
+        if (telemetryIsSensorEnabled(sensor->tcode)) {
+            telemetryScheduleAdd(sensor);
+        }
     }
-    return frameSize;
 }
 
-int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
+static void INIT_CODE crsfInitCustomTelemetry(void)
 {
-    sbuf_t crsfFrameBuf;
-    sbuf_t *sbuf = &crsfFrameBuf;
-
-    crsfInitializeFrame(sbuf);
-    switch (frameType) {
-    default:
-    case CRSF_FRAMETYPE_ATTITUDE:
-        crsfFrameAttitude(sbuf);
-        break;
-    case CRSF_FRAMETYPE_BATTERY_SENSOR:
-        crsfFrameBatterySensor(sbuf);
-        break;
-    case CRSF_FRAMETYPE_FLIGHT_MODE:
-        crsfFrameFlightMode(sbuf);
-        break;
-#if defined(USE_GPS)
-    case CRSF_FRAMETYPE_GPS:
-        crsfFrameGps(sbuf);
-        break;
-#endif
-#if defined(USE_MSP_OVER_TELEMETRY)
-    case CRSF_FRAMETYPE_DEVICE_INFO:
-        crsfFrameDeviceInfo(sbuf);
-        break;
-#endif
+    telemetryScheduleInit(crsfCustomTelemetrySensors, ARRAYLEN(crsfCustomTelemetrySensors));
+
+    for (int i = 0; i < TELEM_SENSOR_SLOT_COUNT; i++) {
+        sensor_id_e id = telemetryConfig()->crsf_telemetry_sensors[i];
+        if (telemetrySensorActive(id)) {
+            telemetrySensor_t * sensor = crsfGetCustomSensor(id);
+            if (sensor) {
+                if (telemetryConfig()->crsf_telemetry_interval[i])
+                    sensor->min_interval = telemetryConfig()->crsf_telemetry_interval[i];
+                if (sensor->max_interval > 1000)
+                    sensor->max_interval += rand() % 100;
+                telemetryScheduleAdd(sensor);
+            }
+        }
     }
-    const int frameSize = crsfFinalizeBuf(sbuf, frame);
-    return frameSize;
 }
 
-#if defined(USE_MSP_OVER_TELEMETRY)
-int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize)
+void INIT_CODE initCrsfTelemetry(void)
 {
-    sbuf_t crsfFrameBuf;
-    sbuf_t *sbuf = &crsfFrameBuf;
+    crsfTelemetryState = !crsfRxIsActive() ? TELEMETRY_STATE_OFF :
+        (telemetryConfig()->crsf_telemetry_mode == CRSF_TELEMETRY_MODE_NATIVE ?
+            TELEMETRY_STATE_NATIVE : TELEMETRY_STATE_POPULATE);
 
-    crsfInitializeFrame(sbuf);
-    sbufWriteU8(sbuf, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
-    sbufWriteU8(sbuf, CRSF_FRAMETYPE_MSP_RESP);
-    sbufWriteU8(sbuf, CRSF_ADDRESS_RADIO_TRANSMITTER);
-    sbufWriteU8(sbuf, CRSF_ADDRESS_FLIGHT_CONTROLLER);
-    sbufWriteData(sbuf, payload, payloadSize);
-    const int frameSize = crsfFinalizeBuf(sbuf, frame);
-    return frameSize;
-}
-#endif
+    if (crsfTelemetryState)
+    {
+        const float rate = telemetryConfig()->crsf_telemetry_link_rate;
+        const float ratio = telemetryConfig()->crsf_telemetry_link_ratio;
+
+        crsfTelemetryRateQuanta = rate / (ratio * 1000000);
+        crsfTelemetryRateBucket = 0;
+        crsfTelemetryFrameId = 0;
+
+#if defined(USE_CRSF_V3)
+        crsfHeartBeatRateBucket = 0;
 #endif
+
+        if (crsfTelemetryState == TELEMETRY_STATE_NATIVE) {
+            crsfInitNativeTelemetry();
+        }
+        else {
+            crsfInitCustomTelemetry();
+        }
+
+#if defined(USE_CRSF_CMS_TELEMETRY)
+        crsfDisplayportRegister();
 #endif
+    }
+}
+
+#endif /* USE_TELEMETRY_CRSF */
diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h
index 5710e3a440..ed514c342c 100644
--- a/src/main/telemetry/crsf.h
+++ b/src/main/telemetry/crsf.h
@@ -1,21 +1,18 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
  *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
  *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight 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.
  *
  * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see <http://www.gnu.org/licenses/>.
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
  */
 
 #pragma once
@@ -28,91 +25,32 @@
 #include "rx/crsf_protocol.h"
 #include "telemetry/msp_shared.h"
 
-enum {
-    CRSF_FM_REUSE_NONE = 0,
-    CRSF_FM_REUSE_GOV_STATE,
-    CRSF_FM_REUSE_HEADSPEED,
-    CRSF_FM_REUSE_THROTTLE,
-    CRSF_FM_REUSE_ESC_TEMP,
-    CRSF_FM_REUSE_MCU_TEMP,
-    CRSF_FM_REUSE_MCU_LOAD,
-    CRSF_FM_REUSE_SYS_LOAD,
-    CRSF_FM_REUSE_RT_LOAD,
-    CRSF_FM_REUSE_BEC_VOLTAGE,
-    CRSF_FM_REUSE_BUS_VOLTAGE,
-    CRSF_FM_REUSE_MCU_VOLTAGE,
-    CRSF_FM_REUSE_ADJFUNC,
-    CRSF_FM_REUSE_GOV_ADJFUNC,
-};
-
-enum {
-    CRSF_ATT_REUSE_NONE = 0,
-    CRSF_ATT_REUSE_THROTTLE,
-    CRSF_ATT_REUSE_ESC_TEMP,
-    CRSF_ATT_REUSE_ESC_PWM,
-    CRSF_ATT_REUSE_ESC_BEC_VOLTAGE,
-    CRSF_ATT_REUSE_ESC_BEC_CURRENT,
-    CRSF_ATT_REUSE_ESC_BEC_TEMP,
-    CRSF_ATT_REUSE_ESC_STATUS,
-    CRSF_ATT_REUSE_ESC_STATUS2,
-    CRSF_ATT_REUSE_MCU_TEMP,
-    CRSF_ATT_REUSE_MCU_LOAD,
-    CRSF_ATT_REUSE_SYS_LOAD,
-    CRSF_ATT_REUSE_RT_LOAD,
-    CRSF_ATT_REUSE_BEC_VOLTAGE,
-    CRSF_ATT_REUSE_BUS_VOLTAGE,
-    CRSF_ATT_REUSE_MCU_VOLTAGE,
-};
-
-enum {
-    CRSF_GPS_REUSE_NONE = 0,
-    CRSF_GPS_REUSE_HEADSPEED,
-    CRSF_GPS_REUSE_THROTTLE,
-    CRSF_GPS_REUSE_ESC_TEMP,
-    CRSF_GPS_REUSE_ESC_PWM,
-    CRSF_GPS_REUSE_ESC_THROTTLE,
-    CRSF_GPS_REUSE_ESC_BEC_VOLTAGE,
-    CRSF_GPS_REUSE_ESC_BEC_CURRENT,
-    CRSF_GPS_REUSE_ESC_BEC_TEMP,
-    CRSF_GPS_REUSE_ESC_STATUS,
-    CRSF_GPS_REUSE_ESC_STATUS2,
-    CRSF_GPS_REUSE_MCU_TEMP,
-    CRSF_GPS_REUSE_MCU_LOAD,
-    CRSF_GPS_REUSE_SYS_LOAD,
-    CRSF_GPS_REUSE_RT_LOAD,
-    CRSF_GPS_REUSE_BEC_VOLTAGE,
-    CRSF_GPS_REUSE_BUS_VOLTAGE,
-    CRSF_GPS_REUSE_MCU_VOLTAGE,
-};
-
-enum {
-    CRSF_GPS_SATS_REUSE_NONE = 0,
-    CRSF_GPS_SATS_REUSE_ESC_TEMP,
-    CRSF_GPS_SATS_REUSE_MCU_TEMP,
-    CRSF_GPS_SATS_REUSE_PROFILE,
-    CRSF_GPS_SATS_REUSE_RATE_PROFILE,
-    CRSF_GPS_SATS_REUSE_LED_PROFILE,
-    CRSF_GPS_SATS_REUSE_MODEL_ID,
-};
 
 void initCrsfTelemetry(void);
+void handleCrsfTelemetry(timeUs_t currentTimeUs);
+
 uint32_t getCrsfDesiredSpeed(void);
 void setCrsfDefaultSpeed(void);
+
 bool checkCrsfTelemetryState(void);
-void handleCrsfTelemetry(timeUs_t currentTimeUs);
+
 void crsfScheduleDeviceInfoResponse(void);
 void crsfScheduleMspResponse(uint8_t requestOriginID);
+
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
 void crsfProcessCommand(uint8_t *frameStart);
+
 #if defined(USE_CRSF_CMS_TELEMETRY)
 void crsfProcessDisplayPortCmd(uint8_t *frameStart);
 #endif
+
 #if defined(USE_MSP_OVER_TELEMETRY)
 void initCrsfMspBuffer(void);
 bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
 bool handleCrsfMspFrameBuffer(mspResponseFnPtr responseFn);
 int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize);
 #endif
+
 #if defined(USE_CRSF_V3)
 void speedNegotiationProcess(uint32_t currentTime);
 bool crsfBaudNegotiationInProgress(void);
diff --git a/src/main/telemetry/sensors.c b/src/main/telemetry/sensors.c
new file mode 100644
index 0000000000..280f85b2ff
--- /dev/null
+++ b/src/main/telemetry/sensors.c
@@ -0,0 +1,511 @@
+/*
+ * This file is part of Rotorflight.
+ *
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Rotorflight 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include <stddef.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef USE_TELEMETRY
+
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+#include "pg/pilot.h"
+#include "pg/telemetry.h"
+
+#include "build/debug.h"
+
+#include "common/crc.h"
+
+#include "sensors/battery.h"
+#include "sensors/voltage.h"
+#include "sensors/current.h"
+#include "sensors/esc_sensor.h"
+#include "sensors/adcinternal.h"
+#include "sensors/acceleration.h"
+
+#include "flight/position.h"
+#include "flight/governor.h"
+#include "flight/rescue.h"
+#include "flight/mixer.h"
+#include "flight/imu.h"
+
+#include "io/gps.h"
+#include "io/ledstrip.h"
+
+#include "fc/rc_modes.h"
+#include "fc/rc_adjustments.h"
+#include "fc/runtime_config.h"
+
+#include "scheduler/scheduler.h"
+
+#include "telemetry/sensors.h"
+
+
+/** Sensor functions **/
+
+static int getVoltage(voltageMeterId_e id)
+{
+    voltageMeter_t voltage;
+    return voltageMeterRead(id, &voltage) ? voltage.voltage / 10 : 0;
+}
+
+static int getCurrent(currentMeterId_e id)
+{
+    currentMeter_t current;
+    return currentMeterRead(id, &current) ? current.current / 10: 0;
+}
+
+static int getEscSensorValue(uint8_t motor, uint8_t id)
+{
+    escSensorData_t * data = getEscSensorData(motor);
+
+    if (data) {
+        switch (id) {
+            case 1:
+                return data->voltage / 10;
+            case 2:
+                return data->current / 10;
+            case 3:
+                return data->consumption;
+            case 4:
+                return data->erpm;
+            case 5:
+                return data->pwm;
+            case 6:
+                return data->throttle;
+            case 7:
+                return data->temperature / 10;
+            case 8:
+                return data->temperature2 / 10;
+            case 9:
+                return data->bec_voltage;
+            case 10:
+                return data->bec_current;
+            case 11:
+                return data->status;
+            case 12:
+                return 0; // data->model_id
+        }
+    }
+
+    return 0;
+}
+
+static uint32_t getTupleHash(uint32_t a, uint32_t b)
+{
+    uint32_t data[2] = { a, b };
+    return fnv_update(0x42424242, data, sizeof(data));
+}
+
+
+int telemetrySensorValue(sensor_id_e id)
+{
+    switch (id) {
+        case TELEM_NONE:
+            return 0;
+
+        case TELEM_HEARTBEAT:
+            return millis() % 60000;
+
+        case TELEM_BATTERY:
+            return 0;
+        case TELEM_BATTERY_VOLTAGE:
+            return getBatteryVoltage();
+        case TELEM_BATTERY_CURRENT:
+            return getBatteryCurrent();
+        case TELEM_BATTERY_CONSUMPTION:
+            return getBatteryCapacityUsed();
+        case TELEM_BATTERY_CHARGE_LEVEL:
+            return calculateBatteryPercentageRemaining();
+        case TELEM_BATTERY_CELL_COUNT:
+            return getBatteryCellCount();
+        case TELEM_BATTERY_CELL_VOLTAGE:
+            return getBatteryAverageCellVoltage();
+        case TELEM_BATTERY_CELL_VOLTAGES:
+            return 0;
+
+        case TELEM_CONTROL:
+            return millis();
+        case TELEM_PITCH_CONTROL:
+            return lrintf(mixerGetInput(MIXER_IN_STABILIZED_PITCH) * 120);
+        case TELEM_ROLL_CONTROL:
+            return lrintf(mixerGetInput(MIXER_IN_STABILIZED_ROLL) * 120);
+        case TELEM_YAW_CONTROL:
+            return lrintf(mixerGetInput(MIXER_IN_STABILIZED_YAW) * 240);
+        case TELEM_COLLECTIVE_CONTROL:
+            return lrintf(mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE) * 120);
+        case TELEM_THROTTLE_CONTROL:
+            return lrintf(mixerGetInput(MIXER_IN_STABILIZED_THROTTLE) * 100);
+
+        case TELEM_ESC1_DATA:
+        case TELEM_ESC1_VOLTAGE:
+        case TELEM_ESC1_CURRENT:
+        case TELEM_ESC1_CAPACITY:
+        case TELEM_ESC1_ERPM:
+        case TELEM_ESC1_POWER:
+        case TELEM_ESC1_THROTTLE:
+        case TELEM_ESC1_TEMP1:
+        case TELEM_ESC1_TEMP2:
+        case TELEM_ESC1_BEC_VOLTAGE:
+        case TELEM_ESC1_BEC_CURRENT:
+        case TELEM_ESC1_STATUS:
+        case TELEM_ESC1_MODEL:
+            return getEscSensorValue(0, id - TELEM_ESC1_DATA);
+
+        case TELEM_ESC2_DATA:
+        case TELEM_ESC2_VOLTAGE:
+        case TELEM_ESC2_CURRENT:
+        case TELEM_ESC2_CAPACITY:
+        case TELEM_ESC2_ERPM:
+        case TELEM_ESC2_POWER:
+        case TELEM_ESC2_THROTTLE:
+        case TELEM_ESC2_TEMP1:
+        case TELEM_ESC2_TEMP2:
+        case TELEM_ESC2_BEC_VOLTAGE:
+        case TELEM_ESC2_BEC_CURRENT:
+        case TELEM_ESC2_STATUS:
+        case TELEM_ESC2_MODEL:
+            return getEscSensorValue(1, id - TELEM_ESC2_DATA);
+
+        case TELEM_ESC_VOLTAGE:
+            return getVoltage(VOLTAGE_METER_ID_ESC_COMBINED);
+        case TELEM_BEC_VOLTAGE:
+            return getVoltage(VOLTAGE_METER_ID_BEC);
+        case TELEM_BUS_VOLTAGE:
+            return getVoltage(VOLTAGE_METER_ID_BUS);
+        case TELEM_MCU_VOLTAGE:
+            return getVoltage(VOLTAGE_METER_ID_MCU);
+
+        case TELEM_ESC_CURRENT:
+            return getCurrent(CURRENT_METER_ID_ESC_COMBINED);
+        case TELEM_BEC_CURRENT:
+            return getCurrent(CURRENT_METER_ID_BEC);
+        case TELEM_BUS_CURRENT:
+            return getCurrent(CURRENT_METER_ID_BUS);
+        case TELEM_MCU_CURRENT:
+            return getCurrent(CURRENT_METER_ID_MCU);
+
+        case TELEM_MCU_TEMP:
+            return getCoreTemperatureCelsius();
+
+        case TELEM_ESC_TEMP:
+        case TELEM_BEC_TEMP:
+        case TELEM_AIR_TEMP:
+        case TELEM_MOTOR_TEMP:
+        case TELEM_BATTERY_TEMP:
+        case TELEM_EXHAUST_TEMP:
+            return 0;
+
+        case TELEM_HEADING:
+            return attitude.values.yaw;
+        case TELEM_ALTITUDE:
+            return getEstimatedAltitudeCm();
+        case TELEM_VARIOMETER:
+            return getEstimatedVarioCms();
+
+        case TELEM_HEADSPEED:
+            return getHeadSpeed();
+        case TELEM_TAILSPEED:
+            return getTailSpeed();
+
+        case TELEM_MOTOR_RPM:
+        case TELEM_TRANS_RPM:
+            return 0;
+
+        case TELEM_ATTITUDE:
+            return millis();
+        case TELEM_ATTITUDE_PITCH:
+            return attitude.values.pitch / 10;
+        case TELEM_ATTITUDE_ROLL:
+            return attitude.values.roll / 10;
+        case TELEM_ATTITUDE_YAW:
+            return attitude.values.yaw / 10;
+
+        case TELEM_ACCEL:
+            return millis();
+        case TELEM_ACCEL_X:
+            return lrintf(acc.accADC[0] * acc.dev.acc_1G_rec * 10);
+        case TELEM_ACCEL_Y:
+            return lrintf(acc.accADC[1] * acc.dev.acc_1G_rec * 10);
+        case TELEM_ACCEL_Z:
+            return lrintf(acc.accADC[2] * acc.dev.acc_1G_rec * 10);
+
+        case TELEM_GPS:
+            return 0;
+        case TELEM_GPS_SATS:
+            return gpsSol.numSat;
+        case TELEM_GPS_PDOP:
+            return 0;
+        case TELEM_GPS_HDOP:
+            return gpsSol.hdop;
+        case TELEM_GPS_VDOP:
+            return 0;
+        case TELEM_GPS_COORD:
+            return getTupleHash(gpsSol.llh.lat, gpsSol.llh.lat);
+        case TELEM_GPS_ALTITUDE:
+            return gpsSol.llh.altCm;
+        case TELEM_GPS_HEADING:
+            return gpsSol.groundCourse;
+        case TELEM_GPS_GROUNDSPEED:
+            return gpsSol.groundSpeed;
+        case TELEM_GPS_HOME_DISTANCE:
+            return GPS_distanceToHome;
+        case TELEM_GPS_HOME_DIRECTION:
+            return GPS_directionToHome;
+        case TELEM_GPS_DATE_TIME:
+            return 0;
+
+        case TELEM_LOAD:
+            return 0;
+        case TELEM_CPU_LOAD:
+            return getAverageCPULoadPercent();
+        case TELEM_SYS_LOAD:
+            return getAverageSystemLoadPercent();
+        case TELEM_RT_LOAD:
+            return getMaxRealTimeLoadPercent();
+
+        case TELEM_MODEL_ID:
+            return pilotConfig()->modelId;
+        case TELEM_FLIGHT_MODE:
+            return flightModeFlags;
+        case TELEM_ARMING_FLAGS:
+            return armingFlags;
+        case TELEM_ARMING_DISABLE_FLAGS:
+            return getArmingDisableFlags();
+        case TELEM_RESCUE_STATE:
+            return getRescueState();
+        case TELEM_GOVERNOR_STATE:
+            return getGovernorState();
+        case TELEM_GOVERNOR_FLAGS:
+            return 0;
+
+        case TELEM_PID_PROFILE:
+            return getCurrentPidProfileIndex() + 1;
+        case TELEM_RATES_PROFILE:
+            return getCurrentControlRateProfileIndex() + 1;
+        case TELEM_LED_PROFILE:
+            return getLedProfile() + 1;
+        case TELEM_BATTERY_PROFILE:
+            return 0;
+
+        case TELEM_ADJFUNC:
+            return getAdjustmentsRangeName() ?
+                getTupleHash(getAdjustmentsRangeFunc(), getAdjustmentsRangeValue()) : 0;
+
+        case TELEM_DEBUG_0:
+            return debug[0];
+        case TELEM_DEBUG_1:
+            return debug[1];
+        case TELEM_DEBUG_2:
+            return debug[2];
+        case TELEM_DEBUG_3:
+            return debug[3];
+        case TELEM_DEBUG_4:
+            return debug[4];
+        case TELEM_DEBUG_5:
+            return debug[5];
+        case TELEM_DEBUG_6:
+            return debug[6];
+        case TELEM_DEBUG_7:
+            return debug[7];
+
+        default:
+            return 0;
+    }
+
+    return 0;
+}
+
+
+bool telemetrySensorActive(sensor_id_e id)
+{
+    switch (id) {
+        case TELEM_NONE:
+            return false;
+
+        case TELEM_HEARTBEAT:
+            return true;
+
+        case TELEM_BATTERY:
+        case TELEM_BATTERY_VOLTAGE:
+        case TELEM_BATTERY_CURRENT:
+        case TELEM_BATTERY_CONSUMPTION:
+        case TELEM_BATTERY_CHARGE_LEVEL:
+        case TELEM_BATTERY_CELL_COUNT:
+        case TELEM_BATTERY_CELL_VOLTAGE:
+            return true;
+
+        case TELEM_BATTERY_CELL_VOLTAGES:
+            return false;
+
+        case TELEM_CONTROL:
+        case TELEM_ROLL_CONTROL:
+        case TELEM_PITCH_CONTROL:
+        case TELEM_YAW_CONTROL:
+        case TELEM_COLLECTIVE_CONTROL:
+        case TELEM_THROTTLE_CONTROL:
+            return true;
+
+        case TELEM_ESC1_DATA:
+        case TELEM_ESC1_VOLTAGE:
+        case TELEM_ESC1_CURRENT:
+        case TELEM_ESC1_CAPACITY:
+        case TELEM_ESC1_ERPM:
+        case TELEM_ESC1_POWER:
+        case TELEM_ESC1_THROTTLE:
+        case TELEM_ESC1_TEMP1:
+        case TELEM_ESC1_TEMP2:
+        case TELEM_ESC1_BEC_VOLTAGE:
+        case TELEM_ESC1_BEC_CURRENT:
+        case TELEM_ESC1_STATUS:
+        case TELEM_ESC1_MODEL:
+            return true;
+
+        case TELEM_ESC2_DATA:
+        case TELEM_ESC2_VOLTAGE:
+        case TELEM_ESC2_CURRENT:
+        case TELEM_ESC2_CAPACITY:
+        case TELEM_ESC2_ERPM:
+        case TELEM_ESC2_POWER:
+        case TELEM_ESC2_THROTTLE:
+        case TELEM_ESC2_TEMP1:
+        case TELEM_ESC2_TEMP2:
+        case TELEM_ESC2_BEC_VOLTAGE:
+        case TELEM_ESC2_BEC_CURRENT:
+        case TELEM_ESC2_STATUS:
+        case TELEM_ESC2_MODEL:
+            return true;
+
+        case TELEM_ESC_VOLTAGE:
+        case TELEM_BEC_VOLTAGE:
+        case TELEM_BUS_VOLTAGE:
+        case TELEM_MCU_VOLTAGE:
+            return true;
+
+        case TELEM_ESC_CURRENT:
+        case TELEM_BEC_CURRENT:
+        case TELEM_BUS_CURRENT:
+        case TELEM_MCU_CURRENT:
+            return true;
+
+        case TELEM_MCU_TEMP:
+            return true;
+        case TELEM_ESC_TEMP:
+        case TELEM_BEC_TEMP:
+        case TELEM_AIR_TEMP:
+        case TELEM_MOTOR_TEMP:
+        case TELEM_BATTERY_TEMP:
+        case TELEM_EXHAUST_TEMP:
+            return false;
+
+        case TELEM_HEADING:
+        case TELEM_ALTITUDE:
+        case TELEM_VARIOMETER:
+            return true;
+
+        case TELEM_HEADSPEED:
+        case TELEM_TAILSPEED:
+            return true;
+
+        case TELEM_MOTOR_RPM:
+        case TELEM_TRANS_RPM:
+            return false;
+
+        case TELEM_ATTITUDE:
+        case TELEM_ATTITUDE_PITCH:
+        case TELEM_ATTITUDE_ROLL:
+        case TELEM_ATTITUDE_YAW:
+            return true;
+
+        case TELEM_ACCEL:
+        case TELEM_ACCEL_X:
+        case TELEM_ACCEL_Y:
+        case TELEM_ACCEL_Z:
+            return true;
+
+        case TELEM_GPS:
+        case TELEM_GPS_SATS:
+        case TELEM_GPS_HDOP:
+        case TELEM_GPS_COORD:
+        case TELEM_GPS_ALTITUDE:
+        case TELEM_GPS_HEADING:
+        case TELEM_GPS_GROUNDSPEED:
+        case TELEM_GPS_HOME_DISTANCE:
+        case TELEM_GPS_HOME_DIRECTION:
+            return true;
+
+        case TELEM_GPS_PDOP:
+        case TELEM_GPS_VDOP:
+        case TELEM_GPS_DATE_TIME:
+            return false;
+
+        case TELEM_LOAD:
+        case TELEM_CPU_LOAD:
+        case TELEM_SYS_LOAD:
+        case TELEM_RT_LOAD:
+            return true;
+
+        case TELEM_MODEL_ID:
+        case TELEM_FLIGHT_MODE:
+        case TELEM_ARMING_FLAGS:
+        case TELEM_ARMING_DISABLE_FLAGS:
+        case TELEM_RESCUE_STATE:
+        case TELEM_GOVERNOR_STATE:
+        case TELEM_GOVERNOR_FLAGS:
+            return true;
+
+        case TELEM_PID_PROFILE:
+        case TELEM_RATES_PROFILE:
+            return true;
+
+        case TELEM_BATTERY_PROFILE:
+        case TELEM_LED_PROFILE:
+            return false;
+
+        case TELEM_ADJFUNC:
+            return true;
+
+        case TELEM_DEBUG_0:
+        case TELEM_DEBUG_1:
+        case TELEM_DEBUG_2:
+        case TELEM_DEBUG_3:
+        case TELEM_DEBUG_4:
+        case TELEM_DEBUG_5:
+        case TELEM_DEBUG_6:
+        case TELEM_DEBUG_7:
+            return debugMode;
+
+        default:
+            return false;
+    }
+
+    return false;
+}
+
+
+/** Legacy sensors **/
+
+bool telemetryIsSensorEnabled(uint32_t sensor_bits)
+{
+    return (telemetryConfig()->enableSensors & sensor_bits);
+}
+
+#endif /* USE_TELEMETRY */
diff --git a/src/main/telemetry/sensors.h b/src/main/telemetry/sensors.h
new file mode 100644
index 0000000000..481e81abe3
--- /dev/null
+++ b/src/main/telemetry/sensors.h
@@ -0,0 +1,198 @@
+/*
+ * This file is part of Rotorflight.
+ *
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Rotorflight 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stddef.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "pg/pg.h"
+#include "pg/telemetry.h"
+
+#include "common/streambuf.h"
+
+#include "flight/motors.h"
+#include "flight/servos.h"
+
+
+/** Custom telemetry sensor types **/
+
+typedef enum
+{
+    TELEM_NONE                          = 0,
+
+    TELEM_HEARTBEAT                     = 1,
+
+    TELEM_BATTERY                       = 2,
+    TELEM_BATTERY_VOLTAGE               = 3,
+    TELEM_BATTERY_CURRENT               = 4,
+    TELEM_BATTERY_CONSUMPTION           = 5,
+    TELEM_BATTERY_CHARGE_LEVEL          = 6,
+    TELEM_BATTERY_CELL_COUNT            = 7,
+    TELEM_BATTERY_CELL_VOLTAGE          = 8,
+    TELEM_BATTERY_CELL_VOLTAGES         = 9,
+
+    TELEM_CONTROL                       = 10,
+    TELEM_PITCH_CONTROL                 = 11,
+    TELEM_ROLL_CONTROL                  = 12,
+    TELEM_YAW_CONTROL                   = 13,
+    TELEM_COLLECTIVE_CONTROL            = 14,
+    TELEM_THROTTLE_CONTROL              = 15,
+
+    TELEM_ESC1_DATA                     = 16,
+    TELEM_ESC1_VOLTAGE                  = 17,
+    TELEM_ESC1_CURRENT                  = 18,
+    TELEM_ESC1_CAPACITY                 = 19,
+    TELEM_ESC1_ERPM                     = 20,
+    TELEM_ESC1_POWER                    = 21,
+    TELEM_ESC1_THROTTLE                 = 22,
+    TELEM_ESC1_TEMP1                    = 23,
+    TELEM_ESC1_TEMP2                    = 24,
+    TELEM_ESC1_BEC_VOLTAGE              = 25,
+    TELEM_ESC1_BEC_CURRENT              = 26,
+    TELEM_ESC1_STATUS                   = 27,
+    TELEM_ESC1_MODEL                    = 28,
+
+    TELEM_ESC2_DATA                     = 29,
+    TELEM_ESC2_VOLTAGE                  = 30,
+    TELEM_ESC2_CURRENT                  = 31,
+    TELEM_ESC2_CAPACITY                 = 32,
+    TELEM_ESC2_ERPM                     = 33,
+    TELEM_ESC2_POWER                    = 34,
+    TELEM_ESC2_THROTTLE                 = 35,
+    TELEM_ESC2_TEMP1                    = 36,
+    TELEM_ESC2_TEMP2                    = 37,
+    TELEM_ESC2_BEC_VOLTAGE              = 38,
+    TELEM_ESC2_BEC_CURRENT              = 39,
+    TELEM_ESC2_STATUS                   = 40,
+    TELEM_ESC2_MODEL                    = 41,
+
+    TELEM_ESC_VOLTAGE                   = 42,
+    TELEM_BEC_VOLTAGE                   = 43,
+    TELEM_BUS_VOLTAGE                   = 44,
+    TELEM_MCU_VOLTAGE                   = 45,
+
+    TELEM_ESC_CURRENT                   = 46,
+    TELEM_BEC_CURRENT                   = 47,
+    TELEM_BUS_CURRENT                   = 48,
+    TELEM_MCU_CURRENT                   = 49,
+
+    TELEM_ESC_TEMP                      = 50,
+    TELEM_BEC_TEMP                      = 51,
+    TELEM_MCU_TEMP                      = 52,
+    TELEM_AIR_TEMP                      = 53,
+    TELEM_MOTOR_TEMP                    = 54,
+    TELEM_BATTERY_TEMP                  = 55,
+    TELEM_EXHAUST_TEMP                  = 56,
+
+    TELEM_HEADING                       = 57,
+    TELEM_ALTITUDE                      = 58,
+    TELEM_VARIOMETER                    = 59,
+
+    TELEM_HEADSPEED                     = 60,
+    TELEM_TAILSPEED                     = 61,
+    TELEM_MOTOR_RPM                     = 62,
+    TELEM_TRANS_RPM                     = 63,
+
+    TELEM_ATTITUDE                      = 64,
+    TELEM_ATTITUDE_PITCH                = 65,
+    TELEM_ATTITUDE_ROLL                 = 66,
+    TELEM_ATTITUDE_YAW                  = 67,
+
+    TELEM_ACCEL                         = 68,
+    TELEM_ACCEL_X                       = 69,
+    TELEM_ACCEL_Y                       = 70,
+    TELEM_ACCEL_Z                       = 71,
+
+    TELEM_GPS                           = 72,
+    TELEM_GPS_SATS                      = 73,
+    TELEM_GPS_PDOP                      = 74,
+    TELEM_GPS_HDOP                      = 75,
+    TELEM_GPS_VDOP                      = 76,
+    TELEM_GPS_COORD                     = 77,
+    TELEM_GPS_ALTITUDE                  = 78,
+    TELEM_GPS_HEADING                   = 79,
+    TELEM_GPS_GROUNDSPEED               = 80,
+    TELEM_GPS_HOME_DISTANCE             = 81,
+    TELEM_GPS_HOME_DIRECTION            = 82,
+    TELEM_GPS_DATE_TIME                 = 83,
+
+    TELEM_LOAD                          = 84,
+    TELEM_CPU_LOAD                      = 85,
+    TELEM_SYS_LOAD                      = 86,
+    TELEM_RT_LOAD                       = 87,
+
+    TELEM_MODEL_ID                      = 88,
+    TELEM_FLIGHT_MODE                   = 89,
+    TELEM_ARMING_FLAGS                  = 90,
+    TELEM_ARMING_DISABLE_FLAGS          = 91,
+    TELEM_RESCUE_STATE                  = 92,
+    TELEM_GOVERNOR_STATE                = 93,
+    TELEM_GOVERNOR_FLAGS                = 94,
+
+    TELEM_PID_PROFILE                   = 95,
+    TELEM_RATES_PROFILE                 = 96,
+    TELEM_BATTERY_PROFILE               = 97,
+    TELEM_LED_PROFILE                   = 98,
+
+    TELEM_ADJFUNC                       = 99,
+
+    TELEM_DEBUG_0                       = 100,
+    TELEM_DEBUG_1                       = 101,
+    TELEM_DEBUG_2                       = 102,
+    TELEM_DEBUG_3                       = 103,
+    TELEM_DEBUG_4                       = 104,
+    TELEM_DEBUG_5                       = 105,
+    TELEM_DEBUG_6                       = 106,
+    TELEM_DEBUG_7                       = 107,
+
+    TELEM_SENSOR_COUNT
+
+} sensor_id_e;
+
+
+typedef struct telemetrySensor_s telemetrySensor_t;
+
+typedef void (*telemetryEncode_f)(sbuf_t *buf, telemetrySensor_t *sensor);
+
+struct telemetrySensor_s {
+
+    uint16_t                telid;
+    uint32_t                tcode;
+
+    uint16_t                min_interval;
+    uint16_t                max_interval;
+
+    bool                    active;
+    bool                    update;
+    int                     bucket;
+    int                     value;
+
+    telemetryEncode_f       encode;
+};
+
+
+int telemetrySensorValue(sensor_id_e id);
+bool telemetrySensorActive(sensor_id_e id);
+
+
+/** Legacy sensors **/
+
+bool telemetryIsSensorEnabled(uint32_t sensor_bits);
+
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 5450a0354c..ce9e58ca9c 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -1,26 +1,24 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
  *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
  *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight 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.
  *
  * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see <http://www.gnu.org/licenses/>.
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
  */
 
 #include <stddef.h>
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include "platform.h"
 
@@ -62,51 +60,12 @@
 #include "telemetry/msp_shared.h"
 
 
-void telemetryInit(void)
-{
-#ifdef USE_TELEMETRY_FRSKY_HUB
-    initFrSkyHubTelemetry();
-#endif
-#ifdef USE_TELEMETRY_HOTT
-    initHoTTTelemetry();
-#endif
-#ifdef USE_TELEMETRY_SMARTPORT
-    initSmartPortTelemetry();
-#endif
-#ifdef USE_TELEMETRY_LTM
-    initLtmTelemetry();
-#endif
-#ifdef USE_TELEMETRY_JETIEXBUS
-    initJetiExBusTelemetry();
-#endif
-#ifdef USE_TELEMETRY_MAVLINK
-    initMAVLinkTelemetry();
-#endif
-#ifdef USE_TELEMETRY_GHST
-    initGhstTelemetry();
-#endif
-#ifdef USE_TELEMETRY_CRSF
-    initCrsfTelemetry();
-#if defined(USE_MSP_OVER_TELEMETRY)
-    initCrsfMspBuffer();
-#endif
-#endif
-#ifdef USE_TELEMETRY_SRXL
-    initSrxlTelemetry();
-#endif
-#ifdef USE_TELEMETRY_IBUS
-    initIbusTelemetry();
-#endif
-#if defined(USE_MSP_OVER_TELEMETRY)
-    initSharedMsp();
-#endif
+serialPort_t *telemetrySharedPort = NULL;
 
-    telemetryCheckState();
-}
 
 bool telemetryDetermineEnabledState(portSharing_e portSharing)
 {
-    bool enabled = portSharing == PORTSHARING_NOT_SHARED;
+    bool enabled = (portSharing == PORTSHARING_NOT_SHARED);
 
     if (portSharing == PORTSHARING_SHARED) {
         if (isModeActivationConditionPresent(BOXTELEMETRY))
@@ -120,22 +79,23 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing)
 
 bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const SerialRXType serialrxProvider)
 {
-    if (portConfig->functionMask & FUNCTION_RX_SERIAL && portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK &&
+    if ((portConfig->functionMask & FUNCTION_RX_SERIAL) &&
+        (portConfig->functionMask & TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK) &&
         (serialrxProvider == SERIALRX_SPEKTRUM1024 ||
-        serialrxProvider == SERIALRX_SPEKTRUM2048 ||
-        serialrxProvider == SERIALRX_SBUS ||
-        serialrxProvider == SERIALRX_SUMD ||
-        serialrxProvider == SERIALRX_SUMH ||
-        serialrxProvider == SERIALRX_XBUS_MODE_B ||
-        serialrxProvider == SERIALRX_XBUS_MODE_B_RJ01 ||
-        serialrxProvider == SERIALRX_IBUS)) {
+         serialrxProvider == SERIALRX_SPEKTRUM2048 ||
+         serialrxProvider == SERIALRX_SBUS ||
+         serialrxProvider == SERIALRX_SUMD ||
+         serialrxProvider == SERIALRX_SUMH ||
+         serialrxProvider == SERIALRX_XBUS_MODE_B ||
+         serialrxProvider == SERIALRX_XBUS_MODE_B_RJ01 ||
+         serialrxProvider == SERIALRX_IBUS)) {
 
         return true;
     }
 #ifdef USE_TELEMETRY_IBUS
-    if (portConfig->functionMask & FUNCTION_TELEMETRY_IBUS
-        && portConfig->functionMask & FUNCTION_RX_SERIAL
-        && serialrxProvider == SERIALRX_IBUS) {
+    if (portConfig->functionMask & FUNCTION_TELEMETRY_IBUS &&
+        portConfig->functionMask & FUNCTION_RX_SERIAL &&
+        serialrxProvider == SERIALRX_IBUS) {
         // IBUS serial RX & telemetry
         return true;
     }
@@ -143,7 +103,40 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const Seri
     return false;
 }
 
-serialPort_t *telemetrySharedPort = NULL;
+
+void telemetryProcess(timeUs_t currentTime)
+{
+#ifdef USE_TELEMETRY_FRSKY_HUB
+    handleFrSkyHubTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_HOTT
+    handleHoTTTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_SMARTPORT
+    handleSmartPortTelemetry();
+#endif
+#ifdef USE_TELEMETRY_LTM
+    handleLtmTelemetry();
+#endif
+#ifdef USE_TELEMETRY_JETIEXBUS
+    handleJetiExBusTelemetry();
+#endif
+#ifdef USE_TELEMETRY_MAVLINK
+    handleMAVLinkTelemetry();
+#endif
+#ifdef USE_TELEMETRY_CRSF
+    handleCrsfTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_GHST
+    handleGhstTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_SRXL
+    handleSrxlTelemetry(currentTime);
+#endif
+#ifdef USE_TELEMETRY_IBUS
+    handleIbusTelemetry();
+#endif
+}
 
 void telemetryCheckState(void)
 {
@@ -179,46 +172,111 @@ void telemetryCheckState(void)
 #endif
 }
 
-void telemetryProcess(uint32_t currentTime)
+void INIT_CODE telemetryInit(void)
 {
 #ifdef USE_TELEMETRY_FRSKY_HUB
-    handleFrSkyHubTelemetry(currentTime);
-#else
-    UNUSED(currentTime);
+    initFrSkyHubTelemetry();
 #endif
 #ifdef USE_TELEMETRY_HOTT
-    handleHoTTTelemetry(currentTime);
-#else
-    UNUSED(currentTime);
+    initHoTTTelemetry();
 #endif
 #ifdef USE_TELEMETRY_SMARTPORT
-    handleSmartPortTelemetry();
+    initSmartPortTelemetry();
 #endif
 #ifdef USE_TELEMETRY_LTM
-    handleLtmTelemetry();
+    initLtmTelemetry();
 #endif
 #ifdef USE_TELEMETRY_JETIEXBUS
-    handleJetiExBusTelemetry();
+    initJetiExBusTelemetry();
 #endif
 #ifdef USE_TELEMETRY_MAVLINK
-    handleMAVLinkTelemetry();
-#endif
-#ifdef USE_TELEMETRY_CRSF
-    handleCrsfTelemetry(currentTime);
+    initMAVLinkTelemetry();
 #endif
 #ifdef USE_TELEMETRY_GHST
-    handleGhstTelemetry(currentTime);
+    initGhstTelemetry();
+#endif
+#ifdef USE_TELEMETRY_CRSF
+    initCrsfTelemetry();
 #endif
 #ifdef USE_TELEMETRY_SRXL
-    handleSrxlTelemetry(currentTime);
+    initSrxlTelemetry();
 #endif
 #ifdef USE_TELEMETRY_IBUS
-    handleIbusTelemetry();
+    initIbusTelemetry();
 #endif
+#if defined(USE_MSP_OVER_TELEMETRY)
+    initSharedMsp();
+#endif
+
+    telemetryCheckState();
 }
 
-bool telemetryIsSensorEnabled(sensor_e sensor)
+
+/** Telemetry scheduling framework **/
+
+static telemetryScheduler_t sch = INIT_ZERO;
+
+
+void INIT_CODE telemetryScheduleAdd(telemetrySensor_t * sensor)
 {
-    return telemetryConfig()->enableSensors & sensor;
+    if (sensor) {
+        sensor->bucket = 0;
+        sensor->value = 0;
+        sensor->update = true;
+        sensor->active = true;
+    }
 }
-#endif
+
+void telemetryScheduleUpdate(timeUs_t currentTime)
+{
+    timeDelta_t delta = cmpTimeUs(currentTime, sch.update_time);
+
+    for (int i = 0; i < sch.sensor_count; i++) {
+        telemetrySensor_t * sensor = &sch.sensors[i];
+        if (sensor->active) {
+            const int value = telemetrySensorValue(sensor->telid);
+            sensor->update |= (value != sensor->value);
+            sensor->value = value;
+
+            const int interval = (sensor->update) ? sensor->min_interval : sensor->max_interval;
+            sensor->bucket += delta * 1000 / interval;
+            sensor->bucket = constrain(sensor->bucket, -1000000, 250000);
+        }
+    }
+
+    sch.update_time = currentTime;
+}
+
+telemetrySensor_t * telemetryScheduleNext(void)
+{
+    int index = sch.start_index;
+
+    for (int i = 0; i < sch.sensor_count; i++) {
+        index = (index + 1) % sch.sensor_count;
+        telemetrySensor_t * sensor = &sch.sensors[index];
+        if (sensor->active && sensor->bucket >= 0) {
+            sch.start_index = index;
+            return sensor;
+        }
+    }
+
+    return NULL;
+}
+
+void telemetryScheduleCommit(telemetrySensor_t * sensor)
+{
+    if (sensor) {
+        sensor->bucket -= 1000000;
+        sensor->update = false;
+    }
+}
+
+void INIT_CODE telemetryScheduleInit(telemetrySensor_t * sensors, size_t count)
+{
+    sch.update_time = 0;
+    sch.start_index = 0;
+    sch.sensor_count = count;
+    sch.sensors = sensors;
+}
+
+#endif /* USE_TELEMETRY */
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index 0b2f3386cf..d87bfab3d9 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -1,52 +1,55 @@
 /*
- * This file is part of Cleanflight and Betaflight.
+ * This file is part of Rotorflight.
  *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
+ * Rotorflight is free software. You can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
  *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * Rotorflight 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.
  *
  * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see <http://www.gnu.org/licenses/>.
- */
-
-/*
- * telemetry.h
- *
- *  Created on: 6 Apr 2014
- *      Author: Hydra
+ * along with this software. If not, see <https://www.gnu.org/licenses/>.
  */
 
 #pragma once
 
 #include "common/unit.h"
 
-#include "io/serial.h"
-
 #include "pg/pg.h"
 #include "pg/telemetry.h"
 
+#include "io/serial.h"
+
 #include "rx/rx.h"
 
-#include "telemetry/ibus_shared.h"
+#include "telemetry/sensors.h"
+
+
+typedef struct {
+    timeUs_t                    update_time;
+    uint16_t                    start_index;
+    uint16_t                    sensor_count;
+    telemetrySensor_t *         sensors;
+} telemetryScheduler_t;
 
 
 extern serialPort_t *telemetrySharedPort;
 
-void telemetryInit(void);
+bool telemetryDetermineEnabledState(portSharing_e portSharing);
 bool telemetryCheckRxPortShared(const serialPortConfig_t *portConfig, const SerialRXType serialrxProvider);
 
+void telemetryProcess(timeUs_t currentTime);
 void telemetryCheckState(void);
-void telemetryProcess(uint32_t currentTime);
+void telemetryInit(void);
 
-bool telemetryDetermineEnabledState(portSharing_e portSharing);
+telemetrySensor_t * telemetryScheduleNext(void);
+
+void telemetryScheduleAdd(telemetrySensor_t * sensor);
+void telemetryScheduleUpdate(timeUs_t currentTime);
+void telemetryScheduleCommit(telemetrySensor_t * sensor);
+void telemetryScheduleInit(telemetrySensor_t * sensors, size_t count);
 
-bool telemetryIsSensorEnabled(sensor_e sensor);