From 8b30841ab7f3edd7b6230539c0503b65b5a8344b Mon Sep 17 00:00:00 2001
From: giacomo892 <giacomo892@users.noreply.github.com>
Date: Wed, 11 Apr 2018 11:07:50 +0200
Subject: [PATCH] ltm shared scheduler fix

---
 src/main/telemetry/ltm.c | 29 ++++++++++++++++++++---------
 1 file changed, 20 insertions(+), 9 deletions(-)

diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
index ed73fef532..3f0cb539e9 100644
--- a/src/main/telemetry/ltm.c
+++ b/src/main/telemetry/ltm.c
@@ -400,6 +400,21 @@ void initLtmTelemetry(void)
     ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
 }
 
+
+
+static void configureLtmScheduler(void)
+{
+
+    /* setup scheduler, default to 'normal' */
+    if (telemetryConfig()->ltmUpdateRate == LTM_RATE_MEDIUM)
+        ltm_schedule = ltm_medium_schedule;
+    else if (telemetryConfig()->ltmUpdateRate == LTM_RATE_SLOW)
+        ltm_schedule = ltm_slow_schedule;
+    else
+        ltm_schedule = ltm_normal_schedule;
+
+}
+
 void configureLtmTelemetryPort(void)
 {
     if (!portConfig) {
@@ -410,14 +425,6 @@ void configureLtmTelemetryPort(void)
         baudRateIndex = BAUD_19200;
     }
 
-    /* setup scheduler, default to 'normal' */
-    if (telemetryConfig()->ltmUpdateRate == LTM_RATE_MEDIUM)
-        ltm_schedule = ltm_medium_schedule;
-    else if (telemetryConfig()->ltmUpdateRate == LTM_RATE_SLOW)
-        ltm_schedule = ltm_slow_schedule;
-    else
-        ltm_schedule = ltm_normal_schedule;
-
     /* Sanity check that we can support the scheduler */
     if (baudRateIndex == BAUD_2400 && telemetryConfig()->ltmUpdateRate == LTM_RATE_NORMAL)
          ltm_schedule = ltm_medium_schedule;
@@ -436,14 +443,18 @@ void checkLtmTelemetryState(void)
     if (portConfig && telemetryCheckRxPortShared(portConfig)) {
         if (!ltmEnabled && telemetrySharedPort != NULL) {
             ltmPort = telemetrySharedPort;
+            configureLtmScheduler();
             ltmEnabled = true;
         }
     } else {
         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing);
         if (newTelemetryEnabledValue == ltmEnabled)
             return;
-        if (newTelemetryEnabledValue)
+        if (newTelemetryEnabledValue){
+            configureLtmScheduler();
             configureLtmTelemetryPort();
+            
+    }
         else
             freeLtmTelemetryPort();
     }