Handle MSP out of band data with guard interval

src/main/io/serial.c

@@ -502,20 +502,6 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)
     };
 }
 
-void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar)
-{
-#ifndef USE_CLI
-    UNUSED(serialPort);
-#else
-    if (receivedChar == '#') {
-        cliEnter(serialPort);
-    }
-#endif
-    if (receivedChar == serialConfig()->reboot_character) {
-        systemResetToBootloader();
-    }
-}
-
 #if defined(USE_GPS) || ! defined(SKIP_SERIAL_PASSTHROUGH)
 // Default data consumer for serialPassThrough.
 static void nopConsumer(uint8_t data)

src/main/io/serial.h

@@ -166,5 +166,4 @@ baudRate_e lookupBaudRateIndex(uint32_t baudRate);
 //
 // msp/cli/bootloader
 //
-void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar);
 void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer *leftC, serialConsumer *rightC);

src/main/msp/msp_serial.c

@@ -26,7 +26,10 @@
 #include "common/streambuf.h"
 #include "common/utils.h"
 
+#include "drivers/system.h"
+
 #include "interface/msp.h"
+#include "interface/cli.h"
 
 #include "io/serial.h"
 
@@ -194,6 +197,41 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspPr
     return mspPostProcessFn;
 }
 
+static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar)
+{
+#ifdef USE_CLI
+    if (receivedChar == '#') {
+        mspPort->pendingRequest = MSP_PENDING_CLI;
+        return;
+    }
+#endif
+
+    if (receivedChar == serialConfig()->reboot_character) {
+        mspPort->pendingRequest = MSP_PENDING_BOOTLOADER;
+        return;
+    }
+}
+
+static void mspProcessPendingRequest(mspPort_t * mspPort)
+{
+    // If no request is pending or 100ms guard time has not elapsed - do nothing
+    if ((mspPort->pendingRequest == MSP_PENDING_NONE) || (millis() - mspPort->lastActivityMs < 100)) {
+        return;
+    }
+
+    switch(mspPort->pendingRequest) {
+        case MSP_PENDING_BOOTLOADER:
+            systemResetToBootloader();
+            break;
+
+        case MSP_PENDING_CLI:
+            cliEnter(mspPort->port);
+            break;
+
+        default:
+            break;
+    }
+}
 
 static void mspSerialProcessReceivedReply(mspPort_t *msp, mspProcessReplyFnPtr mspProcessReplyFn)
 {
@@ -226,30 +264,38 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
 
         mspPostProcessFnPtr mspPostProcessFn = NULL;
 
-        while (serialRxBytesWaiting(mspPort->port)) {
+        if (serialRxBytesWaiting(mspPort->port)) {
+            // There are bytes incoming - abort pending request
+            mspPort->lastActivityMs = millis();
+            mspPort->pendingRequest = MSP_PENDING_NONE;
 
-            const uint8_t c = serialRead(mspPort->port);
+            while (serialRxBytesWaiting(mspPort->port)) {
-            const bool consumed = mspSerialProcessReceivedData(mspPort, c);
+                const uint8_t c = serialRead(mspPort->port);
+                const bool consumed = mspSerialProcessReceivedData(mspPort, c);
 
-            if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
+                if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
-                serialEvaluateNonMspData(mspPort->port, c);
+                    mspEvaluateNonMspData(mspPort, c);
-            }
-
-            if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
-                if (mspPort->packetType == MSP_PACKET_COMMAND) {
-                    mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
-                } else if (mspPort->packetType == MSP_PACKET_REPLY) {
-                    mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
                 }
 
-                mspPort->c_state = MSP_IDLE;
+                if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
-                break; // process one command at a time so as not to block.
+                    if (mspPort->packetType == MSP_PACKET_COMMAND) {
+                        mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
+                    } else if (mspPort->packetType == MSP_PACKET_REPLY) {
+                        mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
+                    }
+
+                    mspPort->c_state = MSP_IDLE;
+                    break; // process one command at a time so as not to block.
+                }
+            }
+
+            if (mspPostProcessFn) {
+                waitForSerialPortToFinishTransmitting(mspPort->port);
+                mspPostProcessFn(mspPort->port);
             }
         }
-
+        else {
-        if (mspPostProcessFn) {
+            mspProcessPendingRequest(mspPort);
-            waitForSerialPortToFinishTransmitting(mspPort->port);
-            mspPostProcessFn(mspPort->port);
         }
     }
 }

src/main/msp/msp_serial.h

@@ -17,6 +17,7 @@
 
 #pragma once
 
+#include "drivers/time.h"
 #include "interface/msp.h"
 
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 3 MSP ports.
@@ -42,6 +43,12 @@ typedef enum {
     MSP_SKIP_NON_MSP_DATA
 } mspEvaluateNonMspData_e;
 
+typedef enum {
+    MSP_PENDING_NONE,
+    MSP_PENDING_BOOTLOADER,
+    MSP_PENDING_CLI
+} mspPendingSystemRequest_e;
+
 #define MSP_PORT_INBUF_SIZE 192
 #ifdef USE_FLASHFS
 #ifdef STM32F1
@@ -58,6 +65,8 @@ typedef enum {
 struct serialPort_s;
 typedef struct mspPort_s {
     struct serialPort_s *port; // null when port unused.
+    timeMs_t lastActivityMs;
+    mspPendingSystemRequest_e pendingRequest;
     uint8_t offset;
     uint8_t dataSize;
     uint8_t checksum;