Merge branch 'telemetry-switch' of https://github.com/Netsplits/cleanflight...

Merge branch 'telemetry-switch' of https://github.com/Netsplits/cleanflight into Netsplits-telemetry-switch

src/main/mw.c

@@ -320,14 +320,6 @@ void mwDisarm(void)
     if (ARMING_FLAG(ARMED)) {
         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();
-            mspAllocateSerialPorts(&masterConfig.serialConfig);
-        }
-#endif
-
 #ifdef BLACKBOX
         if (feature(FEATURE_BLACKBOX)) {
             finishBlackbox();
@@ -350,18 +342,6 @@ void mwArm(void)
             ENABLE_ARMING_FLAG(ARMED);
             headFreeModeHold = heading;
 
-#ifdef TELEMETRY
-            if (feature(FEATURE_TELEMETRY)) {
-
-
-                serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
-                while (sharedPort) {
-                    mspReleasePortIfAllocated(sharedPort);
-                    sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
-                }
-            }
-#endif
-
 #ifdef BLACKBOX
             if (feature(FEATURE_BLACKBOX)) {
                 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
@@ -680,6 +660,24 @@ void processRx(void)
     if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
     }
+
+#ifdef TELEMETRY
+	if (feature(FEATURE_TELEMETRY)) {
+		if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) ||
+		    (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
+			serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+			while (sharedPort) {
+				mspReleasePortIfAllocated(sharedPort);
+				sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
+			}
+		} else {
+            // the telemetry state must be checked immediately so that shared serial ports are released.
+            checkTelemetryState();
+            mspAllocateSerialPorts(&masterConfig.serialConfig);
+		}
+	}
+#endif
+
 }
 
 void loop(void)