Tidied MSP serial code

Makefile

@@ -399,7 +399,7 @@ COMMON_SRC = \
             drivers/stack_check.c \
             drivers/system.c \
             drivers/timer.c \
-			drivers/io_pca9685.c \
+            drivers/io_pca9685.c \
             flight/failsafe.c \
             flight/imu.c \
             flight/hil.c \
@@ -407,6 +407,7 @@ COMMON_SRC = \
             flight/servos.c \
             flight/pid.c \
             io/beeper.c \
+            fc/msp_fc.c \
             fc/rc_controls.c \
             fc/rc_curves.c \
             io/serial.c \
@@ -414,10 +415,9 @@ COMMON_SRC = \
             io/serial_4way_avrootloader.c \
             io/serial_4way_stk500v2.c \
             io/serial_cli.c \
-            io/serial_msp.c \
             io/statusindicator.c \
-            fc/msp_server_fc.c \
+            io/pwmdriver_i2c.c \
-			io/pwmdriver_i2c.c \
+            msp/msp_serial.c \
             rx/ibus.c \
             rx/jetiexbus.c \
             rx/msp.c \

src/main/blackbox/blackbox.c

@@ -62,7 +62,6 @@
 #include "io/ledstrip.h"
 #include "io/serial.h"
 #include "io/serial_cli.h"
-#include "io/serial_msp.h"
 #include "io/statusindicator.h"
 
 #include "rx/rx.h"

src/main/blackbox/blackbox_io.c

@@ -28,6 +28,7 @@
 #include "common/axis.h"
 #include "common/color.h"
 #include "common/encoding.h"
+#include "common/streambuf.h"
 
 #include "drivers/gpio.h"
 #include "drivers/sensor.h"
@@ -52,18 +53,20 @@
 #include "io/display.h"
 #include "io/motors.h"
 #include "io/servos.h"
-#include "rx/rx.h"
 #include "fc/rc_controls.h"
 
-
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
 #include "io/serial.h"
 #include "io/serial_cli.h"
-#include "io/serial_msp.h"
 #include "io/statusindicator.h"
+
+#include "msp/msp_serial.h"
+
 #include "rx/msp.h"
+#include "rx/rx.h"
+
 #include "telemetry/telemetry.h"
 #include "common/printf.h"
 

src/main/config/config.h

@@ -91,6 +91,8 @@ void changeProfile(uint8_t profileIndex);
 
 void setProfile(uint8_t profileIndex);
 void setControlRateProfile(uint8_t profileIndex);
+struct pidProfile_s;
+void resetPidProfile(struct pidProfile_s *pidProfile);
 
 uint8_t getCurrentControlRateProfile(void);
 void changeControlRateProfile(uint8_t profileIndex);

src/main/fc/msp_fc.c

@@ -99,11 +99,6 @@
 
 extern uint16_t cycleTime; // FIXME dependency on mw.c
 extern uint16_t rssi; // FIXME dependency on mw.c
-extern void resetPidProfile(pidProfile_t *pidProfile);
-
-void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse);
-
-static mspPostProcessFuncPtr mspPostProcessFn = NULL;
 
 static const char * const flightControllerIdentifier = INAV_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
@@ -473,7 +468,11 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, uint8_t s
 }
 #endif
 
-static bool processOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src)
+/*
+ * Returns true if the command was processd, false otherwise.
+ * May set mspPostProcessFunc to a function to be called once the command has been processed
+ */
+static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr *mspPostProcessFn)
 {
 #if !defined(NAV) && !defined(USE_FLASHFS)
     UNUSED(src);
@@ -1037,7 +1036,9 @@ static bool processOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src)
         break;
 
     case MSP_REBOOT:
-        mspPostProcessFn = mspRebootFn;
+        if (mspPostProcessFn) {
+            *mspPostProcessFn = mspRebootFn;
+        }
         break;
 
 
@@ -1047,7 +1048,9 @@ static bool processOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src)
         // switch all motor lines HI
         // reply with the count of ESC found
         sbufWriteU8(dst, esc4wayInit());
-        mspPostProcessFn = msp4WayIfFn;
+        if (mspPostProcessFn) {
+            *mspPostProcessFn = msp4WayIfFn;
+        }
         break;
 #endif
 
@@ -1057,7 +1060,7 @@ static bool processOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src)
     return true;
 }
 
-static mspResult_e processInCommand(uint8_t cmdMSP, sbuf_t *src)
+static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
 {
     uint32_t i;
     uint16_t tmp;
@@ -1567,29 +1570,33 @@ static mspResult_e processInCommand(uint8_t cmdMSP, sbuf_t *src)
     return MSP_RESULT_ACK;
 }
 
-// return positive for ACK, negative on error, zero for no reply
+/*
-mspResult_e mspProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFuncPtr *mspPostProcessFunc)
+ * Returns MSP_RESULT_ACK, MSP_RESULT_ERROR or MSP_RESULT_NO_REPLY
+ */
+mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn)
 {
     int ret = MSP_RESULT_ACK;
-    mspPostProcessFn = NULL;
     sbuf_t *dst = &reply->buf;
     sbuf_t *src = &cmd->buf;
     const uint8_t cmdMSP = cmd->cmd;
     // initialize reply by default
     reply->cmd = cmd->cmd;
 
-    if (processOutCommand(cmdMSP, dst, src)) {
+    if (mspFcProcessOutCommand(cmdMSP, dst, src, mspPostProcessFn)) {
         ret = MSP_RESULT_ACK;
     } else {
-        ret = processInCommand(cmdMSP, src);
+        ret = mspFcProcessInCommand(cmdMSP, src);
     }
-    *mspPostProcessFunc = mspPostProcessFn;
     reply->result = ret;
     return ret;
 }
 
-void mspInit(void)
+/*
+ * Return a pointer to the process command function
+ */
+mspProcessCommandFnPtr mspFcInit(void)
 {
     initActiveBoxIds();
+    return mspFcProcessCommand;
 }
 

src/main/fc/msp_fc.h

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

src/main/fc/mw.c

@@ -22,6 +22,8 @@
 
 #include "platform.h"
 
+#include "blackbox/blackbox.h"
+
 #include "build/debug.h"
 
 #include "scheduler/scheduler.h"
@@ -67,15 +69,15 @@
 #include "io/ledstrip.h"
 #include "io/serial.h"
 #include "io/serial_cli.h"
-#include "io/serial_msp.h"
 #include "io/statusindicator.h"
 #include "io/asyncfatfs/asyncfatfs.h"
 
+#include "msp/msp_serial.h"
+
 #include "rx/rx.h"
 #include "rx/msp.h"
 
 #include "telemetry/telemetry.h"
-#include "blackbox/blackbox.h"
 
 #include "flight/mixer.h"
 #include "flight/servos.h"
@@ -659,7 +661,14 @@ void taskMainPidLoopChecker(void) {
 
 void taskHandleSerial(void)
 {
-    handleSerial();
+#ifdef USE_CLI
+    // in cli mode, all serial stuff goes to here. enter cli mode by sending #
+    if (cliMode) {
+        cliProcess();
+        return;
+    }
+#endif
+    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_ARMED : MSP_NOT_ARMED);
 }
 
 void taskUpdateBeeper(void)

src/main/io/serial.c

@@ -44,8 +44,8 @@
 
 #include "io/serial.h"
 #include "serial_cli.h"
-#include "serial_msp.h"
 
+#include "msp/msp_serial.h"
 
 #ifdef TELEMETRY
 #include "telemetry/telemetry.h"
@@ -420,18 +420,6 @@ bool serialIsPortAvailable(serialPortIdentifier_e identifier)
     return false;
 }
 
-void handleSerial(void)
-{
-#ifdef USE_CLI
-    // in cli mode, all serial stuff goes to here. enter cli mode by sending #
-    if (cliMode) {
-        cliProcess();
-        return;
-    }
-#endif
-
-    mspSerialProcess();
-}
 
 void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)
 {

src/main/io/serial.h

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

src/main/io/serial_4way.c

@@ -37,7 +37,6 @@
 #include "drivers/system.h"
 #include "flight/mixer.h"
 #include "io/beeper.h"
-#include "io/serial_msp.h"
 #include "io/serial_4way.h"
 #include "io/serial_4way_impl.h"
 

src/main/io/serial_4way_avrootloader.c

@@ -33,7 +33,6 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/gpio.h"
 #include "io/serial.h"
-#include "io/serial_msp.h"
 #include "io/serial_4way.h"
 #include "io/serial_4way_impl.h"
 #include "io/serial_4way_avrootloader.h"

src/main/io/serial_4way_stk500v2.c

@@ -32,7 +32,6 @@
 #include "drivers/system.h"
 #include "config/config.h"
 #include "io/serial.h"
-#include "io/serial_msp.h"
 #include "io/serial_4way.h"
 #include "io/serial_4way_impl.h"
 #include "io/serial_4way_stk500v2.h"

src/main/main.c

@@ -62,9 +62,10 @@
 #include "drivers/gyro_sync.h"
 #include "drivers/io.h"
 #include "drivers/exti.h"
-#include "io/pwmdriver_i2c.h"
 #include "drivers/io_pca9685.h"
 
+#include "fc/msp_fc.h"
+
 #include "rx/rx.h"
 #include "rx/spektrum.h"
 
@@ -80,9 +81,11 @@
 #include "io/ledstrip.h"
 #include "io/display.h"
 #include "io/asyncfatfs/asyncfatfs.h"
-#include "io/serial_msp.h"
+#include "io/pwmdriver_i2c.h"
 #include "io/serial_cli.h"
 
+#include "msp/msp_serial.h"
+
 #include "scheduler/scheduler.h"
 
 #include "sensors/sensors.h"
@@ -468,7 +471,7 @@ void init(void)
 
     imuInit();
 
-    mspSerialInit();
+    mspSerialInit(mspFcInit());
 
 #ifdef USE_CLI
     cliInit(&masterConfig.serialConfig);

src/main/msp/msp.h

@@ -17,8 +17,7 @@
 
 #pragma once
 
-struct serialPort_s;
+#include "common/streambuf.h"
-typedef void (*mspPostProcessFuncPtr)(struct serialPort_s *); // msp post process function, used for gracefully handling reboots, etc.
 
 // return positive for ACK, negative on error, zero for no reply
 typedef enum {
@@ -33,5 +32,6 @@ typedef struct mspPacket_s {
     int16_t result;
 } mspPacket_t;
 
-void mspInit(void);
+struct serialPort_s;
-mspResult_e mspProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFuncPtr *mspPostProcessFunc);
+typedef void (*mspPostProcessFnPtr)(struct serialPort_s *port); // msp post process function, used for gracefully handling reboots, etc.
+typedef mspResult_e (*mspProcessCommandFnPtr)(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);

src/main/msp/msp_serial.c

@@ -26,14 +26,12 @@
 
 #include "drivers/serial.h"
 
-#include "fc/runtime_config.h"
-
 #include "io/serial.h"
-#include "io/serial_msp.h"
 
 #include "msp/msp.h"
+#include "msp/msp_serial.h"
 
-
+static mspProcessCommandFnPtr mspProcessCommandFn;
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
 
@@ -76,14 +74,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
     }
 }
 
-void mspSerialInit(void)
+static bool mspSerialProcessReceivedData(mspPort_t * mspPort, uint8_t c)
-{
-    mspInit();
-    memset(mspPorts, 0, sizeof(mspPorts));
-    mspSerialAllocatePorts();
-}
-
-static bool mspProcessReceivedData(mspPort_t * mspPort, uint8_t c)
 {
     if (mspPort->c_state == MSP_IDLE) {
         if (c == '$') {
@@ -145,7 +136,7 @@ static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
     serialEndWrite(msp->port);
 }
 
-static mspPostProcessFuncPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
+static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
 {
     static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
 
@@ -162,8 +153,8 @@ static mspPostProcessFuncPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
         .result = 0,
     };
 
-    mspPostProcessFuncPtr mspPostProcessFn = NULL;
+    mspPostProcessFnPtr mspPostProcessFn = NULL;
-    const mspResult_e status = mspProcessCommand(&command, &reply, &mspPostProcessFn);
+    const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
 
     if (status != MSP_RESULT_NO_REPLY) {
         sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
@@ -174,20 +165,20 @@ static mspPostProcessFuncPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
     return mspPostProcessFn;
 }
 
-void mspSerialProcess(void)
+void mspSerialProcess(mspArmedState_e armedState)
 {
     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
         mspPort_t * const mspPort = &mspPorts[portIndex];
         if (!mspPort->port) {
             continue;
         }
-        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 && armedState == MSP_ARMED) {
                 evaluateOtherData(mspPort->port, c);
             }
 
@@ -202,3 +193,11 @@ void mspSerialProcess(void)
         }
     }
 }
+
+void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
+{
+    mspProcessCommandFn = mspProcessCommandFnToUse;
+    memset(mspPorts, 0, sizeof(mspPorts));
+    mspSerialAllocatePorts();
+}
+

src/main/msp/msp_serial.h

@@ -17,6 +17,8 @@
 
 #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
 
@@ -30,6 +32,11 @@ typedef enum {
     MSP_COMMAND_RECEIVED
 } mspState_e;
 
+typedef enum {
+    MSP_NOT_ARMED,
+    MSP_ARMED
+} mspArmedState_e;
+
 #define MSP_PORT_INBUF_SIZE 64
 #define MSP_PORT_OUTBUF_SIZE 256
 
@@ -45,7 +52,7 @@ typedef struct mspPort_s {
 } mspPort_t;
 
 
-void mspSerialInit(void);
+void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
-void mspSerialProcess(void);
+void mspSerialProcess(mspArmedState_e armedState);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);