From 6aa40846fe57943c283e77e157136f2083d034a7 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Mon, 24 Oct 2016 18:49:08 +0100
Subject: [PATCH] Improved MSP initialisation and independence from serial code

---
 src/main/fc/fc_msp.c      |  3 +--
 src/main/fc/fc_msp.h      |  3 ++-
 src/main/fc/fc_tasks.c    |  3 ++-
 src/main/main.c           |  3 ++-
 src/main/msp/msp_serial.c | 10 ++++------
 src/main/msp/msp_serial.h |  4 ++--
 6 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
index c469175535..1f5a5cd943 100755
--- a/src/main/fc/fc_msp.c
+++ b/src/main/fc/fc_msp.c
@@ -1782,8 +1782,7 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
 /*
  * Return a pointer to the process command function
  */
-mspProcessCommandFnPtr mspFcInit(void)
+void mspFcInit(void)
 {
     initActiveBoxIds();
-    return mspFcProcessCommand;
 }
diff --git a/src/main/fc/fc_msp.h b/src/main/fc/fc_msp.h
index 745fd551a9..53edbd8ed1 100644
--- a/src/main/fc/fc_msp.h
+++ b/src/main/fc/fc_msp.h
@@ -19,4 +19,5 @@
 
 #include "msp/msp.h"
 
-mspProcessCommandFnPtr mspFcInit(void);
+void mspFcInit(void);
+mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostProcessFnPtr *mspPostProcessFn);
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index d3d088f29e..b81d36ce01 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -31,6 +31,7 @@
 #include "drivers/serial.h"
 
 #include "fc/config.h"
+#include "fc/fc_msp.h"
 #include "fc/fc_tasks.h"
 #include "fc/mw.h"
 #include "fc/rc_controls.h"
@@ -97,7 +98,7 @@ static void taskHandleSerial(uint32_t currentTime)
         return;
     }
 #endif
-    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA);
+    mspSerialProcess(ARMING_FLAG(ARMED) ? MSP_SKIP_NON_MSP_DATA : MSP_EVALUATE_NON_MSP_DATA, mspFcProcessCommand);
 }
 
 #ifdef BEEPER
diff --git a/src/main/main.c b/src/main/main.c
index ae8e88d179..c4ca10b686 100644
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -449,7 +449,8 @@ void init(void)
 
     imuInit();
 
-    mspSerialInit(mspFcInit());
+    mspFcInit();
+    mspSerialInit();
 
 #ifdef USE_CLI
     cliInit(&masterConfig.serialConfig);
diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c
index b0c80d116f..8bfff6b37b 100644
--- a/src/main/msp/msp_serial.c
+++ b/src/main/msp/msp_serial.c
@@ -31,7 +31,6 @@
 #include "msp/msp.h"
 #include "msp/msp_serial.h"
 
-static mspProcessCommandFnPtr mspProcessCommandFn;
 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
 
@@ -145,7 +144,7 @@ static void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
     serialEndWrite(msp->port);
 }
 
-static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
+static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
 {
     static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
 
@@ -179,7 +178,7 @@ static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp)
  *
  * Called periodically by the scheduler.
  */
-void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
+void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
 {
     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
         mspPort_t * const mspPort = &mspPorts[portIndex];
@@ -197,7 +196,7 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
             }
 
             if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
-                mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort);
+                mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
                 break; // process one command at a time so as not to block.
             }
         }
@@ -208,9 +207,8 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData)
     }
 }
 
-void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFnToUse)
+void mspSerialInit(void)
 {
-    mspProcessCommandFn = mspProcessCommandFnToUse;
     memset(mspPorts, 0, sizeof(mspPorts));
     mspSerialAllocatePorts();
 }
diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h
index 57704f33a3..f2f071464b 100644
--- a/src/main/msp/msp_serial.h
+++ b/src/main/msp/msp_serial.h
@@ -62,7 +62,7 @@ typedef struct mspPort_s {
 } mspPort_t;
 
 
-void mspSerialInit(mspProcessCommandFnPtr mspProcessCommandFn);
-void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData);
+void mspSerialInit(void);
+void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn);
 void mspSerialAllocatePorts(void);
 void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);