Merge pull request #1297 from martinbudden/bf_msp_split_stage4

MSP split stage4 - Better split between MSP and serial

src/main/fc/fc_tasks.c

@@ -96,7 +96,7 @@ static void taskHandleSerial(uint32_t currentTime)
         return;
     }
 #endif
-    mspSerialProcess();
+    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA);
 }
 
 #ifdef BEEPER

src/main/io/serial.c

@@ -433,7 +433,7 @@ void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)
 
 void cliEnter(serialPort_t *serialPort);
 
-void evaluateOtherData(serialPort_t *serialPort, uint8_t receivedChar)
+void serialEvaluateNonMspData(serialPort_t *serialPort, uint8_t receivedChar)
 {
 #ifndef USE_CLI
     UNUSED(serialPort);

src/main/io/serial.h

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

src/main/io/serial_msp.c

@@ -34,6 +34,7 @@
 #include "msp/msp.h"
 
 
+static mspProcessCommandFnPtr mspProcessCommandFn;
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 bufWriter_t *writer;
 
@@ -77,11 +78,54 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
     }
 }
 
-void mspSerialInit(void)
+bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
 {
-    mspInit();
+    if (mspPort->c_state == MSP_IDLE) {
-    memset(mspPorts, 0, sizeof(mspPorts));
+        if (c == '$') {
-    mspSerialAllocatePorts();
+            mspPort->c_state = MSP_HEADER_START;
+        } else {
+            return false;
+        }
+    } else if (mspPort->c_state == MSP_HEADER_START) {
+        mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
+    } else if (mspPort->c_state == MSP_HEADER_M) {
+        mspPort->c_state = (c == '<') ? MSP_HEADER_ARROW : MSP_IDLE;
+    } else if (mspPort->c_state == MSP_HEADER_ARROW) {
+        if (c > MSP_PORT_INBUF_SIZE) {
+            mspPort->c_state = MSP_IDLE;
+
+        } else {
+            mspPort->dataSize = c;
+            mspPort->offset = 0;
+            mspPort->checksum = 0;
+            mspPort->indRX = 0;
+            mspPort->checksum ^= c;
+            mspPort->c_state = MSP_HEADER_SIZE;
+        }
+    } else if (mspPort->c_state == MSP_HEADER_SIZE) {
+        mspPort->cmdMSP = c;
+        mspPort->checksum ^= c;
+        mspPort->c_state = MSP_HEADER_CMD;
+    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset < mspPort->dataSize) {
+        mspPort->checksum ^= c;
+        mspPort->inBuf[mspPort->offset++] = c;
+    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
+        if (mspPort->checksum == c) {
+            mspPort->c_state = MSP_COMMAND_RECEIVED;
+        } else {
+            mspPort->c_state = MSP_IDLE;
+        }
+    }
+    return true;
+}
+
+static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *mspPort)
+{
+    mspPostProcessFnPtr mspPostProcessFn = NULL;
+    mspProcessCommandFn(mspPort, &mspPostProcessFn);
+
+    mspPort->c_state = MSP_IDLE;
+    return mspPostProcessFn;
 }
 
 /*
@@ -89,7 +133,7 @@ void mspSerialInit(void)
  *
  * Called periodically by the scheduler.
  */
-void mspSerialProcess(void)
+void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
 {
     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
         mspPort_t * const mspPort = &mspPorts[portIndex];
@@ -101,18 +145,18 @@ void mspSerialProcess(void)
         uint8_t buf[sizeof(bufWriter_t) + 20];
         writer = bufWriterInit(buf, sizeof(buf), (bufWrite_t)serialWriteBufShim, mspPort->port);
 
-        mspPostProcessFuncPtr mspPostProcessFn = NULL;
+        mspPostProcessFnPtr mspPostProcessFn = NULL;
         while (serialRxBytesWaiting(mspPort->port)) {
 
             const uint8_t c = serialRead(mspPort->port);
-            const bool consumed = mspProcessReceivedData(mspPort, c);
+            const bool consumed = mspSerialProcessReceivedData(mspPort, c);
 
-            if (!consumed && !ARMING_FLAG(ARMED)) {
+            if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
-                evaluateOtherData(mspPort->port, c);
+                serialEvaluateNonMspData(mspPort->port, c);
             }
 
-            if (mspPort->c_state == COMMAND_RECEIVED) {
+            if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
-                mspPostProcessFn = mspProcessReceivedCommand(mspPort);
+                mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort);
                 break; // process one command at a time so as not to block.
             }
         }
@@ -121,7 +165,14 @@ void mspSerialProcess(void)
 
         if (mspPostProcessFn) {
             waitForSerialPortToFinishTransmitting(mspPort->port);
-            mspPostProcessFn(mspPort);
+            mspPostProcessFn(mspPort->port);
         }
     }
 }
+
+void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
+{
+    mspProcessCommandFn = mspProcessCommandFnToUse;
+    memset(mspPorts, 0, sizeof(mspPorts));
+    mspSerialAllocatePorts();
+}

src/main/io/serial_msp.h

@@ -17,19 +17,27 @@
 
 #pragma once
 
+
+#include "msp/msp.h"
+
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
 #define MAX_MSP_PORT_COUNT 2
 
 typedef enum {
-    IDLE,
+    MSP_IDLE,
-    HEADER_START,
+    MSP_HEADER_START,
-    HEADER_M,
+    MSP_HEADER_M,
-    HEADER_ARROW,
+    MSP_HEADER_ARROW,
-    HEADER_SIZE,
+    MSP_HEADER_SIZE,
-    HEADER_CMD,
+    MSP_HEADER_CMD,
-    COMMAND_RECEIVED
+    MSP_COMMAND_RECEIVED
 } mspState_e;
 
+typedef enum {
+    MSP_EVALUATE_NON_MSP_DATA,
+    MSP_SKIP_NON_MSP_DATA
+} mspEvaluateNonMspData_e;
+
 #define MSP_PORT_INBUF_SIZE 64
 
 struct serialPort_s;
@@ -48,7 +56,7 @@ typedef struct mspPort_s {
 struct bufWriter_s;
 extern struct bufWriter_s *writer;
 
-void mspSerialInit(void);
+void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
-void mspSerialProcess(void);
+void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);

src/main/main.c

@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+#include "blackbox/blackbox.h"
+
 #include "common/axis.h"
 #include "common/color.h"
 #include "common/maths.h"
@@ -63,6 +65,9 @@
 
 #include "fc/fc_tasks.h"
 #include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "msp/msp_server_fc.h"
 
 #include "rx/rx.h"
 #include "rx/spektrum.h"
@@ -73,7 +78,6 @@
 #include "io/gps.h"
 #include "io/motors.h"
 #include "io/servos.h"
-#include "fc/rc_controls.h"
 #include "io/gimbal.h"
 #include "io/ledstrip.h"
 #include "io/display.h"
@@ -97,7 +101,6 @@
 #include "sensors/initialisation.h"
 
 #include "telemetry/telemetry.h"
-#include "blackbox/blackbox.h"
 
 #include "flight/pid.h"
 #include "flight/imu.h"
@@ -105,8 +108,6 @@
 #include "flight/failsafe.h"
 #include "flight/navigation.h"
 
-#include "fc/runtime_config.h"
-
 #include "config/config.h"
 #include "config/config_eeprom.h"
 #include "config/config_profile.h"
@@ -442,7 +443,7 @@ void init(void)
 
     imuInit();
 
-    mspSerialInit();
+    mspSerialInit(mspFcInit());
 
 #ifdef USE_CLI
     cliInit(&masterConfig.serialConfig);

src/main/msp/msp.h

@@ -17,9 +17,15 @@
 
 #pragma once
 
-struct mspPort_s;
+// return positive for ACK, negative on error, zero for no reply
-typedef void (*mspPostProcessFuncPtr)(struct mspPort_s *); // msp post process function, used for gracefully handling reboots, etc.
+typedef enum {
+    MSP_RESULT_ACK = 1,
+    MSP_RESULT_ERROR = -1,
+    MSP_RESULT_NO_REPLY = 0
+} mspResult_e;
 
-void mspInit(void);
+
-bool mspProcessReceivedData(struct mspPort_s *mspPort, uint8_t c);
+struct serialPort_s;
-mspPostProcessFuncPtr mspProcessReceivedCommand(struct mspPort_s *mspPort);
+typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc.
+struct mspPort_s;
+typedef mspResult_e (*mspProcessCommandFnPtr)(struct mspPort_s *mspPort, mspPostProcessFnPtr *mspPostProcessFn);

src/main/msp/msp_server_fc.c

@@ -112,7 +112,6 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
 extern void resetProfile(profile_t *profile);
 
 // cause reboot after MSP processing complete
-static mspPostProcessFuncPtr mspPostProcessFn = NULL;
 static mspPort_t *currentPort;
 
 static const char * const flightControllerIdentifier = BETAFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
@@ -193,26 +192,26 @@ typedef enum {
 #define DATAFLASH_BUFFER_SIZE 4096
 
 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
-void msp4WayIfFn(mspPort_t *mspPort)
+static void msp4WayIfFn(serialPort_t *serialPort)
 {
     // rem: App: Wait at least appx. 500 ms for BLHeli to jump into
     // bootloader mode before try to connect any ESC
     // Start to activate here
-    esc4wayProcess(mspPort->port);
+    esc4wayProcess(serialPort);
     // former used MSP uart is still active
     // proceed as usual with MSP commands
 }
 #endif
 
-static void mspRebootFn(mspPort_t *mspPort)
+static void mspRebootFn(serialPort_t *serialPort)
 {
-    UNUSED(mspPort);
+    UNUSED(serialPort);
 
     stopPwmAllMotors();
     systemReset();
 
     // control should never return here.
-    while(1) ;
+    while (true) ;
 }
 
 static void serialize8(uint8_t a)
@@ -463,7 +462,7 @@ static void serializeDataflashReadReply(uint32_t address, uint16_t size, bool us
 }
 #endif
 
-void mspInit(void)
+void initActiveBoxIds(void)
 {
     // calculate used boxes based on features and fill availableBoxes[] array
     memset(activeBoxIds, 0xFF, sizeof(activeBoxIds));
@@ -608,9 +607,12 @@ static uint32_t packFlightModeFlags(void)
     return junk;
 }
 
-static bool processOutCommand(uint8_t cmdMSP)
+static bool processOutCommand(uint8_t cmdMSP, mspPostProcessFnPtr *mspPostProcessFn)
 {
     uint32_t i;
+#ifdef USE_FLASHFS
+    const unsigned int dataSize = currentPort->dataSize;
+#endif
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0;
@@ -1157,7 +1159,7 @@ static bool processOutCommand(uint8_t cmdMSP)
             uint32_t readAddress = read32();
             uint16_t readLength;
             bool useLegacyFormat;
-            if (currentPort->dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
+            if (dataSize >= sizeof(uint32_t) + sizeof(uint16_t)) {
                 readLength = read16();
                 useLegacyFormat = false;
             } else {
@@ -1303,7 +1305,9 @@ static bool processOutCommand(uint8_t cmdMSP)
 
     case MSP_REBOOT:
         headSerialReply(0);
-        mspPostProcessFn = mspRebootFn;
+        if (mspPostProcessFn) {
+            *mspPostProcessFn = mspRebootFn;
+        }
         break;
 
 #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE
@@ -1313,7 +1317,9 @@ static bool processOutCommand(uint8_t cmdMSP)
         // switch all motor lines HI
         // reply with the count of ESC found
         serialize8(esc4wayInit());
-        mspPostProcessFn = msp4WayIfFn;
+        if (mspPostProcessFn) {
+            *mspPostProcessFn = mspRebootFn;
+        }
         break;
 #endif
 
@@ -1916,55 +1922,25 @@ static bool processInCommand(uint8_t cmdMSP)
     return true;
 }
 
-mspPostProcessFuncPtr mspProcessReceivedCommand(mspPort_t *mspPort)
+mspResult_e mspFcProcessCommand(mspPort_t *mspPort, mspPostProcessFnPtr *mspPostProcessFn)
 {
+    mspResult_e ret = MSP_RESULT_ACK;
     currentPort = mspPort;
     mspPostProcessFn = NULL;
-    if (!(processOutCommand(mspPort->cmdMSP) || processInCommand(mspPort->cmdMSP))) {
+    if (!(processOutCommand(mspPort->cmdMSP, mspPostProcessFn) || processInCommand(mspPort->cmdMSP))) {
         headSerialError(0);
+        ret = MSP_RESULT_ERROR;
     }
     tailSerialReply();
-    mspPort->c_state = IDLE;
+    mspPort->c_state = MSP_IDLE;
-    return mspPostProcessFn;
+    return ret;
 }
 
-bool mspProcessReceivedData(mspPort_t *mspPort, uint8_t c)
+/*
+ * Return a pointer to the process command function
+ */
+mspProcessCommandFnPtr mspFcInit(void)
 {
-    if (mspPort->c_state == IDLE) {
+    initActiveBoxIds();
-        if (c == '$') {
+    return mspFcProcessCommand;
-            mspPort->c_state = HEADER_START;
-        } else {
-            return false;
-        }
-    } else if (mspPort->c_state == HEADER_START) {
-        mspPort->c_state = (c == 'M') ? HEADER_M : IDLE;
-    } else if (mspPort->c_state == HEADER_M) {
-        mspPort->c_state = (c == '<') ? HEADER_ARROW : IDLE;
-    } else if (mspPort->c_state == HEADER_ARROW) {
-        if (c > MSP_PORT_INBUF_SIZE) {
-            mspPort->c_state = IDLE;
-
-        } else {
-            mspPort->dataSize = c;
-            mspPort->offset = 0;
-            mspPort->checksum = 0;
-            mspPort->indRX = 0;
-            mspPort->checksum ^= c;
-            mspPort->c_state = HEADER_SIZE;
-        }
-    } else if (mspPort->c_state == HEADER_SIZE) {
-        mspPort->cmdMSP = c;
-        mspPort->checksum ^= c;
-        mspPort->c_state = HEADER_CMD;
-    } else if (mspPort->c_state == HEADER_CMD && mspPort->offset < mspPort->dataSize) {
-        mspPort->checksum ^= c;
-        mspPort->inBuf[mspPort->offset++] = c;
-    } else if (mspPort->c_state == HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
-        if (mspPort->checksum == c) {
-            mspPort->c_state = COMMAND_RECEIVED;
-        } else {
-            mspPort->c_state = IDLE;
-        }
-    }
-    return true;
 }

src/main/msp/msp_server_fc.h

@@ -0,0 +1,22 @@
+/*
+ * 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/>.
+ */
+
+#pragma once
+
+#include "msp/msp.h"
+
+mspProcessCommandFnPtr mspFcInit(void);