diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 4f4d7408d5..d84a1073a5 100644 --- a/src/main/telemetry/crsf.c +++ b/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; diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 3177d38f80..2c0910f42d 100644 --- a/src/main/telemetry/msp_shared.c +++ b/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(); diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index f216b04740..9bf9154eae 100644 --- a/src/main/telemetry/msp_shared.h +++ b/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); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index ed83b5becc..e0d581f3f8 100644 --- a/src/main/telemetry/smartport.c +++ b/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) { diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index cef85b5e2b..2b8526c2d6 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/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); diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index fda07ea6a0..805ba70db9 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/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; }