From 6b1a6332b94a82bd9e91fd3062f6cd69ee9e8a56 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 7 Apr 2014 21:38:50 +0100
Subject: [PATCH] Ensure that telemetry state is always updated after
 initialising telemetry so that providers can configure the telemetry port as
 applicable.

---
 src/mw.c               | 5 ++---
 src/telemetry_common.c | 6 ++++++
 2 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/src/mw.c b/src/mw.c
index 4460bb68ec..463b5951dd 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -180,9 +180,8 @@ void annexCode(void)
             LED0_OFF;
         if (f.ARMED)
             LED0_ON;
-        // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes. With telemetry_softserial>0 telemetry is always enabled, also see updateTelemetryState()
-        if (feature(FEATURE_TELEMETRY))
-            checkTelemetryState();
+
+        checkTelemetryState();
     }
 
 #ifdef LEDRING
diff --git a/src/telemetry_common.c b/src/telemetry_common.c
index 5c2fee999b..cc7031e198 100644
--- a/src/telemetry_common.c
+++ b/src/telemetry_common.c
@@ -26,6 +26,8 @@ void initTelemetry(void)
         core.telemport = &(softSerialPorts[1].port);
     else
         core.telemport = core.mainport;
+
+    checkTelemetryState();
 }
 
 static bool telemetryEnabled = false;
@@ -71,6 +73,10 @@ void freeTelemetryPort(void) {
 
 void checkTelemetryState(void)
 {
+    if (!feature(FEATURE_TELEMETRY)) {
+        return;
+    }
+
     bool telemetryCurrentlyEnabled = isTelemetryEnabled();
 
     if (!shouldChangeTelemetryStateNow(telemetryCurrentlyEnabled)) {