Prioritize module with higher update rates

radio/src/gui/128x64/model_setup.cpp

@@ -1405,7 +1405,7 @@ void menuModelSetup(event_t event)
       case ITEM_MODEL_SETUP_EXTERNAL_MODULE_SERIALSTATUS:
 #endif
         lcdDrawTextIndented(y, STR_STATUS);
-        lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerPeriod(), LEFT | attr);
+        lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerRealPeriod(moduleIdx), LEFT | attr);
         lcdDrawText(lcdNextPos, y, "Hz ", attr);
         break;
 #endif

radio/src/gui/212x64/model_setup.cpp

@@ -1260,8 +1260,8 @@ void menuModelSetup(event_t event)
       }
 
       case ITEM_MODEL_SETUP_EXTERNAL_MODULE_SERIALSTATUS:
-        lcdDrawTextIndented(y, STR_STATUS);
-        lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerPeriod(), LEFT | attr);
+         lcdDrawTextIndented(y, STR_STATUS);
+        lcdDrawNumber(MODEL_SETUP_2ND_COLUMN, y, 1000000 / getMixerSchedulerRealPeriod(EXTERNAL_MODULE), LEFT | attr);
         lcdDrawText(lcdNextPos, y, "Hz ", attr);
         break;
 #endif

radio/src/gui/colorlcd/module/crossfire_settings.cpp

@@ -61,9 +61,7 @@ CrossfireSettings::CrossfireSettings(Window* parent, const FlexGridLayout& g,
   new StaticText(line, rect_t{}, STR_STATUS);
   new DynamicText(line, rect_t{}, [=] {
     char msg[64] = "";
-    // sprintf(msg, "%d Hz %" PRIu32 " Err", 1000000 / getMixerSchedulerPeriod(),
-    //         telemetryErrors);
-    sprintf(msg, "%d Hz", 1000000 / getMixerSchedulerPeriod());
+    snprintf(msg, 64, "%d Hz", 1000000 / getMixerSchedulerRealPeriod(moduleIdx));
     return std::string(msg);
   });
 }

radio/src/gui/colorlcd/radio/radio_version.cpp

@@ -189,11 +189,10 @@ class VersionDialog : public BaseDialog
 #if defined(CROSSFIRE)
     // CRSF is able to provide status
     if (isModuleCrossfire(module)) {
-      char statusText[64];
+     char statusText[64];
+
+      snprintf(statusText, 64, "%d Hz", 1000000 / getMixerSchedulerRealPeriod(module));
 
-      auto hz = 1000000 / getMixerSchedulerPeriod();
-      // snprintf(statusText, 64, "%d Hz %" PRIu32 " Err", hz, telemetryErrors);
-      snprintf(statusText, 64, "%d Hz", hz);
       status->setText(statusText);
       snprintf(statusText, 64, "%s V%u.%u.%u", crossfireModuleStatus[module].name, crossfireModuleStatus[module].major, crossfireModuleStatus[module].minor, crossfireModuleStatus[module].revision);
       name->setText(statusText);
@@ -217,9 +216,12 @@ class VersionDialog : public BaseDialog
 #if defined(MULTIMODULE)
     // MPM is able to provide status
     if (isModuleMultimodule(module)) {
-      char statusText[64];
+      char mpmStatusText[20];
+      char statusText[40];
+
+      getMultiModuleStatus(module).getStatusString(mpmStatusText);
+      snprintf(statusText, 40, "%s %d Hz", mpmStatusText, 1000000 / getMixerSchedulerRealPeriod(module));
 
-      getMultiModuleStatus(module).getStatusString(statusText);
       status->setText(statusText);
       module_status_w->show();
     }
@@ -255,6 +257,9 @@ class VersionDialog : public BaseDialog
           mod_ver += " ";
           mod_ver += variants[variant];
         }
+
+        snprintf(tmp, 20, " %d Hz", 1000000 / getMixerSchedulerRealPeriod(module));
+        mod_ver += tmp;
       }
       status->setText(mod_ver);
       module_status_w->show();

radio/src/gui/common/stdlcd/radio_version.cpp

@@ -165,7 +165,7 @@ if (event == EVT_ENTRY) {
       if (isModuleCrossfire(module)) {
         char statusText[64] = "";
         sprintf(statusText, "%d Hz"/* %" PRIu32" Err"*/,
-                1000000 / getMixerSchedulerPeriod()/*, UINT32_C(telemetryErrors)*/);
+                1000000 / getMixerSchedulerRealPeriod(module)/*, UINT32_C(telemetryErrors)*/);
         lcdDrawText(COLUMN2_X, y, statusText);
         y += FH;
         lcdDrawText(INDENT_WIDTH, y, crossfireModuleStatus[module].name);

radio/src/mixer_scheduler.cpp

@@ -50,42 +50,91 @@ bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs)
 #endif
 }
 
-#if !defined(SIMU)
-
 // Global trigger flag
 
 // Mixer schedule
 struct MixerSchedule {
 
   // period in us
-  volatile uint16_t period;
+  volatile uint16_t period = 0;
+  volatile uint16_t divider = 1;
 };
 
 static MixerSchedule mixerSchedules[NUM_MODULES];
+static volatile uint8_t _syncedModule;
 
+// Called from ISR
 uint16_t getMixerSchedulerPeriod()
 {
-#if defined(HARDWARE_INTERNAL_MODULE)
-  if (mixerSchedules[INTERNAL_MODULE].period) {
-    return mixerSchedules[INTERNAL_MODULE].period;
+  bool hasActiveModules = false;
+  uint8_t synced_module = INTERNAL_MODULE;
+  uint16_t sync_period = MAX_REFRESH_RATE;
+
+  // Find the fastest of the modules. It becomes
+  // master, i.e. the one getting synced
+  for(uint8_t module = 0; module < NUM_MODULES; module++) {
+    auto& sched = mixerSchedules[module];
+
+    if(!isModuleNone(module)) {
+      hasActiveModules = true;
+
+      if(sched.period < sync_period) {
+        synced_module = module;
+        sync_period = sched.period;
+      }
+    } else 
+        sched.period = MIXER_SCHEDULER_DEFAULT_PERIOD_US;
   }
-#endif
-#if defined(HARDWARE_EXTERNAL_MODULE)
-  if (mixerSchedules[EXTERNAL_MODULE].period) {
-    return mixerSchedules[EXTERNAL_MODULE].period;
+
+  // Compute dividers for master and slave modules
+  for(uint8_t module = 0; module < NUM_MODULES; module++) {
+    auto& sched = mixerSchedules[module];
+
+    if(module == synced_module) 
+      sched.divider = 1;
+    else
+      sched.divider = sched.period / sync_period;
   }
-#endif
+
+  _syncedModule = synced_module;
+
+  // No active module
+  if(!hasActiveModules) {
 #if defined(STM32) && !defined(SIMU)
-  if (getSelectedUsbMode() == USB_JOYSTICK_MODE) {
-    return MIXER_SCHEDULER_JOYSTICK_PERIOD_US;
-  }
+    // no internal/external module and Joystick connected
+    if(getSelectedUsbMode() == USB_JOYSTICK_MODE) {
+      return MIXER_SCHEDULER_JOYSTICK_PERIOD_US;
+    }
 #endif
-  return MIXER_SCHEDULER_DEFAULT_PERIOD_US;
+
+    return MIXER_SCHEDULER_DEFAULT_PERIOD_US;
+  }
+
+  // at least one module is active
+  return sync_period;
+}
+
+uint16_t getMixerSchedulerRealPeriod(uint8_t moduleIdx) {
+  return mixerSchedules[_syncedModule].period * getMixerSchedulerDivider(moduleIdx);
+}
+
+uint16_t getMixerSchedulerDivider(uint8_t moduleIdx) {
+  return mixerSchedules[moduleIdx].divider;
+}
+
+uint8_t getMixerSchedulerSyncedModule() {
+  return _syncedModule;
 }
 
 void mixerSchedulerInit()
 {
-  memset(mixerSchedules, 0, sizeof(mixerSchedules));
+  _syncedModule = 0;
+
+  // set default divider and period (for simu as sync not active)
+  for(uint8_t module = 0; module < NUM_MODULES; module++) {
+    mixerSchedules[module].period = MIXER_SCHEDULER_DEFAULT_PERIOD_US;
+    mixerSchedules[module].divider = 1;
+  }
 }
 
 void mixerSchedulerSetPeriod(uint8_t moduleIdx, uint16_t periodUs)
@@ -105,6 +154,7 @@ uint16_t mixerSchedulerGetPeriod(uint8_t moduleIdx)
   return mixerSchedules[moduleIdx].period;
 }
 
+#if !defined(SIMU)
 void mixerSchedulerISRTrigger()
 {
   BaseType_t xHigherPriorityTaskWoken = pdFALSE;
@@ -124,5 +174,4 @@ void mixerSchedulerISRTrigger()
      called portEND_SWITCHING_ISR(). */
   portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
 }
-
 #endif

radio/src/mixer_scheduler.h

@@ -29,8 +29,6 @@
 #define MIN_REFRESH_RATE       850 /* us */
 #define MAX_REFRESH_RATE     50000 /* us */
 
-#if !defined(SIMU)
-
 // Call once to initialize the mixer scheduler
 void mixerSchedulerInit();
 
@@ -58,27 +56,33 @@ void mixerSchedulerSoftTrigger();
 // Fetch the current scheduling period
 uint16_t getMixerSchedulerPeriod();
 
+// fetch the mixer schedule divider
+uint16_t getMixerSchedulerDivider(uint8_t moduleIdx);
+
+// Fetch the module index of the module responsible for synchro
+uint8_t getMixerSchedulerSyncedModule();
+
+// Fetch the real mixer task period
+uint16_t getMixerSchedulerRealPeriod(uint8_t moduleIdx);
+
 // Trigger mixer from an ISR
 void mixerSchedulerISRTrigger();
 
-#else
+// Wait for the scheduler timer to trigger
+// returns true if timeout, false otherwise
+bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs);
 
-#define mixerSchedulerInit()
-#define mixerSchedulerStart()
-#define mixerSchedulerStop()
-#define mixerSchedulerSetPeriod(m,p) ((void)(p))
-#define mixerSchedulerGetPeriod(m) ((uint16_t)MIXER_SCHEDULER_DEFAULT_PERIOD_US)
+#if !defined(SIMU)
+// Configure and start the scheduler timer
+void mixerSchedulerStart();
 
+// Enable the timer trigger
+void mixerSchedulerEnableTrigger();
+
+// Disable the timer trigger
+void mixerSchedulerDisableTrigger();
+#else
+#define mixerSchedulerStart()
 #define mixerSchedulerEnableTrigger()
 #define mixerSchedulerDisableTrigger()
-
-#define mixerSchedulerSoftTrigger()
-
-#define getMixerSchedulerPeriod() (MIXER_SCHEDULER_DEFAULT_PERIOD_US)
-#define mixerSchedulerISRTrigger()
-
 #endif
-
-// Wait for the scheduler timer to trigger
-// returns true if timeout, false otherwise
-bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs);

radio/src/tasks/mixer_task.cpp

@@ -144,6 +144,8 @@ void execMixerFrequentActions()
 
 TASK_FUNCTION(mixerTask)
 {
+static uint16_t syncCounter = 0;
+
 #if defined(IMU)
   gyroInit();
 #endif
@@ -192,7 +194,21 @@ TASK_FUNCTION(mixerTask)
       mixerTaskLock();
 
       doMixerCalculations();
-      pulsesSendChannels();
+
+      syncCounter++;
+
+      if(getMixerSchedulerSyncedModule() == EXTERNAL_MODULE) {
+        pulsesSendNextFrame(EXTERNAL_MODULE);
+
+        if((syncCounter % getMixerSchedulerDivider(INTERNAL_MODULE)) == 0)
+          pulsesSendNextFrame(INTERNAL_MODULE);
+      } else {
+        pulsesSendNextFrame(INTERNAL_MODULE);
+
+        if((syncCounter % getMixerSchedulerDivider(EXTERNAL_MODULE)) == 0)
+            pulsesSendNextFrame(EXTERNAL_MODULE);
+      }
+
       doMixerPeriodicUpdates();
 
       // TODO: what are these for???
@@ -217,7 +233,6 @@ TASK_FUNCTION(mixerTask)
         maxMixerDuration = t0;
     }
   }
-
   TASK_RETURN();
 }
 

radio/src/tests/mixer_scheduler.cpp

@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) EdgeTX
+ *
+ * Based on code named
+ *   opentx - https://github.com/opentx/opentx
+ *   th9x - http://code.google.com/p/th9x
+ *   er9x - http://code.google.com/p/er9x
+ *   gruvin9x - http://code.google.com/p/gruvin9x
+ *
+ * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "gtests.h"
+#include "mixer_scheduler.h"
+
+TEST(MixerScheduler, MultiModules)
+{
+  // Init: both modules at 250Hz (4000us = MIXER_SCHEDULER_DEFAULT_PERIOD_US)
+  mixerSchedulerInit();
+  g_model.moduleData[INTERNAL_MODULE].type = MODULE_TYPE_MULTIMODULE;
+  g_model.moduleData[EXTERNAL_MODULE].type = MODULE_TYPE_CROSSFIRE;
+
+  EXPECT_EQ(getMixerSchedulerPeriod(), MIXER_SCHEDULER_DEFAULT_PERIOD_US);
+  EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 1);
+  EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 1);
+
+  // internal module 143Hz
+  // external module 500Hz
+  mixerSchedulerSetPeriod(INTERNAL_MODULE, 7000);
+  mixerSchedulerSetPeriod(EXTERNAL_MODULE, 2000);
+
+  EXPECT_EQ(getMixerSchedulerPeriod(), 2000);
+  EXPECT_EQ(getMixerSchedulerRealPeriod(INTERNAL_MODULE), 6000);
+  EXPECT_EQ(getMixerSchedulerRealPeriod(EXTERNAL_MODULE), 2000);
+  EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 3);
+  EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 1);
+
+  // internal module 143Hz
+  // external module 333Hz
+  mixerSchedulerSetPeriod(INTERNAL_MODULE, 7000);
+  mixerSchedulerSetPeriod(EXTERNAL_MODULE, 3003);
+
+  EXPECT_EQ(getMixerSchedulerPeriod(), 3003);
+  EXPECT_EQ(getMixerSchedulerRealPeriod(INTERNAL_MODULE), 6006);
+  EXPECT_EQ(getMixerSchedulerRealPeriod(EXTERNAL_MODULE), 3003);
+  EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 2);
+  EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 1);
+
+
+  // internal module 143Hz
+  // external module 100Hz
+  mixerSchedulerSetPeriod(INTERNAL_MODULE, 7000);
+  mixerSchedulerSetPeriod(EXTERNAL_MODULE, 10000);
+
+  EXPECT_EQ(getMixerSchedulerPeriod(), 7000);
+  EXPECT_EQ(getMixerSchedulerRealPeriod(INTERNAL_MODULE), 7000);
+  EXPECT_EQ(getMixerSchedulerRealPeriod(EXTERNAL_MODULE), 7000);
+  EXPECT_EQ(getMixerSchedulerDivider(INTERNAL_MODULE), 1);
+  EXPECT_EQ(getMixerSchedulerDivider(EXTERNAL_MODULE), 1);
+}