From 2461973dd66c48984d9ecdddfd50e42e73afebf0 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Sat, 8 Nov 2014 01:11:10 +0000
Subject: [PATCH] There was a little more fixed to do when telemetry was not
 eanbled.

Tested arm/disarm/arm/disarm with telemetry on and off, all working now.

See #155.
---
 src/main/io/serial.c     | 11 +++++++++++
 src/main/io/serial.h     |  1 +
 src/main/io/serial_msp.c | 12 ++++++++++++
 src/main/io/serial_msp.h |  1 +
 src/main/mw.c            | 19 +++++++++++++++----
 5 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index 301bd45b6..3a185da56 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -513,6 +513,17 @@ bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t fun
     return result->portFunction->scenario & functionMask;
 }
 
+serialPort_t *findSharedSerialPort(serialPortFunction_e functionToUse, uint16_t functionMask)
+{
+    functionConstraint_t *functionConstraint = getConfiguredFunctionConstraint(functionToUse);
+    serialPortSearchResult_t *result = findSerialPort(functionToUse, functionConstraint);
+
+    if (result->portFunction->scenario & functionMask) {
+        return result->portFunction->port;
+    }
+    return NULL;
+}
+
 void applySerialConfigToPortFunctions(serialConfig_t *serialConfig)
 {
     uint32_t portIndex = 0, serialPortIdentifier;
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index a1a371476..f104b8c15 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -161,6 +161,7 @@ void applySerialConfigToPortFunctions(serialConfig_t *serialConfig);
 bool isSerialConfigValid(serialConfig_t *serialConfig);
 bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier);
 bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask);
+serialPort_t *findSharedSerialPort(serialPortFunction_e functionToUse, uint16_t functionMask);
 
 const serialPortFunctionList_t *getSerialPortFunctionList(void);
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 510d78846..82027b638 100755
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -561,6 +561,18 @@ static void openAllMSPSerialPorts(serialConfig_t *serialConfig)
     UNUSED(serialPortFunctionList);
 }
 
+void mspReleasePortIfAllocated(serialPort_t *serialPort)
+{
+    uint8_t portIndex;
+    for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
+        mspPort_t *candidateMspPort = &mspPorts[portIndex++];
+        if (candidateMspPort->port == serialPort) {
+            endSerialPortFunction(serialPort, FUNCTION_MSP);
+            memset(candidateMspPort, 0, sizeof(mspPort_t));
+        }
+    }
+}
+
 void mspInit(serialConfig_t *serialConfig)
 {
     // calculate used boxes based on features and fill availableBoxes[] array
diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h
index 6b06b42db..bbc6303f1 100644
--- a/src/main/io/serial_msp.h
+++ b/src/main/io/serial_msp.h
@@ -24,3 +24,4 @@ void mspProcess(void);
 void sendMspTelemetry(void);
 void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);
 void mspReset(serialConfig_t *serialConfig);
+void mspReleasePortIfAllocated(serialPort_t *serialPort);
diff --git a/src/main/mw.c b/src/main/mw.c
index 4f2fa1915..ee8473b52 100755
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -299,10 +299,12 @@ void mwDisarm(void)
         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);
+        if (feature(FEATURE_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
     }
@@ -317,6 +319,15 @@ void mwArm(void)
         if (!ARMING_FLAG(PREVENT_ARMING)) {
             ENABLE_ARMING_FLAG(ARMED);
             headFreeModeHold = heading;
+
+#ifdef TELEMETRY
+            if (feature(FEATURE_TELEMETRY)) {
+                serialPort_t *sharedTelemetryAndMspPort = findSharedSerialPort(FUNCTION_TELEMETRY, FUNCTION_MSP);
+                if (sharedTelemetryAndMspPort) {
+                    mspReleasePortIfAllocated(sharedTelemetryAndMspPort);
+                }
+            }
+#endif
             return;
         }
     }
-- 
2.11.4.GIT