Created buffer for msp frames and improved msp over telemetry performance for CRSF.

src/main/rx/crsf.c

@@ -44,7 +44,6 @@
 #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
@@ -112,6 +111,15 @@ struct crsfPayloadRcChannelsPacked_s {
 
 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 
+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;
+}
 
 // Receive ISR callback, called back from serial port
 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
@@ -141,18 +149,19 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
         crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
         if (crsfFrameDone) {
             crsfFramePosition = 0;
+#if defined(USE_MSP_OVER_TELEMETRY)
+            if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) {
+                const uint8_t crc = crsfFrameCRC();
+                if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
+                    uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
+                    if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
+                        crsfScheduleMspResponse();
                     }
                 }
             }
-
+#endif
-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)
@@ -185,22 +194,9 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
             crsfChannelData[14] = rcChannels->chan14;
             crsfChannelData[15] = rcChannels->chan15;
             return RX_FRAME_COMPLETE;
-        } else {
+        } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) {
-            if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) {
-                // TODO: CRC CHECK
             crsfScheduleDeviceInfoResponse();
             return RX_FRAME_COMPLETE;
-#if defined(USE_MSP_OVER_TELEMETRY)
-            } 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)) {
-                    crsfScheduleMspResponse();
-                }
-                return RX_FRAME_COMPLETE;
-#endif
-            }
         }
     }
     return RX_FRAME_PENDING;

src/main/rx/crsf.h

@@ -45,8 +45,9 @@ 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_TX_MSP_FRAME_SIZE = 58,
-    CRSF_FRAME_RX_MSP_PAYLOAD_SIZE = 8,
+    CRSF_FRAME_RX_MSP_FRAME_SIZE = 8,
+    CRSF_FRAME_ORIGIN_DEST_SIZE = 2,
     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

src/main/target/STM32F3DISCOVERY/target.h

@@ -33,7 +33,6 @@
 
 #undef TELEMETRY_JETIEXBUS // ROM SAVING
 
-
 #define CURRENT_TARGET_CPU_VOLTAGE 3.0
 
 #define LED0_PIN                PE8  // Blue LEDs - PE8/PE12

src/main/telemetry/crsf.c

@@ -24,6 +24,7 @@
 #ifdef TELEMETRY
 
 #include "config/feature.h"
+#include "build/atomic.h"
 #include "build/build_config.h"
 #include "build/version.h"
 
@@ -36,6 +37,8 @@
 #include "common/streambuf.h"
 #include "common/utils.h"
 
+#include "drivers/nvic.h"
+
 #include "sensors/battery.h"
 
 #include "io/gps.h"
@@ -59,12 +62,65 @@
 
 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
 #define CRSF_DEVICEINFO_VERSION             0x01
-#define CRSF_DEVICEINFO_PARAMETER_COUNT     255
+#define CRSF_DEVICEINFO_PARAMETER_COUNT     0
+
+#define CRSF_MSP_BUFFER_SIZE 96
+#define CRSF_MSP_LENGTH_OFFSET 1
 
 static bool crsfTelemetryEnabled;
 static bool deviceInfoReplyPending;
 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
 
+#if defined(USE_MSP_OVER_TELEMETRY)
+typedef struct mspBuffer_s {
+    uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
+    int len;
+} mspBuffer_t;
+
+static mspBuffer_t mspRxBuffer;
+
+void initCrsfMspBuffer(void)
+{
+    mspRxBuffer.len = 0;
+}
+
+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;
+        *p++ = frameLength;
+        memcpy(p, frameStart, frameLength);
+        mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
+        return true;
+    }
+}
+
+bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
+{
+    bool requestHandled = false;
+    if (!mspRxBuffer.len) {
+        return false;
+    }
+    int pos = 0;
+    while (true) {
+        const int mspFrameLength = mspRxBuffer.bytes[pos];
+        if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
+            requestHandled |= sendMspReply(payloadSize, responseFn);
+        }
+        pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
+        ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
+            if (pos >= mspRxBuffer.len) {
+                mspRxBuffer.len = 0;
+                return requestHandled;
+            }
+        }
+    }
+    return requestHandled;
+}
+#endif
+
 static void crsfInitializeFrame(sbuf_t *dst)
 {
     dst->ptr = crsfFrame;
@@ -290,11 +346,11 @@ void crsfSendMspResponse(uint8_t *payload)
     sbuf_t *dst = &crsfPayloadBuf;
 
     crsfInitializeFrame(dst);
-    sbufWriteU8(dst, CRSF_FRAME_TX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
+    sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_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);
+    sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
     crsfFinalize(dst);
 }
 #endif
@@ -380,6 +436,12 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
     // in between the RX frames.
     crsfRxSendTelemetryData();
 
+#if defined(USE_MSP_OVER_TELEMETRY)
+    if (mspReplyPending) {
+        mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
+    }
+#endif
+
     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
     if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
         crsfLastCycleTime = currentTimeUs;
@@ -390,10 +452,6 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
             crsfFrameDeviceInfo(dst);
             crsfFinalize(dst);
             deviceInfoReplyPending = false;
-#if defined(USE_MSP_OVER_TELEMETRY)
-        } else if (mspReplyPending) {
-            mspReplyPending = sendMspReply(CRSF_FRAME_TX_MSP_PAYLOAD_SIZE, &crsfSendMspResponse);
-#endif
         } else {
             processCrsf();
         }

src/main/telemetry/crsf.h

@@ -26,5 +26,8 @@ bool checkCrsfTelemetryState(void);
 void handleCrsfTelemetry(timeUs_t currentTimeUs);
 void crsfScheduleDeviceInfoResponse(void);
 void crsfScheduleMspResponse(void);
-
 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
+#if defined(USE_MSP_OVER_TELEMETRY)
+void initCrsfMspBuffer(void);
+bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
+#endif

src/main/telemetry/msp_shared.c

@@ -36,9 +36,6 @@ enum {
     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;
@@ -87,7 +84,7 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd)
     sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
 }
 
-bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd)
+bool handleMspFrame(uint8_t *frameStart, int frameLength)
 {
     static uint8_t mspStarted = 0;
     static uint8_t lastSeq = 0;
@@ -101,7 +98,7 @@ bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd)
     }
 
     mspPacket_t *packet = mspPackage.requestPacket;
-    sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameEnd);
+    sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength);
     sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
     const uint8_t header = sbufReadU8(frameBuf);
     const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;

src/main/telemetry/msp_shared.h

@@ -25,5 +25,5 @@ typedef union mspTxBuffer_u {
 } mspTxBuffer_t;
 
 void initSharedMsp(void);
-bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
+bool handleMspFrame(uint8_t *frameStart, int frameLength);
 bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);

src/main/telemetry/smartport.c

@@ -361,9 +361,7 @@ void handleSmartPortTelemetry(void)
         if (smartPortRxBuffer.frameId == FSSP_MSPC_FRAME) {
             // Pass only the payload: skip sensorId & frameId
             uint8_t *frameStart = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET;
-            uint8_t *frameEnd = (uint8_t *)&smartPortRxBuffer + SMARTPORT_PAYLOAD_OFFSET + SMARTPORT_PAYLOAD_SIZE;
+            smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_PAYLOAD_SIZE);
-
-            smartPortMspReplyPending = handleMspFrame(frameStart, frameEnd);
         }
 #endif
     }

src/main/telemetry/telemetry.c

@@ -103,6 +103,7 @@ void telemetryInit(void)
 #endif
 #if defined(USE_MSP_OVER_TELEMETRY)
     initSharedMsp();
+    initCrsfMspBuffer();
 #endif
 
     telemetryCheckState();

src/test/Makefile

@@ -227,6 +227,7 @@ telemetry_crsf_unittest_DEFINES := \
 
 telemetry_crsf_msp_unittest_SRC := \
 		$(USER_DIR)/rx/crsf.c \
+		$(USER_DIR)/build/atomic.c \
 		$(USER_DIR)/common/crc.c \
 		$(USER_DIR)/common/streambuf.c \
 		$(USER_DIR)/common/printf.c \
@@ -280,9 +281,6 @@ 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/platform.h

@@ -94,5 +94,6 @@ typedef struct
 
 #define WS2811_DMA_TC_FLAG (void *)1
 #define WS2811_DMA_HANDLER_IDENTIFER 0
+#define NVIC_PriorityGroup_2 0x500
 
 #include "target.h"

src/test/unit/rx_crsf_unittest.cc

@@ -288,5 +288,5 @@ bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
 serialPort_t *telemetrySharedPort = NULL;
 void crsfScheduleDeviceInfoResponse(void) {};
 void crsfScheduleMspResponse(mspPackage_t *package) { UNUSED(package); };
-bool handleMspFrame(uint8_t *, uint8_t *) { return false; }
+bool bufferMspFrame(uint8_t *, int) {return true;}
 }

src/test/unit/telemetry_crsf_msp_unittest.cc

@@ -26,6 +26,7 @@ extern "C" {
     #include <platform.h>
 
     #include "build/debug.h"
+    #include "build/atomic.h"
 
     #include "common/crc.h"
     #include "common/utils.h"
@@ -37,6 +38,7 @@ extern "C" {
     #include "config/parameter_group.h"
     #include "config/parameter_group_ids.h"
 
+    #include "drivers/nvic.h"
     #include "drivers/serial.h"
     #include "drivers/system.h"
 
@@ -60,7 +62,7 @@ extern "C" {
     #include "telemetry/msp_shared.h"
     #include "telemetry/smartport.h"
 
-    bool handleMspFrame(uint8_t *frameStart, uint8_t *frameEnd);
+    bool handleMspFrame(uint8_t *frameStart, int frameLength);
     bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
     uint8_t sbufReadU8(sbuf_t *src);
     int sbufBytesRemaining(sbuf_t *buf);
@@ -97,7 +99,7 @@ typedef struct crsfMspFrame_s {
     uint8_t type;
     uint8_t destination;
     uint8_t origin;
-    uint8_t payload[CRSF_FRAME_RX_MSP_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC];
+    uint8_t payload[CRSF_FRAME_RX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_CRC];
 } crsfMspFrame_t;
 
 const uint8_t crsfPidRequest[] = {
@@ -106,17 +108,19 @@ const uint8_t crsfPidRequest[] = {
 
 TEST(CrossFireMSPTest, RequestBufferTest)
 {
+    atomic_BASEPRI = 0;
+
     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_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_RX_MSP_FRAME_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;
+    uint8_t *frameEnd = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_RX_MSP_FRAME_SIZE + 2;
     EXPECT_EQ(0xC8, *destination);
     EXPECT_EQ(0xEA, *origin);
     EXPECT_EQ(0x30, *frameStart);
@@ -130,8 +134,7 @@ TEST(CrossFireMSPTest, ResponsePacketTest)
     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, CRSF_FRAME_RX_MSP_FRAME_SIZE);
-    handleMspFrame(frameStart, frameEnd);
     for (unsigned int ii=1; ii<30; ii++) {
         EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
     }
@@ -151,8 +154,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     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, CRSF_FRAME_RX_MSP_FRAME_SIZE);
-    bool pending1 = handleMspFrame(frameStart, frameEnd);
     EXPECT_FALSE(pending1); // not done yet*/
     EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
     EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
@@ -164,8 +166,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     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, CRSF_FRAME_RX_MSP_FRAME_SIZE);
-    bool pending2 = handleMspFrame(frameStart2, frameEnd2);
     EXPECT_FALSE(pending2); // not done yet
     EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
     EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
@@ -179,8 +180,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     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, CRSF_FRAME_RX_MSP_FRAME_SIZE);
-    bool pending3 = handleMspFrame(frameStart3, frameEnd3);
     EXPECT_FALSE(pending3); // not done yet
     EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
     EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
@@ -194,8 +194,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     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, CRSF_FRAME_RX_MSP_FRAME_SIZE);
-    bool pending4 = handleMspFrame(frameStart4, frameEnd4);
     EXPECT_FALSE(pending4); // not done yet
     EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
     EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
@@ -210,8 +209,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     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, CRSF_FRAME_RX_MSP_FRAME_SIZE);
-    bool pending5 = handleMspFrame(frameStart5, frameEnd5);
     EXPECT_TRUE(pending5); // not done yet
     EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
     EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
@@ -233,8 +231,7 @@ TEST(CrossFireMSPTest, SendMspReply) {
     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, CRSF_FRAME_RX_MSP_FRAME_SIZE);
-    bool handled = handleMspFrame(frameStart, frameEnd);
     EXPECT_TRUE(handled);
     bool replyPending = sendMspReply(64, &testSendMspResponse);
     EXPECT_FALSE(replyPending);
@@ -276,7 +273,6 @@ extern "C" {
         UNUSED(mspPostProcessFn);
 
         sbuf_t *dst = &reply->buf;
-        //sbuf_t *src = &cmd->buf;
         const uint8_t cmdMSP = cmd->cmd;
         reply->cmd = cmd->cmd;
 
@@ -296,5 +292,4 @@ extern "C" {
     int32_t getMAhDrawn(void) {
       return testmAhDrawn;
     }
-
 }

src/test/unit/telemetry_crsf_unittest.cc

@@ -332,6 +332,7 @@ int32_t getMAhDrawn(void){
 }
 
 bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
-bool handleMspFrame(uint8_t *, uint8_t *)  { return false; }
+bool handleMspFrame(uint8_t *, int)  { return false; }
+void crsfScheduleMspResponse(void) {};
 
 }