TBS Crossfire SerialRX protocol and telemetry (grafting from Betaflight)

Inject link quality to virtual channel 17 for CRSF telemetry, change to integer arithmetic for channel value conversion
Rename USE_PWM and USE_PPM to USE_RX_PWM and USE_RX_PPM

Makefile

@@ -543,6 +543,7 @@ COMMON_SRC = \
             rx/pwm.c \
             rx/rx.c \
             rx/rx_spi.c \
+            rx/crsf.c \
             rx/sbus.c \
             rx/spektrum.c \
             rx/sumd.c \
@@ -595,6 +596,7 @@ HIGHEND_SRC = \
             sensors/barometer.c \
             sensors/pitotmeter.c \
             telemetry/telemetry.c \
+            telemetry/crsf.c \
             telemetry/frsky.c \
             telemetry/hott.c \
             telemetry/smartport.c \

src/main/common/maths.c

@@ -459,6 +459,19 @@ uint16_t crc16_ccitt(uint16_t crc, unsigned char a)
     return crc;
 }
 
+uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
+{
+    crc ^= a;
+    for (int ii = 0; ii < 8; ++ii) {
+        if (crc & 0x80) {
+            crc = (crc << 1) ^ 0xD5;
+        } else {
+            crc = crc << 1;
+        }
+    }
+    return crc;
+}
+
 float bellCurve(const float x, const float curveWidth)
 {
     return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));

src/main/common/maths.h

@@ -175,5 +175,6 @@ float acos_approx(float x);
 
 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 uint16_t crc16_ccitt(uint16_t crc, unsigned char a);
+uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a);
 
 float bellCurve(const float x, const float curveWidth);

src/main/drivers/pwm_mapping.c

@@ -95,7 +95,7 @@ bool CheckGPIOPinSource(ioTag_t tag, GPIO_TypeDef *gpio, uint16_t pin)
 
 pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
 {
-#ifndef SKIP_RX_PWM_PPM
+#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
     int channelIndex = 0;
 #endif
 
@@ -359,7 +359,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
 #endif
 
         if (type == MAP_TO_PPM_INPUT) {
-#ifndef SKIP_RX_PWM_PPM
+#if defined(USE_RX_PPM)
 #ifdef CC3D_PPM1
             if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) {
                 ppmAvoidPWMTimerClash(timerHardwarePtr, TIM4);
@@ -377,7 +377,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
             addBootlogEvent6(BOOT_EVENT_TIMER_CH_MAPPED, BOOT_EVENT_FLAGS_NONE, i, pwmIOConfiguration.motorCount, pwmIOConfiguration.servoCount, 0);
 #endif
         } else if (type == MAP_TO_PWM_INPUT) {
-#ifndef SKIP_RX_PWM_PPM
+#if defined(USE_RX_PWM)
             pwmInConfig(timerHardwarePtr, channelIndex);
             pwmIOConfiguration.ioConfigurations[pwmIOConfiguration.ioCount].flags = PWM_PF_PWM;
             pwmIOConfiguration.pwmInputCount++;

src/main/drivers/pwm_rx.c

@@ -20,7 +20,7 @@
 
 #include <platform.h>
 
-#ifndef SKIP_RX_PWM_PPM
+#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
 
 #include "build/build_config.h"
 #include "build/debug.h"

src/main/fc/fc_init.c

@@ -310,7 +310,7 @@ void init(void)
 
     pwm_params.enablePWMOutput = feature(FEATURE_PWM_OUTPUT_ENABLE);
 
-#ifndef SKIP_RX_PWM_PPM
+#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
     pwmRxInit(pwmRxConfig());
 #endif
 

src/main/fc/fc_msp.c

@@ -1266,7 +1266,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         break;
 
     case MSP_SET_RAW_RC:
-#ifndef SKIP_RX_MSP
+#ifdef USE_RX_MSP
         {
             uint8_t channelCount = dataSize / sizeof(uint16_t);
             if (channelCount > MAX_SUPPORTED_RC_CHANNEL_COUNT) {

src/main/fc/mw.c

@@ -715,7 +715,7 @@ bool taskUpdateRxCheck(timeUs_t currentTimeUs, uint32_t currentDeltaTime)
 {
     UNUSED(currentDeltaTime);
 
-    return updateRx(currentTimeUs);
+    return rxUpdateCheck(currentTimeUs, currentDeltaTime);
 }
 
 void taskUpdateRxMain(timeUs_t currentTimeUs)

src/main/fc/serial_cli.c

@@ -266,7 +266,9 @@ static const char * const lookupTableSerialRX[] = {
     "SUMH",
     "XB-B",
     "XB-B-RJ01",
-    "IBUS"
+    "IBUS",
+    "JETIEXBUS",
+    "CRSF"
 };
 #endif
 

src/main/rx/crsf.c

@@ -0,0 +1,278 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef USE_SERIALRX_CRSF
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/system.h"
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+
+#include "io/serial.h"
+
+#include "rx/rx.h"
+#include "rx/crsf.h"
+
+#define CRSF_TIME_NEEDED_PER_FRAME_US   1000
+#define CRSF_TIME_BETWEEN_FRAMES_US     4000 // a frame is sent by the transmitter every 4 milliseconds
+
+#define CRSF_DIGITAL_CHANNEL_MIN 172
+#define CRSF_DIGITAL_CHANNEL_MAX 1811
+
+STATIC_UNIT_TESTED bool crsfFrameDone = false;
+STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
+
+STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
+
+static serialPort_t *serialPort;
+static uint32_t crsfFrameStartAt = 0;
+static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
+static uint8_t telemetryBufLen = 0;
+
+
+/*
+ * CRSF protocol
+ *
+ * CRSF protocol uses a single wire half duplex uart connection.
+ * The master sends one frame every 4ms and the slave replies between two frames from the master.
+ *
+ * 420000 baud
+ * not inverted
+ * 8 Bit
+ * 1 Stop bit
+ * Big endian
+ * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
+ * Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes
+ * A 36 byte frame can be transmitted in 771 microseconds.
+ *
+ * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds
+ *
+ * Every 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)
+ *
+ */
+
+struct crsfPayloadRcChannelsPacked_s {
+    // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
+    unsigned int chan0 : 11;
+    unsigned int chan1 : 11;
+    unsigned int chan2 : 11;
+    unsigned int chan3 : 11;
+    unsigned int chan4 : 11;
+    unsigned int chan5 : 11;
+    unsigned int chan6 : 11;
+    unsigned int chan7 : 11;
+    unsigned int chan8 : 11;
+    unsigned int chan9 : 11;
+    unsigned int chan10 : 11;
+    unsigned int chan11 : 11;
+    unsigned int chan12 : 11;
+    unsigned int chan13 : 11;
+    unsigned int chan14 : 11;
+    unsigned int chan15 : 11;
+} __attribute__ ((__packed__));
+
+typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
+
+struct crsfPayloadLinkStatistics_s {
+    uint8_t     uplinkRSSIAnt1;
+    uint8_t     uplinkRSSIAnt2;
+    uint8_t     uplinkLQ;
+    int8_t      uplinkSNR;
+    uint8_t     activeAntenna;
+    uint8_t     rfMode;
+    uint8_t     uplinkTXPower;
+    uint8_t     downlinkRSSI;
+    uint8_t     downlinkLQ;
+    int8_t      downlinkSNR;
+} __attribute__ ((__packed__));
+
+typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
+
+// Receive ISR callback, called back from serial port
+STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
+{
+    static uint8_t crsfFramePosition = 0;
+    const uint32_t now = micros();
+
+#ifdef DEBUG_CRSF_PACKETS
+    debug[2] = now - crsfFrameStartAt;
+#endif
+
+    if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) {
+        // We've received a character after max time needed to complete a frame,
+        // so this must be the start of a new frame.
+        crsfFramePosition = 0;
+    }
+
+    if (crsfFramePosition == 0) {
+        crsfFrameStartAt = now;
+    }
+    // assume frame is 5 bytes long until we have received the frame length
+    // full frame length includes the length of the address and framelength fields
+    const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
+
+    if (crsfFramePosition < fullFrameLength) {
+        crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
+        crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
+    }
+}
+
+STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
+{
+    // CRC includes type and payload
+    uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
+    for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
+        crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
+    }
+    return crc;
+}
+
+STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
+{
+    if (crsfFrameDone) {
+        crsfFrameDone = false;
+        if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
+            // CRC includes type and payload of each frame
+            const uint8_t crc = crsfFrameCRC();
+            if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
+                return RX_FRAME_PENDING;
+            }
+            crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
+
+            // unpack the RC channels
+            const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload;
+            crsfChannelData[0] = rcChannels->chan0;
+            crsfChannelData[1] = rcChannels->chan1;
+            crsfChannelData[2] = rcChannels->chan2;
+            crsfChannelData[3] = rcChannels->chan3;
+            crsfChannelData[4] = rcChannels->chan4;
+            crsfChannelData[5] = rcChannels->chan5;
+            crsfChannelData[6] = rcChannels->chan6;
+            crsfChannelData[7] = rcChannels->chan7;
+            crsfChannelData[8] = rcChannels->chan8;
+            crsfChannelData[9] = rcChannels->chan9;
+            crsfChannelData[10] = rcChannels->chan10;
+            crsfChannelData[11] = rcChannels->chan11;
+            crsfChannelData[12] = rcChannels->chan12;
+            crsfChannelData[13] = rcChannels->chan13;
+            crsfChannelData[14] = rcChannels->chan14;
+            crsfChannelData[15] = rcChannels->chan15;
+            return RX_FRAME_COMPLETE;
+        }
+        else if (crsfFrame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) {
+            // CRC includes type and payload of each frame
+            const uint8_t crc = crsfFrameCRC();
+            if (crc != crsfFrame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) {
+                return RX_FRAME_PENDING;
+            }
+            crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
+
+            // Inject link quality into channel 17
+            const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
+            crsfChannelData[16] = scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 191, 1791);    // will map to [1000;2000] range
+
+            // This is not RC channels frame, update channel value but don't indicate frame completion
+            return RX_FRAME_PENDING;
+        }
+    }
+    return RX_FRAME_PENDING;
+}
+
+STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
+{
+    UNUSED(rxRuntimeConfig);
+    /* conversion from RC value to PWM
+     *       RC     PWM
+     * min  172 ->  988us
+     * mid  992 -> 1500us
+     * max 1811 -> 2012us
+     * scale factor = (2012-988) / (1811-172) = 0.62477120195241
+     * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
+     */
+    return (crsfChannelData[chan] * 1024 / 1639) + 881;
+}
+
+void crsfRxWriteTelemetryData(const void *data, int len)
+{
+    len = MIN(len, (int)sizeof(telemetryBuf));
+    memcpy(telemetryBuf, data, len);
+    telemetryBufLen = len;
+}
+
+void crsfRxSendTelemetryData(void)
+{
+    // if there is telemetry data to write
+    if (telemetryBufLen > 0) {
+        // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame)
+        // and that there is time to send the telemetry frame before the next RX frame arrives
+        if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) {
+            const uint32_t timeSinceStartOfFrame = micros() - crsfFrameStartAt;
+            if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) || 
+                (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) {
+                return;
+            }
+        }
+        serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
+        telemetryBufLen = 0; // reset telemetry buffer
+    }
+}
+
+bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+    for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
+        crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408;
+    }
+
+    rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;
+    rxRuntimeConfig->rxRefreshRate = 11000; //!!TODO this needs checking
+
+    rxRuntimeConfig->rcReadRawFn = crsfReadRawRC;
+    rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;
+
+    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
+    if (!portConfig) {
+        return false;
+    }
+
+    serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, crsfDataReceive, CRSF_BAUDRATE, CRSF_PORT_MODE, CRSF_PORT_OPTIONS);
+
+    return serialPort != NULL;
+}
+
+bool crsfRxIsActive(void)
+{
+    return serialPort != NULL;
+}
+#endif

src/main/rx/crsf.h

@@ -0,0 +1,84 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define CRSF_BAUDRATE           420000
+#define CRSF_PORT_OPTIONS       (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
+#define CRSF_PORT_MODE          MODE_RXTX
+
+#define CRSF_MAX_CHANNEL        17
+
+typedef enum {
+    CRSF_FRAMETYPE_GPS = 0x02,
+    CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
+    CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
+    CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
+    CRSF_FRAMETYPE_ATTITUDE = 0x1E,
+    CRSF_FRAMETYPE_FLIGHT_MODE = 0x21
+} crsfFrameTypes_e;
+
+enum {
+    CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
+    CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
+    CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
+    CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
+    CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
+    CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field
+    CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
+    CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field
+    CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field
+    CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined
+};
+
+enum {
+    CRSF_ADDRESS_BROADCAST = 0x00,
+    CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x8,
+    CRSF_ADDRESS_RESERVED1 = 0x8A,
+    CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
+    CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
+    CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8,
+    CRSF_ADDRESS_RESERVED2 = 0xCA,
+    CRSF_ADDRESS_RACE_TAG = 0xCC,
+    CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
+    CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
+    CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
+};
+
+#define CRSF_PAYLOAD_SIZE_MAX   32 // !!TODO needs checking
+#define CRSF_FRAME_SIZE_MAX     (CRSF_PAYLOAD_SIZE_MAX + 4)
+
+typedef struct crsfFrameDef_s {
+    uint8_t deviceAddress;
+    uint8_t frameLength;
+    uint8_t type;
+    uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; // +1 for CRC at end of payload
+} crsfFrameDef_t;
+
+typedef union crsfFrame_u {
+    uint8_t bytes[CRSF_FRAME_SIZE_MAX];
+    crsfFrameDef_t frame;
+} crsfFrame_t;
+
+
+void crsfRxWriteTelemetryData(const void *data, int len);
+void crsfRxSendTelemetryData(void);
+
+struct rxConfig_s;
+struct rxRuntimeConfig_s;
+bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
+bool crsfRxIsActive(void);

src/main/rx/msp.c

@@ -20,7 +20,7 @@
 
 #include "platform.h"
 
-#ifndef SKIP_RX_MSP
+#ifdef USE_RX_MSP
 
 #include "build/build_config.h"
 
@@ -67,6 +67,7 @@ void rxMspInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
     UNUSED(rxConfig);
 
     rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
+    rxRuntimeConfig->rxRefreshRate = 20000;
     rxRuntimeConfig->rcReadRawFn = rxMspReadRawRC;
     rxRuntimeConfig->rcFrameStatusFn = rxMspFrameStatus;
 }

src/main/rx/pwm.c

@@ -23,7 +23,7 @@
 
 #include "platform.h"
 
-#ifndef SKIP_RX_PWM_PPM
+#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
 
 #include "common/utils.h"
 
@@ -51,6 +51,9 @@ static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t c
 void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 {
     UNUSED(rxConfig);
+
+    rxRuntimeConfig->rxRefreshRate = 20000;
+
     // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled
     if (feature(FEATURE_RX_PARALLEL_PWM)) {
         rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT;

src/main/rx/rx.c

@@ -58,6 +58,7 @@
 #include "rx/ibus.h"
 #include "rx/jetiexbus.h"
 #include "rx/rx_spi.h"
+#include "rx/crsf.h"
 
 
 //#define DEBUG_RX_SIGNAL_LOSS
@@ -75,6 +76,7 @@ static bool rxIsInFailsafeModeNotDataDriven = true;
 
 static timeUs_t rxUpdateAt = 0;
 static timeUs_t needRxSignalBefore = 0;
+static uint32_t needRxSignalMaxDelayUs = 0;
 static timeUs_t suspendRxSignalUntil = 0;
 static uint8_t  skipRxSamples = 0;
 
@@ -235,6 +237,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
     case SERIALRX_JETIEXBUS:
         enabled = jetiExBusInit(rxConfig, rxRuntimeConfig);
         break;
+#endif
+#ifdef USE_SERIALRX_CRSF
+    case SERIALRX_CRSF:
+        enabled = crsfRxInit(rxConfig, rxRuntimeConfig);
+        break;
 #endif
     default:
         enabled = false;
@@ -242,54 +249,6 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
     }
     return enabled;
 }
-
-static uint8_t serialRxFrameStatus(const rxConfig_t *rxConfig)
-{
-    /**
-     * FIXME: Each of the xxxxFrameStatus() methods MUST be able to survive being called without the
-     * corresponding xxxInit() method having been called first.
-     *
-     * This situation arises when the cli or the msp changes the value of rxConfig->serialrx_provider
-     *
-     * A solution is for the ___Init() to configure the serialRxFrameStatus function pointer which
-     * should be used instead of the switch statement below.
-     */
-    switch (rxConfig->serialrx_provider) {
-#ifdef USE_SERIALRX_SPEKTRUM
-    case SERIALRX_SPEKTRUM1024:
-    case SERIALRX_SPEKTRUM2048:
-        return spektrumFrameStatus();
-#endif
-#ifdef USE_SERIALRX_SBUS
-    case SERIALRX_SBUS:
-        return sbusFrameStatus();
-#endif
-#ifdef USE_SERIALRX_SUMD
-    case SERIALRX_SUMD:
-        return sumdFrameStatus();
-#endif
-#ifdef USE_SERIALRX_SUMH
-    case SERIALRX_SUMH:
-        return sumhFrameStatus();
-#endif
-#ifdef USE_SERIALRX_XBUS
-    case SERIALRX_XBUS_MODE_B:
-    case SERIALRX_XBUS_MODE_B_RJ01:
-        return xBusFrameStatus();
-#endif
-#ifdef USE_SERIALRX_IBUS
-    case SERIALRX_IBUS:
-        return ibusFrameStatus();
-#endif
-#ifdef USE_SERIALRX_JETIEXBUS
-    case SERIALRX_JETIEXBUS:
-        return jetiExBusFrameStatus();
-#endif
-    default:
-        return RX_FRAME_PENDING;
-    }
-    return RX_FRAME_PENDING;
-}
 #endif
 
 void rxInit(const modeActivationCondition_t *modeActivationConditions)
@@ -297,6 +256,7 @@ void rxInit(const modeActivationCondition_t *modeActivationConditions)
     rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
     rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
     rcSampleIndex = 0;
+    needRxSignalMaxDelayUs = DELAY_10_HZ;
 
     for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
         rcData[i] = rxConfig()->midrc;
@@ -332,9 +292,10 @@ void rxInit(const modeActivationCondition_t *modeActivationConditions)
     }
 #endif
 
-#ifndef SKIP_RX_MSP
+#ifdef USE_RX_MSP
     if (feature(FEATURE_RX_MSP)) {
         rxMspInit(rxConfig(), &rxRuntimeConfig);
+        needRxSignalMaxDelayUs = DELAY_5_HZ;
     }
 #endif
 
@@ -344,13 +305,13 @@ void rxInit(const modeActivationCondition_t *modeActivationConditions)
         if (!enabled) {
             featureClear(FEATURE_RX_SPI);
             rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
+            rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
         }
     }
 #endif
 
-#ifndef SKIP_RX_PWM_PPM
+#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
     if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
-        rxRuntimeConfig.rxRefreshRate = 20000;
         rxPwmInit(rxConfig(), &rxRuntimeConfig);
     }
 #endif
@@ -373,23 +334,12 @@ bool rxAreFlightChannelsValid(void)
 {
     return rxFlightChannelsValid;
 }
+
 static bool isRxDataDriven(void)
 {
     return !(feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM));
 }
 
-static void resetRxSignalReceivedFlagIfNeeded(timeUs_t currentTimeUs)
-{
-    if (!rxSignalReceived) {
-        return;
-    }
-
-    if (((int32_t)(currentTimeUs - needRxSignalBefore) >= 0)) {
-        rxSignalReceived = false;
-        rxSignalReceivedNotDataDriven = false;
-    }
-}
-
 void suspendRxSignal(void)
 {
     suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
@@ -404,69 +354,44 @@ void resumeRxSignal(void)
     failsafeOnRxResume();
 }
 
-bool updateRx(timeUs_t currentTimeUs)
+bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
 {
-    resetRxSignalReceivedFlagIfNeeded(currentTimeUs);
+    UNUSED(currentDeltaTime);
 
-    if (isRxDataDriven()) {
+    if (rxSignalReceived) {
-        rxDataReceived = false;
+        if (((int32_t)(currentTimeUs - needRxSignalBefore) >= 0)) {
-    }
+            rxSignalReceived = false;
-
+            rxSignalReceivedNotDataDriven = false;
-
-#ifdef SERIAL_RX
-    if (feature(FEATURE_RX_SERIAL)) {
-        const uint8_t frameStatus = serialRxFrameStatus(rxConfig());
-        if (frameStatus & RX_FRAME_COMPLETE) {
-            rxDataReceived = true;
-            rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
-            rxSignalReceived = !rxIsInFailsafeMode;
-            needRxSignalBefore = currentTimeUs + DELAY_10_HZ;
         }
     }
-#endif
 
-#ifdef USE_RX_SPI
+#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
-    if (feature(FEATURE_RX_SPI)) {
-        const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
-        if (frameStatus & RX_FRAME_COMPLETE) {
-            rxDataReceived = true;
-            rxIsInFailsafeMode = false;
-            rxSignalReceived = !rxIsInFailsafeMode;
-            needRxSignalBefore = currentTimeUs + DELAY_5_HZ;
-        }
-    }
-#endif
-
-#ifndef SKIP_RX_MSP
-    if (feature(FEATURE_RX_MSP)) {
-        const uint8_t frameStatus = rxMspFrameStatus();
-        if (frameStatus & RX_FRAME_COMPLETE) {
-            rxDataReceived = true;
-            rxIsInFailsafeMode = false;
-            rxSignalReceived = !rxIsInFailsafeMode;
-            needRxSignalBefore = currentTimeUs + DELAY_5_HZ;
-        }
-    }
-#endif
-
-#ifndef SKIP_RX_PWM_PPM
     if (feature(FEATURE_RX_PPM)) {
         if (isPPMDataBeingReceived()) {
             rxSignalReceivedNotDataDriven = true;
             rxIsInFailsafeModeNotDataDriven = false;
-            needRxSignalBefore = currentTimeUs + DELAY_10_HZ;
+            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
             resetPPMDataReceivedState();
         }
-    }
+    } else if (feature(FEATURE_RX_PARALLEL_PWM)) {
-
-    if (feature(FEATURE_RX_PARALLEL_PWM)) {
         if (isPWMDataBeingReceived()) {
             rxSignalReceivedNotDataDriven = true;
             rxIsInFailsafeModeNotDataDriven = false;
-            needRxSignalBefore = currentTimeUs + DELAY_10_HZ;
+            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
+        }
+    } else
+#endif
+    {
+        rxDataReceived = false;
+        const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
+        if (frameStatus & RX_FRAME_COMPLETE) {
+            rxDataReceived = true;
+            rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
+            rxSignalReceived = !rxIsInFailsafeMode;
+            needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
         }
     }
-#endif
+
     return rxDataReceived || ((int32_t)(currentTimeUs - rxUpdateAt) >= 0); // data driven or 50Hz
 }
 
@@ -557,8 +482,8 @@ static void readRxChannelsApplyRanges(void)
 static void detectAndApplySignalLossBehaviour(void)
 {
     bool useValueFromRx = true;
-    bool rxIsDataDriven = isRxDataDriven();
+    const bool rxIsDataDriven = isRxDataDriven();
-    uint32_t currentMilliTime = millis();
+    const uint32_t currentMilliTime = millis();
 
     if (!rxIsDataDriven) {
         rxSignalReceived = rxSignalReceivedNotDataDriven;

src/main/rx/rx.h

@@ -59,7 +59,7 @@ typedef enum {
     SERIALRX_XBUS_MODE_B_RJ01 = 6,
     SERIALRX_IBUS = 7,
     SERIALRX_JETIEXBUS = 8,
-    SERIALRX_PROVIDER_MAX = SERIALRX_JETIEXBUS
+    SERIALRX_CRSF = 9
 } SerialRXType;
 
 #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT          12
@@ -153,7 +153,7 @@ extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only need
 
 struct modeActivationCondition_s;
 void rxInit(const struct modeActivationCondition_s *modeActivationConditions);
-bool updateRx(timeUs_t currentTimeUs);
+bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime);
 bool rxIsReceivingSignal(void);
 bool rxAreFlightChannelsValid(void);
 void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);

src/main/target/CC3D/target.h

@@ -188,7 +188,8 @@
 
 
 #ifdef USE_RX_NRF24
-#define SKIP_RX_PWM_PPM
+#undef USE_RX_PWM
+#undef USE_RX_PPM
 #undef SERIAL_RX
 #undef SPEKTRUM_BIND
 #endif

src/main/target/CJMCU/target.h

@@ -84,14 +84,15 @@
 //#define TELEMETRY
 //#define TELEMETRY_LTM
 //#define TELEMETRY_NRF24_LTM
-#define SKIP_RX_PWM_PPM
+#undef USE_RX_PWM
+#undef USE_RX_PPM
 #undef SERIAL_RX
 #undef SKIP_TASK_STATISTICS
 
 #else
 
 #define DEFAULT_RX_FEATURE      FEATURE_RX_PPM
-#undef SKIP_RX_MSP
+#define USE_RX_MSP
 #define SPEKTRUM_BIND
 #define BIND_PIN                PA3 // UART2, PA3
 

src/main/target/CRAZEPONYMINI/target.h

@@ -67,9 +67,10 @@
 #define RX_IRQ_PIN                  PA15
 #define RX_IRQ_GPIO_CLK_PERIPHERAL  RCC_APB2Periph_GPIOA
 
-#define SKIP_RX_MSP
+#undef USE_RX_MSP
 #define SKIP_INFLIGHT_ADJUSTMENTS
-#define SKIP_RX_PWM_PPM
+#undef USE_RX_PWM
+#undef USE_RX_PPM
 #undef SERIAL_RX
 #undef BLACKBOX
 

src/main/target/common.h

@@ -24,6 +24,8 @@
 #define USE_SERVOS
 #define USE_CLI
 
+#define USE_RX_PWM
+#define USE_RX_PPM
 #define SERIAL_RX
 #define USE_SERIALRX_SPEKTRUM   // Cheap and fairly common protocol
 #define USE_SERIALRX_SBUS       // Very common protocol
@@ -66,11 +68,14 @@
 #define TELEMETRY_IBUS
 #define TELEMETRY_MAVLINK
 #define TELEMETRY_SMARTPORT
+#define TELEMETRY_CRSF
 // These are rather exotic serial protocols
+#define USE_RX_MSP
 #define USE_SERIALRX_SUMD
 #define USE_SERIALRX_SUMH
 #define USE_SERIALRX_XBUS
 #define USE_SERIALRX_JETIEXBUS
+#define USE_SERIALRX_CRSF
 #define USE_PMW_SERVO_DRIVER
 #define PWM_DRIVER_PCA9685
 #define NAV_MAX_WAYPOINTS       60
@@ -79,7 +84,6 @@
 #define CLI_MINIMAL_VERBOSITY
 #define SKIP_CLI_COMMAND_HELP
 #define SKIP_CLI_RESOURCES
-#define SKIP_RX_MSP
 #define DISABLE_UNCOMMON_MIXERS
 #define NAV_MAX_WAYPOINTS       30
 #define MAX_BOOTLOG_ENTRIES     32

src/main/telemetry/crsf.c

@@ -0,0 +1,357 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#if defined(TELEMETRY) && defined(TELEMETRY_CRSF)
+
+#include "build/build_config.h"
+#include "build/version.h"
+
+#include "common/streambuf.h"
+#include "common/maths.h"
+#include "common/axis.h"
+#include "common/utils.h"
+#include "common/time.h"
+
+#include "drivers/system.h"
+#include "drivers/serial.h"
+
+#include "sensors/battery.h"
+
+#include "io/gps.h"
+#include "io/serial.h"
+
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+
+#include "rx/rx.h"
+#include "rx/crsf.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/crsf.h"
+
+#include "config/feature.h"
+//#include "config/config.h"
+#include "config/config_master.h"
+
+#define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
+
+static bool crsfTelemetryEnabled;
+static uint8_t crsfCrc;
+static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
+
+static void crsfInitializeFrame(sbuf_t *dst)
+{
+    crsfCrc = 0;
+    dst->ptr = crsfFrame;
+    dst->end = ARRAYEND(crsfFrame);
+
+    sbufWriteU8(dst, CRSF_ADDRESS_BROADCAST);
+}
+
+static void crsfSerialize8(sbuf_t *dst, uint8_t v)
+{
+    sbufWriteU8(dst, v);
+    crsfCrc = crc8_dvb_s2(crsfCrc, v);
+}
+
+static void crsfSerialize16(sbuf_t *dst, uint16_t v)
+{
+    // Use BigEndian format
+    crsfSerialize8(dst,  (v >> 8));
+    crsfSerialize8(dst, (uint8_t)v);
+}
+
+static void crsfSerialize32(sbuf_t *dst, uint32_t v)
+{
+    // Use BigEndian format
+    crsfSerialize8(dst, (v >> 24));
+    crsfSerialize8(dst, (v >> 16));
+    crsfSerialize8(dst, (v >> 8));
+    crsfSerialize8(dst, (uint8_t)v);
+}
+
+static void crsfSerializeData(sbuf_t *dst, const uint8_t *data, int len)
+{
+    for (int ii = 0; ii< len; ++ii) {
+        crsfSerialize8(dst, data[ii]);
+    }
+}
+
+static void crsfFinalize(sbuf_t *dst)
+{
+    sbufWriteU8(dst, crsfCrc);
+    sbufSwitchToReader(dst, crsfFrame);
+    // write the telemetry frame to the receiver.
+    crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
+}
+
+static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
+{
+    sbufWriteU8(dst, crsfCrc);
+    sbufSwitchToReader(dst, crsfFrame);
+    const int frameSize = sbufBytesRemaining(dst);
+    for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
+        frame[ii] = sbufReadU8(dst);
+    }
+    return frameSize;
+}
+
+/*
+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)
+*/
+
+/*
+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 )
+*/
+void crsfFrameGps(sbuf_t *dst)
+{
+    // use sbufWrite since CRC does not include frame length
+    sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_GPS);
+    crsfSerialize32(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
+    crsfSerialize32(dst, gpsSol.llh.lon);
+    crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
+    crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg
+    //Send real GPS altitude only if it's reliable (there's a GPS fix)
+    const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt : 0) + 1000;
+    crsfSerialize16(dst, altitude);
+    crsfSerialize8(dst, gpsSol.numSat);
+}
+
+/*
+0x08 Battery sensor
+Payload:
+uint16_t    Voltage ( mV * 100 )
+uint16_t    Current ( mA * 100 )
+uint24_t    Capacity ( mAh )
+uint8_t     Battery remaining ( percent )
+*/
+void crsfFrameBatterySensor(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);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
+    crsfSerialize16(dst, vbat); // vbat is in units of 0.1V
+    crsfSerialize16(dst, amperage / 10);
+    const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
+    const uint8_t batteryRemainingPercentage = calculateBatteryPercentage();
+    crsfSerialize8(dst, (batteryCapacity >> 16));
+    crsfSerialize8(dst, (batteryCapacity >> 8));
+    crsfSerialize8(dst, (uint8_t)batteryCapacity);
+
+    crsfSerialize8(dst, batteryRemainingPercentage);
+}
+
+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
+} crsrRfPower_e;
+
+/*
+0x1E Attitude
+Payload:
+int16_t     Pitch angle ( rad / 10000 )
+int16_t     Roll angle ( rad / 10000 )
+int16_t     Yaw angle ( rad / 10000 )
+*/
+
+#define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
+
+void crsfFrameAttitude(sbuf_t *dst)
+{
+     sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
+     crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE);
+     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch));
+     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll));
+     crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw));
+}
+
+/*
+0x21 Flight mode text based
+Payload:
+char[]      Flight mode ( Null­terminated string )
+*/
+void crsfFrameFlightMode(sbuf_t *dst)
+{
+    // just do Angle for the moment as a placeholder
+    // write zero for frame length, since we don't know it yet
+    uint8_t *lengthPtr = sbufPtr(dst);
+    sbufWriteU8(dst, 0);
+    crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
+
+    // use same logic as OSD, so telemetry displays same flight text as OSD
+    const char *flightMode = "ACRO";
+    if (FLIGHT_MODE(FAILSAFE_MODE)) {
+        flightMode = "!FS";
+    } else if (FLIGHT_MODE(ANGLE_MODE)) {
+        flightMode = "STAB";
+    } else if (FLIGHT_MODE(HORIZON_MODE)) {
+        flightMode = "HOR";
+    }
+    crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
+    crsfSerialize8(dst, 0); // zero terminator for string
+    // write in the length
+    *lengthPtr = sbufPtr(dst) - lengthPtr;
+}
+
+#define BV(x)  (1 << (x)) // bit value
+
+// schedule array to decide how often each type of frame is sent
+#define CRSF_SCHEDULE_COUNT_MAX     5
+static uint8_t crsfScheduleCount;
+static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
+
+
+static void processCrsf(void)
+{
+    static uint8_t crsfScheduleIndex = 0;
+    const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
+
+    sbuf_t crsfPayloadBuf;
+    sbuf_t *dst = &crsfPayloadBuf;
+
+    if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) {
+        crsfInitializeFrame(dst);
+        crsfFrameAttitude(dst);
+        crsfFinalize(dst);
+    }
+    if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) {
+        crsfInitializeFrame(dst);
+        crsfFrameBatterySensor(dst);
+        crsfFinalize(dst);
+    }
+    if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) {
+        crsfInitializeFrame(dst);
+        crsfFrameFlightMode(dst);
+        crsfFinalize(dst);
+    }
+#ifdef GPS
+    if (currentSchedule & BV(CRSF_FRAME_GPS)) {
+        crsfInitializeFrame(dst);
+        crsfFrameGps(dst);
+        crsfFinalize(dst);
+    }
+#endif
+    crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
+}
+
+void initCrsfTelemetry(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();
+    int index = 0;
+    crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE);
+    crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR);
+    crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE);
+    if (feature(FEATURE_GPS)) {
+        crsfSchedule[index++] = BV(CRSF_FRAME_GPS);
+    }
+    crsfScheduleCount = (uint8_t)index;
+
+ }
+
+bool checkCrsfTelemetryState(void)
+{
+    return crsfTelemetryEnabled;
+}
+
+/*
+ * Called periodically by the scheduler
+ */
+void handleCrsfTelemetry(timeUs_t currentTimeUs)
+{
+    static uint32_t crsfLastCycleTime;
+
+    if (!crsfTelemetryEnabled) {
+        return;
+    }
+    // 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();
+
+    // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
+    if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
+        crsfLastCycleTime = currentTimeUs;
+        processCrsf();
+    }
+}
+
+int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
+{
+    sbuf_t crsfFrameBuf;
+    sbuf_t *sbuf = &crsfFrameBuf;
+
+    crsfInitializeFrame(sbuf);
+    switch (frameType) {
+    default:
+    case CRSF_FRAME_ATTITUDE:
+        crsfFrameAttitude(sbuf);
+        break;
+    case CRSF_FRAME_BATTERY_SENSOR:
+        crsfFrameBatterySensor(sbuf);
+        break;
+    case CRSF_FRAME_FLIGHT_MODE:
+        crsfFrameFlightMode(sbuf);
+        break;
+#if defined(GPS)
+    case CRSF_FRAME_GPS:
+        crsfFrameGps(sbuf);
+        break;
+#endif
+    }
+    const int frameSize = crsfFinalizeBuf(sbuf, frame);
+    return frameSize;
+}
+#endif

src/main/telemetry/crsf.h

@@ -0,0 +1,35 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight 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 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+typedef enum {
+    CRSF_FRAME_START = 0,
+    CRSF_FRAME_ATTITUDE = CRSF_FRAME_START,
+    CRSF_FRAME_BATTERY_SENSOR,
+    CRSF_FRAME_FLIGHT_MODE,
+    CRSF_FRAME_GPS
+} crsfFrameType_e;
+
+void initCrsfTelemetry(void);
+bool checkCrsfTelemetryState(void);
+void handleCrsfTelemetry(timeUs_t currentTimeUs);
+
+int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
+

src/main/telemetry/telemetry.c

@@ -43,6 +43,7 @@
 #include "telemetry/mavlink.h"
 #include "telemetry/jetiexbus.h"
 #include "telemetry/ibus.h"
+#include "telemetry/crsf.h"
 
 static telemetryConfig_t *telemetryConfig;
 
@@ -81,6 +82,10 @@ void telemetryInit(void)
     initIbusTelemetry(telemetryConfig);
 #endif
 
+#if defined(TELEMETRY_CRSF)
+    initCrsfTelemetry();
+#endif
+
     telemetryCheckState();
 }
 
@@ -135,6 +140,9 @@ void telemetryCheckState(void)
     checkIbusTelemetryState();
 #endif
 
+#if defined(TELEMETRY_CRSF)
+    checkCrsfTelemetryState();
+#endif
 }
 
 void telemetryProcess(timeUs_t currentTimeUs, uint16_t deadband3d_throttle)
@@ -174,6 +182,9 @@ void telemetryProcess(timeUs_t currentTimeUs, uint16_t deadband3d_throttle)
     handleIbusTelemetry();
 #endif
 
+#if defined(TELEMETRY_CRSF)
+    handleCrsfTelemetry(currentTimeUs);
+#endif
 }
 
 #endif