From db36cfe12e70171c8d986b8587e2183dad0dc156 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Wed, 5 Nov 2014 19:39:13 +0000
Subject: [PATCH] Fix arm/disarm/arm bug when using shared msp/telemetry ports.

All msp ports are now re-initialised when disarming.

Bug wa introduced by c06fd78b834582349e1aaed07f582ae0c9fd3462 - see
#144.

Runtime serial port scenario should not have been erased.

Fixes #144 and #155. Cleanup #125.
---
 src/main/drivers/serial_uart.c |  4 ++--
 src/main/io/serial.c           |  3 +--
 src/main/io/serial_msp.c       |  4 ++++
 src/main/io/serial_msp.h       |  1 +
 src/main/mw.c                  | 14 ++++++++++++--
 src/main/telemetry/msp.c       |  2 +-
 6 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c
index facaa3f8c0..9af5f5af0d 100644
--- a/src/main/drivers/serial_uart.c
+++ b/src/main/drivers/serial_uart.c
@@ -41,6 +41,7 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
 static void uartReconfigure(uartPort_t *uartPort)
 {
     USART_InitTypeDef USART_InitStructure;
+    USART_Cmd(uartPort->USARTx, DISABLE);
 
     USART_InitStructure.USART_BaudRate = uartPort->port.baudRate;
     USART_InitStructure.USART_WordLength = USART_WordLength_8b;
@@ -59,6 +60,7 @@ static void uartReconfigure(uartPort_t *uartPort)
         USART_InitStructure.USART_Mode |= USART_Mode_Tx;
 
     USART_Init(uartPort->USARTx, &USART_InitStructure);
+    USART_Cmd(uartPort->USARTx, ENABLE);
 }
 
 serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion)
@@ -170,7 +172,6 @@ void uartSetBaudRate(serialPort_t *instance, uint32_t baudRate)
     uartPort_t *uartPort = (uartPort_t *)instance;
     uartPort->port.baudRate = baudRate;
     uartReconfigure(uartPort);
-    USART_Cmd(uartPort->USARTx, ENABLE);
 }
 
 void uartSetMode(serialPort_t *instance, portMode_t mode)
@@ -178,7 +179,6 @@ void uartSetMode(serialPort_t *instance, portMode_t mode)
     uartPort_t *uartPort = (uartPort_t *)instance;
     uartPort->port.mode = mode;
     uartReconfigure(uartPort);
-    USART_Cmd(uartPort->USARTx, ENABLE);
 }
 
 void uartStartTxDMA(uartPort_t *s)
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index a3622e0d0c..99bb2c4375 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -32,10 +32,10 @@
 #include "drivers/serial_uart.h"
 #include "drivers/serial_usb_vcp.h"
 
+#include "io/serial.h"
 #include "serial_cli.h"
 #include "serial_msp.h"
 
-#include "io/serial.h"
 #include "config/config.h"
 
 uint32_t getTelemetryProviderBaudRate(void);
@@ -372,7 +372,6 @@ void endSerialPortFunction(serialPort_t *port, serialPortFunction_e function)
     serialPortFunction_t *serialPortFunction = findSerialPortFunctionByPort(port);
 
     serialPortFunction->currentFunction = FUNCTION_NONE;
-    serialPortFunction->scenario = SCENARIO_UNUSED;
     serialPortFunction->port = NULL;
 }
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 80341e4d94..9ffed49067 100755
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -615,7 +615,11 @@ void mspInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXSONAR;
     }
 
+    mspReset(serialConfig);
+}
 
+void mspReset(serialConfig_t *serialConfig)
+{
     memset(mspPorts, 0x00, sizeof(mspPorts));
 
     openAllMSPSerialPorts(serialConfig);
diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h
index ff2c09be25..6b06b42dbb 100644
--- a/src/main/io/serial_msp.h
+++ b/src/main/io/serial_msp.h
@@ -23,3 +23,4 @@
 void mspProcess(void);
 void sendMspTelemetry(void);
 void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);
+void mspReset(serialConfig_t *serialConfig);
diff --git a/src/main/mw.c b/src/main/mw.c
index bc56fb1e90..6a4931bd44 100755
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -59,8 +59,9 @@
 #include "io/gimbal.h"
 #include "io/gps.h"
 #include "io/ledstrip.h"
-#include "io/serial_cli.h"
 #include "io/serial.h"
+#include "io/serial_cli.h"
+#include "io/serial_msp.h"
 #include "io/statusindicator.h"
 #include "rx/msp.h"
 #include "telemetry/telemetry.h"
@@ -294,8 +295,17 @@ void annexCode(void)
 
 void mwDisarm(void)
 {
-    if (ARMING_FLAG(ARMED))
+    if (ARMING_FLAG(ARMED)) {
         DISABLE_ARMING_FLAG(ARMED);
+
+#ifdef TELEMETRY
+        // the telemetry state must be checked immediately so that shared serial ports are released.
+        checkTelemetryState();
+        if (isSerialPortFunctionShared(FUNCTION_TELEMETRY, FUNCTION_MSP)) {
+            mspReset(&masterConfig.serialConfig);
+        }
+#endif
+    }
 }
 
 void mwArm(void)
diff --git a/src/main/telemetry/msp.c b/src/main/telemetry/msp.c
index b3c7a7fc43..07a9dc3cbb 100644
--- a/src/main/telemetry/msp.c
+++ b/src/main/telemetry/msp.c
@@ -33,8 +33,8 @@
 
 #include "drivers/serial.h"
 #include "telemetry/telemetry.h"
-#include "io/serial_msp.h"
 #include "io/serial.h"
+#include "io/serial_msp.h"
 
 static telemetryConfig_t *telemetryConfig;