Merge pull request betaflight#7182 from klutvott123/fport-sport-write…

…eeprom-fix

Skip smartport/fport telemetry requests after eeprom write

src/main/telemetry/crsf.c

@@ -112,7 +112,7 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
     int pos = 0;
     while (true) {
         const int mspFrameLength = mspRxBuffer.bytes[pos];
-        if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
+        if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
             requestHandled |= sendMspReply(payloadSize, responseFn);
         }
         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;

src/main/telemetry/msp_shared.c

@@ -31,6 +31,7 @@
 #include "common/utils.h"
 
 #include "interface/msp.h"
+#include "interface/msp_protocol.h"
 
 #include "telemetry/crsf.h"
 #include "telemetry/msp_shared.h"
@@ -44,6 +45,8 @@
 #define TELEMETRY_MSP_SEQ_MASK   0x0F
 #define TELEMETRY_MSP_RES_ERROR (-10)
 
+#define TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
+
 enum {
     TELEMETRY_MSP_VER_MISMATCH=0,
     TELEMETRY_MSP_CRC_ERROR=1,
@@ -98,7 +101,7 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd)
     sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
 }
 
-bool handleMspFrame(uint8_t *frameStart, int frameLength)
+bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse)
 {
     static uint8_t mspStarted = 0;
     static uint8_t lastSeq = 0;
@@ -170,6 +173,11 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength)
         }
     }
 
+    // Skip a few telemetry requests if command is MSP_EEPROM_WRITE
+    if (packet->cmd == MSP_EEPROM_WRITE && skipsBeforeResponse) {
+        *skipsBeforeResponse = TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE;
+    }
+
     mspStarted = 0;
     sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
     processMspPacket();

src/main/telemetry/msp_shared.h

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

src/main/telemetry/smartport.c

@@ -287,7 +287,6 @@ bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
     return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
 }
 
-
 void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
 {
     uint8_t *data = (uint8_t *)payload;
@@ -493,24 +492,35 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
     static uint8_t smartPortIdCycleCnt = 0;
     static uint8_t t1Cnt = 0;
     static uint8_t t2Cnt = 0;
+    static uint8_t skipRequests = 0;
 #ifdef USE_ESC_SENSOR_TELEMETRY
     static uint8_t smartPortIdOffset = 0;
 #endif
 
 #if defined(USE_MSP_OVER_TELEMETRY)
-    if (payload && smartPortPayloadContainsMSP(payload)) {
+    if (skipRequests) {
+        skipRequests--;
+    } else if (payload && smartPortPayloadContainsMSP(payload)) {
         // Do not check the physical ID here again
         // unless we start receiving other sensors' packets
         // Pass only the payload: skip frameId
-         uint8_t *frameStart = (uint8_t *)&payload->valueId;
-         smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
+        uint8_t *frameStart = (uint8_t *)&payload->valueId;
+        smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE, &skipRequests);
+
+        // Don't send MSP response after write to eeprom
+        // CPU just got out of suspended state after writeEEPROM()
+        // We don't know if the receiver is listening again
+        // Skip a few telemetry requests before sending response
+        if (skipRequests) {
+            *clearToSend = false;
+        }
     }
 #else
     UNUSED(payload);
 #endif
 
     bool doRun = true;
-    while (doRun && *clearToSend) {
+    while (doRun && *clearToSend && !skipRequests) {
         // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
         if (requestTimeout) {
             if (cmpTimeUs(micros(), *requestTimeout) >= 0) {

src/test/unit/telemetry_crsf_msp_unittest.cc

@@ -63,7 +63,7 @@ extern "C" {
     #include "telemetry/smartport.h"
     #include "sensors/acceleration.h"
 
-    bool handleMspFrame(uint8_t *frameStart, int frameLength);
+    bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse);
     bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
     uint8_t sbufReadU8(sbuf_t *src);
     int sbufBytesRemaining(sbuf_t *buf);
@@ -137,7 +137,7 @@ TEST(CrossFireMSPTest, ResponsePacketTest)
     crsfFrame = *(const crsfFrame_t*)framePtr;
     crsfFrameDone = true;
     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-    handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
+    handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
     for (unsigned int ii=1; ii<30; ii++) {
         EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf));
     }
@@ -157,7 +157,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr1;
     crsfFrameDone = true;
     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-    bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
+    bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
     EXPECT_FALSE(pending1); // not done yet*/
     EXPECT_EQ(0x29, mspPackage.requestBuffer[0]);
     EXPECT_EQ(0x28, mspPackage.requestBuffer[1]);
@@ -169,7 +169,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr2;
     crsfFrameDone = true;
     uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE);
+    bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
     EXPECT_FALSE(pending2); // not done yet
     EXPECT_EQ(0x23, mspPackage.requestBuffer[5]);
     EXPECT_EQ(0x46, mspPackage.requestBuffer[6]);
@@ -183,7 +183,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr3;
     crsfFrameDone = true;
     uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE);
+    bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
     EXPECT_FALSE(pending3); // not done yet
     EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]);
     EXPECT_EQ(0x00, mspPackage.requestBuffer[13]);
@@ -197,7 +197,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr4;
     crsfFrameDone = true;
     uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE);
+    bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
     EXPECT_FALSE(pending4); // not done yet
     EXPECT_EQ(0x21, mspPackage.requestBuffer[19]);
     EXPECT_EQ(0x53, mspPackage.requestBuffer[20]);
@@ -212,7 +212,7 @@ TEST(CrossFireMSPTest, WriteResponseTest)
     crsfFrame = *(const crsfFrame_t*)framePtr5;
     crsfFrameDone = true;
     uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
-    bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE);
+    bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
     EXPECT_TRUE(pending5); // not done yet
     EXPECT_EQ(0x00, mspPackage.requestBuffer[26]);
     EXPECT_EQ(0x37, mspPackage.requestBuffer[27]);
@@ -234,7 +234,7 @@ TEST(CrossFireMSPTest, SendMspReply) {
     crsfFrame = *(const crsfFrame_t*)framePtr;
     crsfFrameDone = true;
     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
-    bool handled = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE);
+    bool handled = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
     EXPECT_TRUE(handled);
     bool replyPending = sendMspReply(64, &testSendMspResponse);
     EXPECT_FALSE(replyPending);

src/test/unit/telemetry_crsf_unittest.cc

@@ -344,7 +344,7 @@ int32_t getMAhDrawn(void){
 }
 
 bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
-bool handleMspFrame(uint8_t *, int)  { return false; }
+bool handleMspFrame(uint8_t *, int, uint8_t *)  { return false; }
 void crsfScheduleMspResponse(void) {};
 bool isBatteryVoltageConfigured(void) { return true; }
 bool isAmperageConfigured(void) { return true; }