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

@@ -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;

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);
 

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

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);

src/main/mw.c

@@ -299,11 +299,13 @@ void mwDisarm(void)
         DISABLE_ARMING_FLAG(ARMED);
 
 #ifdef TELEMETRY
+        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;
         }
     }