ltm shared scheduler fix

iNavFlight · Apr 11, 2018 · e4f4c8b · e4f4c8b
1 parent 2bb8bdb
commit e4f4c8b
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 9 deletions.
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
@@ -400,15 +400,10 @@ void initLtmTelemetry(void)
     ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
 }
 
-void configureLtmTelemetryPort(void)
+
+
+void configureLtmScheduler(void)
 {
-    if (!portConfig) {
-        return;
-    }
-    baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
-    if (baudRateIndex == BAUD_AUTO) {
-        baudRateIndex = BAUD_19200;
-    }
 
     /* setup scheduler, default to 'normal' */
     if (telemetryConfig()->ltmUpdateRate == LTM_RATE_MEDIUM)
@@ -418,6 +413,18 @@ void configureLtmTelemetryPort(void)
     else
         ltm_schedule = ltm_normal_schedule;
 
+}
+
+void configureLtmTelemetryPort(void)
+{
+    if (!portConfig) {
+        return;
+    }
+    baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
+    if (baudRateIndex == BAUD_AUTO) {
+        baudRateIndex = BAUD_19200;
+    }
+
     /* 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,17 @@ 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){
             configureLtmTelemetryPort();
+            configureLtmScheduler();
+    }
         else
             freeLtmTelemetryPort();
     }

diff --git a/src/main/telemetry/ltm.h b/src/main/telemetry/ltm.h
@@ -50,6 +50,7 @@ void handleLtmTelemetry(void);
 void checkLtmTelemetryState(void);
 
 void freeLtmTelemetryPort(void);
+void configureLtmScheduler(void);
 void configureLtmTelemetryPort(void);
 
 int getLtmFrame(uint8_t *frame, ltm_frame_e ltmFrameType);