Merge pull request #4050 from codecae/msp_shared_crsf_smartport

Decoupled MSP functionality from S.Port.  Implemented MSP telemetry in CRSF.

make/source.mk

@@ -159,6 +159,7 @@ FC_SRC = \
             telemetry/smartport.c \
             telemetry/ltm.c \
             telemetry/mavlink.c \
+            telemetry/msp_shared.c \
             telemetry/ibus.c \
             telemetry/ibus_shared.c \
             sensors/esc_sensor.c \

src/main/rx/crsf.c

@@ -33,22 +33,29 @@
 
 #include "drivers/serial.h"
 #include "drivers/serial_uart.h"
+#include "drivers/system.h"
 #include "drivers/time.h"
 
 #include "io/serial.h"
 
+#include "msp/msp.h"
+
 #include "rx/rx.h"
 #include "rx/crsf.h"
 
+#include "telemetry/crsf.h"
+#include "telemetry/msp_shared.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
 
+#define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
+
 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;
@@ -56,7 +63,6 @@ static uint32_t crsfFrameStartAt = 0;
 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
 static uint8_t telemetryBufLen = 0;
 
-
 /*
  * CRSF protocol
  *
@@ -133,6 +139,9 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
     if (crsfFramePosition < fullFrameLength) {
         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
         crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
+        if (crsfFrameDone) {
+            crsfFramePosition = 0;
+        }
     }
 }
 
@@ -176,6 +185,20 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
             crsfChannelData[14] = rcChannels->chan14;
             crsfChannelData[15] = rcChannels->chan15;
             return RX_FRAME_COMPLETE;
+        } else {
+            if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) {
+                // TODO: CRC CHECK
+                scheduleDeviceInfoResponse();
+                return RX_FRAME_COMPLETE;
+            } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) {
+                // TODO: CRC CHECK
+                uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
+                uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + 2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
+                if(handleMspFrame(frameStart, frameEnd)) {
+                    scheduleMspResponse();
+                }
+                return RX_FRAME_COMPLETE;
+            }
         }
     }
     return RX_FRAME_PENDING;
@@ -222,6 +245,7 @@ void crsfRxSendTelemetryData(void)
 
 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;
     }

src/main/rx/crsf.h

@@ -22,6 +22,8 @@
 #define CRSF_PORT_MODE          MODE_RXTX
 
 #define CRSF_MAX_CHANNEL        16
+#define CRSF_MSP_RX_BUF_SIZE 128
+#define CRSF_MSP_TX_BUF_SIZE 128
 
 typedef enum {
     CRSF_FRAMETYPE_GPS = 0x02,
@@ -29,7 +31,12 @@ typedef enum {
     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
     CRSF_FRAMETYPE_ATTITUDE = 0x1E,
-    CRSF_FRAMETYPE_FLIGHT_MODE = 0x21
+    CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
+    CRSF_FRAMETYPE_DEVICE_PING = 0x28,
+    CRSF_FRAMETYPE_DEVICE_INFO = 0x29,
+    CRSF_FRAMETYPE_MSP_REQ = 0x7A,   // response request using msp sequence as command
+    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)
 } crsfFrameType_e;
 
 enum {
@@ -38,11 +45,14 @@ enum {
     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_TX_MSP_PAYLOAD_SIZE = 58,
+    CRSF_FRAME_RX_MSP_PAYLOAD_SIZE = 8,
     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
+    CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined
+    CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4 // length of Extended Dest/Origin, TYPE and CRC fields combined
 };
 
 enum {
@@ -51,7 +61,7 @@ enum {
     CRSF_ADDRESS_RESERVED1 = 0x8A,
     CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
     CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
-    CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8,
+    CRSF_ADDRESS_BETAFLIGHT = 0xC8,
     CRSF_ADDRESS_RESERVED2 = 0xCA,
     CRSF_ADDRESS_RACE_TAG = 0xCC,
     CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
@@ -59,7 +69,7 @@ enum {
     CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
 };
 
-#define CRSF_PAYLOAD_SIZE_MAX   32 // !!TODO needs checking
+#define CRSF_PAYLOAD_SIZE_MAX   60 // Size confirmed by Remo
 #define CRSF_FRAME_SIZE_MAX     (CRSF_PAYLOAD_SIZE_MAX + 4)
 
 typedef struct crsfFrameDef_s {
@@ -74,7 +84,6 @@ typedef union crsfFrame_u {
     crsfFrameDef_t frame;
 } crsfFrame_t;
 
-
 void crsfRxWriteTelemetryData(const void *data, int len);
 void crsfRxSendTelemetryData(void);
 

src/main/target/SPRACINGF3EVO/target.h

@@ -189,3 +189,5 @@
 #else
 #define USED_TIMERS             (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(8) | TIM_N(15) | TIM_N(16))
 #endif
+
+#undef USE_DASHBOARD

src/main/target/STM32F3DISCOVERY/target.h

@@ -129,12 +129,12 @@
 #define USE_BARO_BMP280
 #define USE_BARO_MS5611
 
-#define OSD
+//#define OSD
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI2
 #define MAX7456_SPI_CS_PIN      SPI2_NSS_PIN
 
-#define CMS
+//#define CMS
 
 //#define USE_SDCARD
 //

src/main/target/common_fc_pre.h

@@ -102,7 +102,6 @@
 #define TELEMETRY_LTM
 #define TELEMETRY_SMARTPORT
 #define USE_RESOURCE_MGMT
-#define USE_SERVOS
 #endif
 
 #if (FLASH_SIZE > 128)
@@ -118,6 +117,7 @@
 #define USE_RX_MSP
 #define USE_SERIALRX_JETIEXBUS
 #define USE_SENSOR_NAMES
+#define USE_SERVOS
 #define USE_VIRTUAL_CURRENT_METER
 #define VTX_COMMON
 #define VTX_CONTROL

src/main/telemetry/crsf.c

@@ -24,6 +24,7 @@
 #ifdef TELEMETRY
 
 #include "config/feature.h"
+#include "build/build_config.h"
 #include "build/version.h"
 
 #include "config/parameter_group.h"
@@ -31,6 +32,7 @@
 
 #include "common/crc.h"
 #include "common/maths.h"
+#include "common/printf.h"
 #include "common/streambuf.h"
 #include "common/utils.h"
 
@@ -51,12 +53,17 @@
 
 #include "telemetry/telemetry.h"
 #include "telemetry/crsf.h"
+#include "telemetry/msp_shared.h"
 
 #include "fc/config.h"
 
 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
+#define CRSF_DEVICEINFO_VERSION             0x01
+#define CRSF_DEVICEINFO_PARAMETER_COUNT     255
 
 static bool crsfTelemetryEnabled;
+static bool deviceInfoReplyPending;
+static bool mspReplyPending;
 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
 
 static void crsfInitializeFrame(sbuf_t *dst)
@@ -223,6 +230,41 @@ void crsfFrameFlightMode(sbuf_t *dst)
     *lengthPtr = sbufPtr(dst) - lengthPtr;
 }
 
+void scheduleDeviceInfoResponse() {
+    deviceInfoReplyPending = true;
+}
+
+/*
+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) {
+
+    char buff[30];
+    tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
+
+    uint8_t *lengthPtr = sbufPtr(dst);
+    sbufWriteU8(dst, 0);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
+    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT);
+    sbufWriteStringWithZeroTerminator(dst, buff);
+    for (unsigned int ii=0; ii<12; ii++) {
+        sbufWriteU8(dst, 0x00);
+    }
+    sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
+    sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
+    *lengthPtr = sbufPtr(dst) - lengthPtr;
+}
+
 #define BV(x)  (1 << (x)) // bit value
 
 // schedule array to decide how often each type of frame is sent
@@ -238,6 +280,25 @@ typedef enum {
 static uint8_t crsfScheduleCount;
 static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 
+void scheduleMspResponse() {
+    if (!mspReplyPending) {
+        mspReplyPending = true;
+    }
+}
+
+void crsfSendMspResponse(uint8_t *payload)
+{
+    sbuf_t crsfPayloadBuf;
+    sbuf_t *dst = &crsfPayloadBuf;
+
+    crsfInitializeFrame(dst);
+    sbufWriteU8(dst, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
+    sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
+    sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
+    sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT);
+    sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE);
+    crsfFinalize(dst);
+ }
 
 static void processCrsf(void)
 {
@@ -257,6 +318,7 @@ static void processCrsf(void)
         crsfFrameBatterySensor(dst);
         crsfFinalize(dst);
     }
+
     if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
         crsfInitializeFrame(dst);
         crsfFrameFlightMode(dst);
@@ -277,6 +339,10 @@ 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();
+
+    deviceInfoReplyPending = false;
+    mspReplyPending = false;
+
     int index = 0;
     crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
     crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
@@ -311,8 +377,19 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
     if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
         crsfLastCycleTime = currentTimeUs;
+        if (deviceInfoReplyPending) {
+            sbuf_t crsfPayloadBuf;
+            sbuf_t *dst = &crsfPayloadBuf;
+            crsfInitializeFrame(dst);
+            crsfFrameDeviceInfo(dst);
+            crsfFinalize(dst);
+            deviceInfoReplyPending = false;
+        } else if (mspReplyPending) {
+            mspReplyPending = sendMspReply(CRSF_FRAME_TX_MSP_PAYLOAD_SIZE, &crsfSendMspResponse);
+        } else {
             processCrsf();
         }
+    }
 }
 
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)

src/main/telemetry/crsf.h

@@ -19,9 +19,12 @@
 
 #include "common/time.h"
 #include "rx/crsf.h"
+#include "telemetry/msp_shared.h"
 
 void initCrsfTelemetry(void);
 bool checkCrsfTelemetryState(void);
 void handleCrsfTelemetry(timeUs_t currentTimeUs);
+void scheduleDeviceInfoResponse();
+void scheduleMspResponse();
 
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);

src/main/telemetry/msp_shared.c

@@ -0,0 +1,231 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef TELEMETRY
+
+#include "build/build_config.h"
+
+#include "common/utils.h"
+
+#include "fc/fc_msp.h"
+
+#include "msp/msp.h"
+
+#include "rx/crsf.h"
+#include "rx/msp.h"
+
+#include "telemetry/msp_shared.h"
+#include "telemetry/smartport.h"
+
+#define TELEMETRY_MSP_VERSION    1
+#define TELEMETRY_MSP_VER_SHIFT  5
+#define TELEMETRY_MSP_VER_MASK   (0x7 << TELEMETRY_MSP_VER_SHIFT)
+#define TELEMETRY_MSP_ERROR_FLAG (1 << 5)
+#define TELEMETRY_MSP_START_FLAG (1 << 4)
+#define TELEMETRY_MSP_SEQ_MASK   0x0F
+#define TELEMETRY_MSP_RES_ERROR (-10)
+
+enum {
+    TELEMETRY_MSP_VER_MISMATCH=0,
+    TELEMETRY_MSP_CRC_ERROR=1,
+    TELEMETRY_MSP_ERROR=2
+};
+
+#define REQUEST_BUFFER_SIZE 64
+#define RESPONSE_BUFFER_SIZE 64
+
+STATIC_UNIT_TESTED uint8_t checksum = 0;
+STATIC_UNIT_TESTED mspPackage_t mspPackage;
+static mspRxBuffer_t mspRxBuffer;
+static mspTxBuffer_t mspTxBuffer;
+static mspPacket_t mspRxPacket;
+static mspPacket_t mspTxPacket;
+
+void initSharedMsp() {
+    mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer;
+    mspPackage.requestPacket = &mspRxPacket;
+    mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer;
+    mspPackage.requestPacket->buf.end = mspPackage.requestBuffer;
+
+    mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer;
+    mspPackage.responsePacket = &mspTxPacket;
+    mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+}
+
+static void processMspPacket()
+{
+    mspPackage.responsePacket->cmd = 0;
+    mspPackage.responsePacket->result = 0;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+
+    mspPostProcessFnPtr mspPostProcessFn = NULL;
+    if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
+        sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
+    }
+    if (mspPostProcessFn) {
+        mspPostProcessFn(NULL);
+    }
+
+    sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+}
+
+void sendMspErrorResponse(uint8_t error, int16_t cmd)
+{
+    mspPackage.responsePacket->cmd = cmd;
+    mspPackage.responsePacket->result = 0;
+    mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
+
+    sbufWriteU8(&mspPackage.responsePacket->buf, error);
+    mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR;
+    sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+}
+
+bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd)
+{
+    static uint8_t mspStarted = 0;
+    static uint8_t lastSeq = 0;
+
+    if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
+        return false;
+    }
+
+    if (mspStarted == 0) {
+        initSharedMsp();
+    }
+
+    mspPacket_t *packet = mspPackage.requestPacket;
+    sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd);
+    sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
+    uint8_t header = sbufReadU8(frameBuf);
+    uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
+    uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT;
+
+    if (version != TELEMETRY_MSP_VERSION) {
+        sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
+        return true;
+    }
+
+    if (header & TELEMETRY_MSP_START_FLAG) {
+        // first packet in sequence
+        uint8_t mspPayloadSize = sbufReadU8(frameBuf);
+
+        packet->cmd = sbufReadU8(frameBuf);
+        packet->result = 0;
+        packet->buf.ptr = mspPackage.requestBuffer;
+        packet->buf.end = mspPackage.requestBuffer + mspPayloadSize;
+
+        checksum = mspPayloadSize ^ packet->cmd;
+        mspStarted = 1;
+    } else if (!mspStarted) {
+        // no start packet yet, throw this one away
+        return false;
+    } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) {
+        // packet loss detected!
+        mspStarted = 0;
+        return false;
+    }
+
+    uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
+    uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
+    uint8_t payload[frameBytesRemaining];
+
+    if (bufferBytesRemaining >= frameBytesRemaining) {
+        sbufReadData(frameBuf, payload, frameBytesRemaining);
+        sbufAdvance(frameBuf, frameBytesRemaining);
+        sbufWriteData(rxBuf, payload, frameBytesRemaining);
+        lastSeq = seqNumber;
+        return false;
+    } else {
+        sbufReadData(frameBuf, payload, bufferBytesRemaining);
+        sbufAdvance(frameBuf, bufferBytesRemaining);
+        sbufWriteData(rxBuf, payload, bufferBytesRemaining);
+        sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
+        while (sbufBytesRemaining(rxBuf)) {
+            checksum ^= sbufReadU8(rxBuf);
+        }
+
+        if (checksum != *frameBuf->ptr) {
+            mspStarted = 0;
+            sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd);
+            return true;
+        }
+    }
+
+    mspStarted = 0;
+    sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
+    processMspPacket();
+    return true;
+}
+
+bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn)
+{
+    static uint8_t checksum = 0;
+    static uint8_t seq = 0;
+
+    uint8_t payloadOut[payloadSize];
+    sbuf_t payload;
+    sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize);
+    sbuf_t *txBuf = &mspPackage.responsePacket->buf;
+
+    // detect first reply packet
+    if (txBuf->ptr == mspPackage.responseBuffer) {
+
+        // header
+        uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK);
+        if (mspPackage.responsePacket->result < 0) {
+            head |= TELEMETRY_MSP_ERROR_FLAG;
+        }
+        sbufWriteU8(payloadBuf, head);
+
+        uint8_t size = sbufBytesRemaining(txBuf);
+        sbufWriteU8(payloadBuf, size);
+    }
+    else {
+        // header
+        sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK));
+    }
+
+    uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf);
+    uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf);
+    uint8_t frame[payloadBytesRemaining];
+
+    if (bufferBytesRemaining >= payloadBytesRemaining) {
+
+        sbufReadData(txBuf, frame, payloadBytesRemaining);
+        sbufAdvance(txBuf, payloadBytesRemaining);
+        sbufWriteData(payloadBuf, frame, payloadBytesRemaining);
+        responseFn(payloadOut);
+
+        return true;
+
+    } else {
+
+        sbufReadData(txBuf, frame, bufferBytesRemaining);
+        sbufAdvance(txBuf, bufferBytesRemaining);
+        sbufWriteData(payloadBuf, frame, bufferBytesRemaining);
+        sbufSwitchToReader(txBuf, mspPackage.responseBuffer);
+
+        checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd;
+
+        while (sbufBytesRemaining(txBuf)) {
+            checksum ^= sbufReadU8(txBuf);
+        }
+        sbufWriteU8(payloadBuf, checksum);
+
+        while (sbufBytesRemaining(payloadBuf)>1) {
+            sbufWriteU8(payloadBuf, 0);
+        }
+
+    }
+
+    responseFn(payloadOut);
+    return false;
+}
+
+#endif

src/main/telemetry/msp_shared.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "msp/msp.h"
+#include "rx/crsf.h"
+#include "telemetry/smartport.h"
+
+typedef void (*mspResponseFnPtr)(uint8_t *payload);
+
+typedef struct mspPackage_s {
+    sbuf_t requestFrame;
+    uint8_t *requestBuffer;
+    uint8_t *responseBuffer;
+    mspPacket_t *requestPacket;
+    mspPacket_t *responsePacket;
+} mspPackage_t;
+
+typedef union mspRxBuffer_u {
+    uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
+    uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE];
+} mspRxBuffer_t;
+
+typedef union mspTxBuffer_u {
+    uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE];
+    uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE];
+} mspTxBuffer_t;
+
+void initSharedMsp();
+bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
+bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);

src/main/telemetry/smartport.c

@@ -43,8 +43,6 @@
 #include "io/gps.h"
 #include "io/serial.h"
 
-#include "msp/msp.h"
-
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
@@ -54,10 +52,10 @@
 #include "sensors/gyro.h"
 
 #include "rx/rx.h"
-#include "rx/msp.h"
 
 #include "telemetry/telemetry.h"
 #include "telemetry/smartport.h"
+#include "telemetry/msp_shared.h"
 
 enum
 {
@@ -164,7 +162,6 @@ typedef struct smartPortFrame_s {
 } __attribute__((packed)) smartPortFrame_t;
 
 #define SMARTPORT_FRAME_SIZE  sizeof(smartPortFrame_t)
-#define SMARTPORT_TX_BUF_SIZE 256
 
 #define SMARTPORT_PAYLOAD_OFFSET offsetof(smartPortFrame_t, valueId)
 #define SMARTPORT_PAYLOAD_SIZE   (SMARTPORT_FRAME_SIZE - SMARTPORT_PAYLOAD_OFFSET - 1)
@@ -172,30 +169,8 @@ typedef struct smartPortFrame_s {
 static smartPortFrame_t smartPortRxBuffer;
 static uint8_t smartPortRxBytes = 0;
 static bool smartPortFrameReceived = false;
-
-#define SMARTPORT_MSP_VERSION    1
-#define SMARTPORT_MSP_VER_SHIFT  5
-#define SMARTPORT_MSP_VER_MASK   (0x7 << SMARTPORT_MSP_VER_SHIFT)
-#define SMARTPORT_MSP_VERSION_S  (SMARTPORT_MSP_VERSION << SMARTPORT_MSP_VER_SHIFT)
-
-#define SMARTPORT_MSP_ERROR_FLAG (1 << 5)
-#define SMARTPORT_MSP_START_FLAG (1 << 4)
-#define SMARTPORT_MSP_SEQ_MASK   0x0F
-
-#define SMARTPORT_MSP_RX_BUF_SIZE 64
-
-static uint8_t smartPortMspTxBuffer[SMARTPORT_TX_BUF_SIZE];
-static mspPacket_t smartPortMspReply;
 static bool smartPortMspReplyPending = false;
 
-#define SMARTPORT_MSP_RES_ERROR (-10)
-
-enum {
-    SMARTPORT_MSP_VER_MISMATCH=0,
-    SMARTPORT_MSP_CRC_ERROR=1,
-    SMARTPORT_MSP_ERROR=2
-};
-
 static void smartPortDataReceive(uint16_t c)
 {
     static bool skipUntilStart = true;
@@ -352,194 +327,8 @@ void checkSmartPortTelemetryState(void)
         freeSmartPortTelemetryPort();
 }
 
-static void initSmartPortMspReply(int16_t cmd)
+void smartPortSendMspResponse(uint8_t *payload) {
-{
+    smartPortSendPackageEx(FSSP_MSPS_FRAME, payload);
-    smartPortMspReply.buf.ptr    = smartPortMspTxBuffer;
-    smartPortMspReply.buf.end    = ARRAYEND(smartPortMspTxBuffer);
-
-    smartPortMspReply.cmd    = cmd;
-    smartPortMspReply.result = 0;
-}
-
-static void processMspPacket(mspPacket_t* packet)
-{
-    initSmartPortMspReply(0);
-
-    if (mspFcProcessCommand(packet, &smartPortMspReply, NULL) == MSP_RESULT_ERROR) {
-        sbufWriteU8(&smartPortMspReply.buf, SMARTPORT_MSP_ERROR);
-    }
-
-    // change streambuf direction
-    sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer);
-    smartPortMspReplyPending = true;
-}
-
-/**
- * Request frame format:
- * - Header: 1 byte
- *   - Reserved: 2 bits (future use)
- *   - Error-flag: 1 bit
- *   - Start-flag: 1 bit
- *   - CSeq: 4 bits
- *
- * - MSP payload:
- *   - if Error-flag == 0:
- *     - size: 1 byte
- *     - payload
- *     - CRC (request type included)
- *   - if Error-flag == 1:
- *     - size: 1 byte (== 1)
- *     - error: 1 Byte
- *       - 0: Version mismatch (type=0)
- *       - 1: Sequence number error
- *       - 2: MSP error
- *     - CRC (request type included)
- */
-bool smartPortSendMspReply()
-{
-    static uint8_t checksum = 0;
-    static uint8_t seq = 0;
-
-    uint8_t packet[SMARTPORT_PAYLOAD_SIZE];
-    uint8_t* p = packet;
-    uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE;
-
-    sbuf_t* txBuf = &smartPortMspReply.buf;
-
-    // detect first reply packet
-    if (txBuf->ptr == smartPortMspTxBuffer) {
-
-        // header
-        uint8_t head = SMARTPORT_MSP_START_FLAG | (seq++ & SMARTPORT_MSP_SEQ_MASK);
-        if (smartPortMspReply.result < 0) {
-            head |= SMARTPORT_MSP_ERROR_FLAG;
-        }
-        *p++ = head;
-
-        uint8_t size = sbufBytesRemaining(txBuf);
-        *p++ = size;
-
-        checksum = size ^ smartPortMspReply.cmd;
-    }
-    else {
-        // header
-        *p++ = (seq++ & SMARTPORT_MSP_SEQ_MASK);
-    }
-
-    while ((p < end) && (sbufBytesRemaining(txBuf) > 0)) {
-        *p = sbufReadU8(txBuf);
-        checksum ^= *p++; // MSP checksum
-    }
-
-    // to be continued...
-    if (p == end) {
-        smartPortSendPackageEx(FSSP_MSPS_FRAME,packet);
-        return true;
-    }
-
-    // nothing left in txBuf,
-    // append the MSP checksum
-    *p++ = checksum;
-
-    // pad with zeros
-    while (p < end)
-        *p++ = 0;
-
-    smartPortSendPackageEx(FSSP_MSPS_FRAME,packet);
-    return false;
-}
-
-void smartPortSendErrorReply(uint8_t error, int16_t cmd)
-{
-    initSmartPortMspReply(cmd);
-    sbufWriteU8(&smartPortMspReply.buf,error);
-    smartPortMspReply.result = SMARTPORT_MSP_RES_ERROR;
-
-    sbufSwitchToReader(&smartPortMspReply.buf, smartPortMspTxBuffer);
-    smartPortMspReplyPending = true;
-}
-
-/**
- * Request frame format:
- * - Header: 1 byte
- *   - Version: 3 bits
- *   - Start-flag: 1 bit
- *   - CSeq: 4 bits
- *
- * - MSP payload:
- *   - Size: 1 Byte
- *   - Type: 1 Byte
- *   - payload...
- *   - CRC
- */
-void handleSmartPortMspFrame(smartPortFrame_t* sp_frame)
-{
-    static uint8_t mspBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
-    static uint8_t mspStarted = 0;
-    static uint8_t lastSeq = 0;
-    static uint8_t checksum = 0;
-    static mspPacket_t cmd;
-
-    // re-assemble MSP frame & forward to MSP port when complete
-    uint8_t* p = ((uint8_t*)sp_frame) + SMARTPORT_PAYLOAD_OFFSET;
-    uint8_t* end = p + SMARTPORT_PAYLOAD_SIZE;
-
-    uint8_t head = *p++;
-    uint8_t seq = head & SMARTPORT_MSP_SEQ_MASK;
-    uint8_t version = (head & SMARTPORT_MSP_VER_MASK) >> SMARTPORT_MSP_VER_SHIFT;
-
-    if (version != SMARTPORT_MSP_VERSION) {
-        mspStarted = 0;
-        smartPortSendErrorReply(SMARTPORT_MSP_VER_MISMATCH,0);
-        return;
-    }
-
-    // check start-flag
-    if (head & SMARTPORT_MSP_START_FLAG) {
-
-        //TODO: if (p_size > SMARTPORT_MSP_RX_BUF_SIZE) error!
-        uint8_t p_size = *p++;
-        cmd.cmd = *p++;
-        cmd.result = 0;
-
-        cmd.buf.ptr = mspBuffer;
-        cmd.buf.end = mspBuffer + p_size;
-
-        checksum = p_size ^ cmd.cmd;
-        mspStarted = 1;
-    } else if (!mspStarted) {
-        // no start packet yet, throw this one away
-        return;
-    } else if (((lastSeq + 1) & SMARTPORT_MSP_SEQ_MASK) != seq) {
-        // packet loss detected!
-        mspStarted = 0;
-        return;
-    }
-
-    // copy payload bytes
-    while ((p < end) && sbufBytesRemaining(&cmd.buf)) {
-        checksum ^= *p;
-        sbufWriteU8(&cmd.buf,*p++);
-    }
-
-    // reached end of smart port frame
-    if (p == end) {
-        lastSeq = seq;
-        return;
-    }
-
-    // last byte must be the checksum
-    if (checksum != *p) {
-        mspStarted = 0;
-        smartPortSendErrorReply(SMARTPORT_MSP_CRC_ERROR,cmd.cmd);
-        return;
-    }
-
-    // end of MSP packet reached
-    mspStarted = 0;
-    sbufSwitchToReader(&cmd.buf,mspBuffer);
-
-    processMspPacket(&cmd);
 }
 
 void handleSmartPortTelemetry(void)
@@ -566,7 +355,11 @@ void handleSmartPortTelemetry(void)
         if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
 
             // Pass only the payload: skip sensorId & frameId
-            handleSmartPortMspFrame(&smartPortRxBuffer);
+            if (!smartPortMspReplyPending) {
+                uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET;
+                uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE;
+                smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd);
+            }
         }
     }
 
@@ -578,7 +371,7 @@ void handleSmartPortTelemetry(void)
         }
 
         if (smartPortMspReplyPending) {
-            smartPortMspReplyPending = smartPortSendMspReply();
+            smartPortMspReplyPending = sendMspReply(SMARTPORT_PAYLOAD_SIZE, &smartPortSendMspResponse);
             smartPortHasRequest = 0;
             return;
         }

src/main/telemetry/smartport.h

@@ -7,6 +7,9 @@
 
 #pragma once
 
+#define SMARTPORT_MSP_TX_BUF_SIZE 256
+#define SMARTPORT_MSP_RX_BUF_SIZE 64
+
 void initSmartPortTelemetry(void);
 
 void handleSmartPortTelemetry(void);

src/main/telemetry/telemetry.c

@@ -52,6 +52,7 @@
 #include "telemetry/crsf.h"
 #include "telemetry/srxl.h"
 #include "telemetry/ibus.h"
+#include "telemetry/msp_shared.h"
 
 
 PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
@@ -100,6 +101,7 @@ void telemetryInit(void)
 #ifdef TELEMETRY_IBUS
     initIbusTelemetry();
 #endif
+    initSharedMsp();
 
     telemetryCheckState();
 }

src/test/Makefile

@@ -172,7 +172,10 @@ rc_controls_unittest_SRC := \
 rx_crsf_unittest_SRC := \
 		$(USER_DIR)/rx/crsf.c \
 		$(USER_DIR)/common/crc.c \
-		$(USER_DIR)/common/streambuf.c
+		$(USER_DIR)/common/printf.c \
+		$(USER_DIR)/common/typeconversion.c \
+		$(USER_DIR)/common/streambuf.c \
+		$(USER_DIR)/drivers/serial.c
 
 
 rx_ibus_unittest_SRC := \
@@ -207,8 +210,24 @@ telemetry_crsf_unittest_SRC := \
 		$(USER_DIR)/common/maths.c \
 		$(USER_DIR)/common/streambuf.c \
 		$(USER_DIR)/common/gps_conversion.c \
+		$(USER_DIR)/common/printf.c \
+		$(USER_DIR)/common/typeconversion.c \
 		$(USER_DIR)/fc/runtime_config.c
 
+telemetry_crsf_msp_unittest_SRC := \
+		$(USER_DIR)/rx/crsf.c \
+		$(USER_DIR)/common/crc.c \
+		$(USER_DIR)/common/streambuf.c \
+		$(USER_DIR)/common/printf.c \
+		$(USER_DIR)/common/streambuf.c \
+		$(USER_DIR)/drivers/serial.c \
+		$(USER_DIR)/common/typeconversion.c \
+		$(USER_DIR)/telemetry/crsf.c \
+		$(USER_DIR)/common/gps_conversion.c \
+		$(USER_DIR)/telemetry/msp_shared.c \
+		$(USER_DIR)/fc/runtime_config.c
+
+
 telemetry_crsf_unittest_DEFINES := \
 		FLASH_SIZE=128 \
 		STM32F10X_MD \
@@ -254,6 +273,9 @@ huffman_unittest_SRC := \
 huffman_unittest_DEFINES := \
 		USE_HUFFMAN
 
+ringbuffer_unittest_SRC := \
+		$(USER_DIR)/common/ringbuffer.c
+
 # Please tweak the following variable definitions as needed by your
 # project, except GTEST_HEADERS, which you can use in your own targets
 # but shouldn't modify.

src/test/unit/rx_crsf_unittest.cc

@@ -29,11 +29,14 @@ extern "C" {
     #include "common/crc.h"
     #include "common/utils.h"
 
+    #include "drivers/serial.h"
     #include "io/serial.h"
 
     #include "rx/rx.h"
     #include "rx/crsf.h"
 
+    #include "telemetry/msp_shared.h"
+
     void crsfDataReceive(uint16_t c);
     uint8_t crsfFrameCRC(void);
     uint8_t crsfFrameStatus(void);
@@ -277,7 +280,9 @@ int16_t debug[DEBUG16_VALUE_COUNT];
 uint32_t micros(void) {return dummyTimeUs;}
 serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
 serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
-void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
 bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
 serialPort_t *telemetrySharedPort = NULL;
+void scheduleDeviceInfoResponse(void) {};
+void scheduleMspResponse(mspPackage_t *package) { UNUSED(package); };
+bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
 }

src/test/unit/telemetry_crsf_msp_unittest.cc

@@ -0,0 +1,299 @@
+/*
+ * 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 <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include <limits.h>
+#include <algorithm>
+
+extern "C" {
+    #include <platform.h>
+
+    #include "build/debug.h"
+
+    #include "common/crc.h"
+    #include "common/utils.h"
+    #include "common/printf.h"
+    #include "common/gps_conversion.h"
+    #include "common/streambuf.h"
+    #include "common/typeconversion.h"
+
+    #include "config/parameter_group.h"
+    #include "config/parameter_group_ids.h"
+
+    #include "drivers/serial.h"
+    #include "drivers/system.h"
+
+    #include "fc/runtime_config.h"
+    #include "fc/config.h"
+    #include "flight/imu.h"
+    #include "fc/fc_msp.h"
+
+    #include "io/serial.h"
+    #include "io/gps.h"
+
+    #include "msp/msp.h"
+
+    #include "rx/rx.h"
+    #include "rx/crsf.h"
+
+    #include "sensors/battery.h"
+    #include "sensors/sensors.h"
+
+    #include "telemetry/telemetry.h"
+    #include "telemetry/msp_shared.h"
+    #include "telemetry/smartport.h"
+
+    bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
+    bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
+    uint8_t sbufReadU8(sbuf_t *src);
+    int sbufBytesRemaining(sbuf_t *buf);
+    void initSharedMsp();
+    uint16_t testBatteryVoltage = 0;
+    int32_t testAmperage = 0;
+    uint8_t mspTxData[64]; //max frame size
+    sbuf_t mspTxDataBuf;
+    uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX];
+    uint8_t payloadOutput[64];
+    sbuf_t payloadOutputBuf;
+    int32_t testmAhDrawn = 0;
+
+    PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
+    PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
+    PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
+
+    extern bool crsfFrameDone;
+    extern crsfFrame_t crsfFrame;
+    extern mspPackage_t mspPackage;
+    extern uint8_t checksum;
+
+    uint32_t dummyTimeUs;
+
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+typedef struct crsfMspFrame_s {
+    uint8_t deviceAddress;
+    uint8_t frameLength;
+    uint8_t type;
+    uint8_t destination;
+    uint8_t origin;
+    uint8_t payload[CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC];
+} crsfMspFrame_t;
+
+const uint8_t crsfPidRequest[] = {
+    0x00,0x0D,0x7A,0xC8,0xEA,0x30,0x00,0x70,0x70,0x00,0x00,0x00,0x00,0x69
+};
+
+TEST(CrossFireMSPTest, RequestBufferTest)
+{
+    initSharedMsp();
+    const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
+    crsfFrame = *(const crsfFrame_t*)framePtr;
+    crsfFrameDone = true;
+    EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
+    EXPECT_EQ(CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE, crsfFrame.frame.frameLength);
+    EXPECT_EQ(CRSF_FRAMETYPE_MSP_REQ, crsfFrame.frame.type);
+    uint8_t *destination = (uint8_t *)&crsfFrame.frame.payload;
+    uint8_t *origin = (uint8_t *)&crsfFrame.frame.payload + 1;
+    uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
+    EXPECT_EQ(0xC8, *destination);
+    EXPECT_EQ(0xEA, *origin);
+    EXPECT_EQ(0x30, *frameStart);
+    EXPECT_EQ(0x69, *frameEnd);
+}
+
+TEST(CrossFireMSPTest, ResponsePacketTest)
+{
+    initSharedMsp();
+    const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
+    crsfFrame = *(const crsfFrame_t*)framePtr;
+    crsfFrameDone = true;
+    uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
+    handleMspFrame(frameStart, frameEnd);
+    for (unsigned int ii=1; ii<30; ii++) {
+        EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
+    }
+    sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
+}
+
+const uint8_t crsfPidWrite1[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32};
+const uint8_t crsfPidWrite2[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00};
+const uint8_t crsfPidWrite3[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19};
+const uint8_t crsfPidWrite4[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00};
+const uint8_t crsfPidWrite5[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00};
+
+TEST(CrossFireMSPTest, WriteResponseTest)
+{
+    initSharedMsp();
+    const crsfMspFrame_t *framePtr1 = (const crsfMspFrame_t*)crsfPidWrite1;
+    crsfFrame = *(const crsfFrame_t*)framePtr1;
+    crsfFrameDone = true;
+    uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
+    bool pending1 = handleMspFrame(frameStart, frameEnd);
+    EXPECT_FALSE(pending1); // not done yet*/
+    EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
+    EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
+    EXPECT_EQ(0x1E, mspPackage.requestBuffer[2]);
+    EXPECT_EQ(0x3A, mspPackage.requestBuffer[3]);
+    EXPECT_EQ(0x32, mspPackage.requestBuffer[4]);
+
+    const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2;
+    crsfFrame = *(const crsfFrame_t*)framePtr2;
+    crsfFrameDone = true;
+    uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd2 = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
+    bool pending2 = handleMspFrame(frameStart2, frameEnd2);
+    EXPECT_FALSE(pending2); // not done yet
+    EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
+    EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
+    EXPECT_EQ(0x2D, mspPackage.requestBuffer[7]);
+    EXPECT_EQ(0x14, mspPackage.requestBuffer[8]);
+    EXPECT_EQ(0x32, mspPackage.requestBuffer[9]);
+    EXPECT_EQ(0x00, mspPackage.requestBuffer[10]);
+    EXPECT_EQ(0x00, mspPackage.requestBuffer[11]);
+
+    const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3;
+    crsfFrame = *(const crsfFrame_t*)framePtr3;
+    crsfFrameDone = true;
+    uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd3 = frameStart3 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
+    bool pending3 = handleMspFrame(frameStart3, frameEnd3);
+    EXPECT_FALSE(pending3); // not done yet
+    EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
+    EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
+    EXPECT_EQ(0x00, mspPackage.requestBuffer[14]);
+    EXPECT_EQ(0x22, mspPackage.requestBuffer[15]);
+    EXPECT_EQ(0x0E, mspPackage.requestBuffer[16]);
+    EXPECT_EQ(0x35, mspPackage.requestBuffer[17]);
+    EXPECT_EQ(0x19, mspPackage.requestBuffer[18]);
+
+    const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4;
+    crsfFrame = *(const crsfFrame_t*)framePtr4;
+    crsfFrameDone = true;
+    uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd4 = frameStart4 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
+    bool pending4 = handleMspFrame(frameStart4, frameEnd4);
+    EXPECT_FALSE(pending4); // not done yet
+    EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
+    EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
+    EXPECT_EQ(0x32, mspPackage.requestBuffer[21]);
+    EXPECT_EQ(0x32, mspPackage.requestBuffer[22]);
+    EXPECT_EQ(0x4B, mspPackage.requestBuffer[23]);
+    EXPECT_EQ(0x28, mspPackage.requestBuffer[24]);
+    EXPECT_EQ(0x00, mspPackage.requestBuffer[25]);
+    //EXPECT_EQ(0xB3,checksum);
+
+    const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5;
+    crsfFrame = *(const crsfFrame_t*)framePtr5;
+    crsfFrameDone = true;
+    uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd5 = frameStart2 + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE;
+    bool pending5 = handleMspFrame(frameStart5, frameEnd5);
+    EXPECT_TRUE(pending5); // not done yet
+    EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
+    EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
+    EXPECT_EQ(0x37, mspPackage.requestBuffer[28]);
+    EXPECT_EQ(0x4B, mspPackage.requestBuffer[29]);
+    EXPECT_EQ(0xF8,checksum);
+
+}
+
+void testSendMspResponse(uint8_t *payload) {
+    sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64);
+    sbufWriteData(plOut, payload, *payload + 64);
+    sbufSwitchToReader(&payloadOutputBuf, payloadOutput);
+}
+
+TEST(CrossFireMSPTest, SendMspReply) {
+    initSharedMsp();
+    const crsfMspFrame_t *framePtr = (const crsfMspFrame_t*)crsfPidRequest;
+    crsfFrame = *(const crsfFrame_t*)framePtr;
+    crsfFrameDone = true;
+    uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
+    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + 2;
+    bool handled = handleMspFrame(frameStart, frameEnd);
+    EXPECT_TRUE(handled);
+    bool replyPending = sendMspReply(64, &testSendMspResponse);
+    EXPECT_FALSE(replyPending);
+    EXPECT_EQ(0x10, sbufReadU8(&payloadOutputBuf));
+    EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf));
+    for (unsigned int ii=1; ii<=30; ii++) {
+        EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf));
+    }
+    EXPECT_EQ(0x71, sbufReadU8(&payloadOutputBuf)); // CRC
+}
+
+// STUBS
+
+extern "C" {
+
+    gpsSolutionData_t gpsSol;
+    attitudeEulerAngles_t attitude = { { 0, 0, 0 } };
+
+    uint32_t micros(void) {return dummyTimeUs;}
+    serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
+    serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
+    uint16_t getBatteryVoltage(void) {
+        return testBatteryVoltage;
+    }
+    int32_t getAmperage(void) {
+        return testAmperage;
+    }
+
+    uint8_t calculateBatteryPercentageRemaining(void) {
+        return 67;
+    }
+
+    bool feature(uint32_t) {return false;}
+
+    bool isAirmodeActive(void) {return true;}
+
+    mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn) {
+
+        UNUSED(mspPostProcessFn);
+
+        sbuf_t *dst = &reply->buf;
+        //sbuf_t *src = &cmd->buf;
+        const uint8_t cmdMSP = cmd->cmd;
+        reply->cmd = cmd->cmd;
+
+        if (cmdMSP == 0x70) {
+            for (unsigned int ii=1; ii<=30; ii++) {
+                sbufWriteU8(dst, ii);
+            }
+        } else if (cmdMSP == 0xCA) {
+            return MSP_RESULT_ACK;
+        }
+
+        return MSP_RESULT_ACK;
+    }
+
+    void beeperConfirmationBeeps(uint8_t ) {}
+
+    int32_t getMAhDrawn(void) {
+      return testmAhDrawn;
+    }
+
+}

src/test/unit/telemetry_crsf_unittest.cc

@@ -31,6 +31,8 @@ extern "C" {
     #include "common/filter.h"
     #include "common/gps_conversion.h"
     #include "common/maths.h"
+    #include "common/printf.h"
+    #include "common/typeconversion.h"
 
     #include "config/parameter_group.h"
     #include "config/parameter_group_ids.h"
@@ -38,6 +40,7 @@ extern "C" {
     #include "drivers/serial.h"
     #include "drivers/system.h"
 
+    #include "fc/config.h"
     #include "fc/runtime_config.h"
 
     #include "flight/pid.h"
@@ -53,6 +56,7 @@ extern "C" {
 
     #include "telemetry/crsf.h"
     #include "telemetry/telemetry.h"
+    #include "telemetry/msp_shared.h"
 
     bool airMode;
 
@@ -63,6 +67,7 @@ extern "C" {
     serialPort_t *telemetrySharedPort;
     PG_REGISTER(batteryConfig_t, batteryConfig, PG_BATTERY_CONFIG, 0);
     PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
+    PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
 }
 
 #include "unittest_macros.h"
@@ -293,6 +298,7 @@ void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
 void serialSetMode(serialPort_t *, portMode_e) {}
 serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_e, portOptions_e) {return NULL;}
 void closeSerialPort(serialPort_t *) {}
+bool isSerialTransmitBufferEmpty(const serialPort_t *) { return true; }
 
 serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;}
 
@@ -323,4 +329,7 @@ int32_t getMAhDrawn(void){
   return testmAhDrawn;
 }
 
+bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
+bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
+
 }