WiP

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

@@ -1628,7 +1628,7 @@ void menuModelSetup(event_t event)
         lcdDrawTextAlignedLeft(y, STR_MODULE_SYNC);
 
         char statusText[64];
-        getMultiSyncStatus(moduleIdx).getRefreshString(statusText);
+        getModuleSyncStatus(moduleIdx).getRefreshString(statusText);
         lcdDrawText(MODEL_SETUP_2ND_COLUMN, y, statusText);
         break;
       }

radio/src/gui/gui_common.h

@@ -161,6 +161,7 @@ inline uint8_t MODULE_CHANNELS_ROWS(int moduleIdx)
 
 
 #if defined(MULTIMODULE)
+
 inline uint8_t MULTI_DISABLE_CHAN_MAP_ROW(uint8_t moduleIdx)
 {
   if (!isModuleMultimodule(moduleIdx))

radio/src/pulses/pulses.cpp

@@ -273,8 +273,13 @@ void setupPulsesExternalModule(uint8_t protocol)
 
 #if defined(MULTIMODULE)
     case PROTOCOL_CHANNELS_MULTIMODULE:
+    {
+      ModuleSyncStatus& status = getModuleSyncStatus(EXTERNAL_MODULE);
+      if (status.isValid())
+        mixerSchedulerSetPeriod(EXTERNAL_MODULE, status.getAdjustedRefreshRate());
       setupPulsesMultiExternalModule();
       break;
+    }
 #endif
 
 #if defined(PPM)

radio/src/rtos.h

@@ -241,7 +241,7 @@ template<int SIZE>
   #define RTOS_CREATE_FLAG(flag)        flag = CoCreateFlag(false, false)
   #define RTOS_SET_FLAG(flag)           (void)CoSetFlag(flag)
   #define RTOS_CLEAR_FLAG(flag)         (void)CoClearFlag(flag)
-  #define RTOS_WAIT_FLAG(flag,timeout)  (void)CoWaitForSingleFlag(flag,timeout)
+  #define RTOS_WAIT_FLAG(flag,timeout)  CoWaitForSingleFlag(flag,timeout)
 
   static inline void RTOS_ISR_SET_FLAG(RTOS_FLAG_HANDLE flag)
   {

radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp

@@ -91,10 +91,10 @@ void mixerSchedulerDisableTrigger()
     MIXER_SCHEDULER_TIMER->DIER &= ~TIM_DIER_UIE; // disable interrupt
 }
 
-void mixerSchedulerWaitForTrigger(uint8_t timeoutMs)
+bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs)
 {
     RTOS_CLEAR_FLAG(mixerFlag);
-    RTOS_WAIT_FLAG(mixerFlag, timeoutMs);
+    return RTOS_WAIT_FLAG(mixerFlag, timeoutMs) == E_TIMEOUT;
 }
 
 void mixerScheduleISRTrigger()

radio/src/targets/common/arm/stm32/mixer_scheduler_driver.h

@@ -34,7 +34,8 @@ void mixerSchedulerStop();
 void mixerSchedulerSetPeriod(uint8_t moduleIdx, uint16_t periodUs);
 
 // Wait for the scheduler timer to trigger
-void mixerSchedulerWaitForTrigger(uint8_t timeoutMs);
+// returns true if timeout, false otherwise
+bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs);
 
 // Enable the timer trigger
 void mixerSchedulerEnableTrigger();

radio/src/tasks.cpp

@@ -143,7 +143,7 @@ TASK_FUNCTION(mixerTask)
     // - add trigger based on heartbeat driver
     
     // run mixer at least every 10ms
-    mixerSchedulerWaitForTrigger(10);
+    bool timeout = mixerSchedulerWaitForTrigger(10);
 
 #if defined(DEBUG_MIXER_SCHEDULER)
     GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
@@ -200,6 +200,7 @@ TASK_FUNCTION(mixerTask)
       if (t0 > maxMixerDuration)
         maxMixerDuration = t0;
 
+      //if (!timeout)
       sendSynchronousPulses();
     }
   }

radio/src/telemetry/crossfire.cpp

@@ -178,6 +178,17 @@ void processCrossfireTelemetryFrame()
       break;
     }
 
+    // case RADIO_ID:
+    // {
+    //   uint32_t update_interval, offset;
+    //   if (getCrossfireTelemetryValue<4>(2, update_interval) && getCrossfireTelemetryValue<4>(2, offset)) {
+    //     // values are in 10th of micro-seconds
+    //     update_interval /= 10;
+    //     offset /= 10;
+    //     getModuleSyncStatus(EXTERNAL_MODULE).update(update_interval, offset);
+    //   }
+    // }
+
 #if defined(LUA)
     default:
       if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) {

radio/src/telemetry/multi.cpp

@@ -62,7 +62,6 @@ enum MultiBufferState : uint8_t
 #if defined(INTERNAL_MODULE_MULTI)
 
 static MultiModuleStatus multiModuleStatus[NUM_MODULES] = {MultiModuleStatus(), MultiModuleStatus()};
-static MultiModuleSyncStatus multiSyncStatus[NUM_MODULES] = {MultiModuleSyncStatus(), MultiModuleSyncStatus()};
 static uint8_t multiBindStatus[NUM_MODULES] = {MULTI_NORMAL_OPERATION, MULTI_NORMAL_OPERATION};
 
 static MultiBufferState multiTelemetryBufferState[NUM_MODULES];
@@ -73,11 +72,6 @@ MultiModuleStatus &getMultiModuleStatus(uint8_t module)
   return multiModuleStatus[module];
 }
 
-MultiModuleSyncStatus &getMultiSyncStatus(uint8_t module)
-{
-  return multiSyncStatus[module];
-}
-
 uint8_t getMultiBindStatus(uint8_t module)
 {
   return multiBindStatus[module];
@@ -110,7 +104,6 @@ uint8_t intTelemetryRxBufferCount;
 #else // !INTERNAL_MODULE_MULTI
 
 static MultiModuleStatus multiModuleStatus;
-static MultiModuleSyncStatus multiSyncStatus;
 static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION;
 
 static MultiBufferState multiTelemetryBufferState;
@@ -121,11 +114,6 @@ MultiModuleStatus& getMultiModuleStatus(uint8_t)
   return multiModuleStatus;
 }
 
-MultiModuleSyncStatus& getMultiSyncStatus(uint8_t)
-{
-  return multiSyncStatus;
-}
-
 uint8_t getMultiBindStatus(uint8_t)
 {
   return multiBindStatus;
@@ -253,24 +241,41 @@ static void processMultiStatusPacket(const uint8_t * data, uint8_t module, uint8
 
 static void processMultiSyncPacket(const uint8_t * data, uint8_t module)
 {
-  MultiModuleSyncStatus &status = getMultiSyncStatus(module);
+  ModuleSyncStatus &status = getModuleSyncStatus(module);
 
-  status.lastUpdate = get_tmr10ms();
+//   status.lastUpdate = get_tmr10ms();
-  status.interval = data[4];
+//   status.interval = data[4];
-  status.target = data[5];
+//   status.target = data[5];
-#if !defined(PPM_PIN_SERIAL)
+// #if !defined(PPM_PIN_SERIAL)
-  auto oldlag = status.inputLag;
+//   auto oldlag = status.inputLag;
-  (void) oldlag;
+//   (void) oldlag;
-#endif
+// #endif
 
-  status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]);
+//   uint16_t refreshRate = data[0] << 8 | data[1];
+//   status.calcAdjustedRefreshRate(refreshRate, data[2] << 8 | data[3]);
 
-#if !defined(PPM_PIN_SERIAL)
+//   serialPrint("MP ADJ: R %d, L %04d, T %03d, calc %04d",
-  TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d  target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n",
+//               refreshRate,
-        module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
+//               data[2] << 8 | data[3],
-        status.inputLag, oldlag - status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate / 50,
+//               status.target,
-        status.getAdjustedRefreshRate());
+//               status.getAdjustedRefreshRate()/2);
-#endif
+  uint16_t refreshRate = data[0] << 8 | data[1];
+  int16_t  inputLag    = data[2] << 8 | data[3];
+
+  // if (inputLag > refreshRate/2)
+  //   inputLag -= refreshRate;
+
+  status.update(refreshRate, inputLag);
+
+  serialPrint("MP ADJ: R %d, L %04d",
+              refreshRate, inputLag);
+
+// #if !defined(PPM_PIN_SERIAL)
+//   TRACE("MP ADJ: rest: %d, lag %04d, diff: %04d  target: %d, interval: %d, Refresh: %d, intAdjRefresh: %d, adjRefresh %d\r\n",
+//         module == EXTERNAL_MODULE ? extmodulePulsesData.dsm2.rest : 0,
+//         status.inputLag, oldlag - status.inputLag, status.target, status.interval, status.refreshRate, status.adjustedRefreshRate / 50,
+//         status.getAdjustedRefreshRate());
+// #endif
 }
 
 #if defined(PCBTARANIS) || defined(PCBHORUS)
@@ -420,104 +425,6 @@ static void processMultiTelemetryPaket(const uint8_t * packet, uint8_t module)
   }
 }
 
-#define MIN_REFRESH_RATE      5500
-
-void MultiModuleSyncStatus::calcAdjustedRefreshRate(uint16_t newRefreshRate, uint16_t newInputLag)
-{
-  // Check how far off we are from our target, positive means we are too slow, negative we are too fast
-  int lagDifference = newInputLag - inputLag;
-
-  // The refresh rate that we target
-  // Below is least common multiple of MIN_REFRESH_RATE and requested rate
-  uint16_t targetRefreshRate = (uint16_t) (newRefreshRate * ((MIN_REFRESH_RATE / (newRefreshRate - 1)) + 1));
-
-  // Overflow, reverse sample
-  if (lagDifference < -targetRefreshRate / 2)
-    lagDifference = -lagDifference;
-
-
-  // Reset adjusted refresh if rate has changed
-  if (newRefreshRate != refreshRate) {
-    refreshRate = newRefreshRate;
-    adjustedRefreshRate = targetRefreshRate;
-    if (adjustedRefreshRate >= 30000)
-      adjustedRefreshRate /= 2;
-
-    // Our refresh rate in ps
-    adjustedRefreshRate *= 1000;
-    return;
-  }
-
-  // Caluclate how many samples went into the reported input Lag (*10)
-  int numsamples = interval * 10000 / targetRefreshRate;
-
-  // Convert lagDifference to ps
-  lagDifference = lagDifference * 1000;
-
-  // Calculate the time we intentionally were late/early
-  if (inputLag > target * 10 + 30)
-    lagDifference += numsamples * 500;
-  else if (inputLag < target * 10 - 30)
-    lagDifference -= numsamples * 500;
-
-  // Caculate the time in ps each frame is to slow (positive), fast(negative)
-  int perframeps = lagDifference * 10 / numsamples;
-
-  if (perframeps > 20000)
-    perframeps = 20000;
-
-  if (perframeps < -20000)
-    perframeps = -20000;
-
-  adjustedRefreshRate = (adjustedRefreshRate + perframeps);
-
-  // Safeguards
-  if (adjustedRefreshRate < MIN_REFRESH_RATE * 1000)
-    adjustedRefreshRate = MIN_REFRESH_RATE * 1000;
-  if (adjustedRefreshRate > 30 * 1000 * 1000)
-    adjustedRefreshRate = 30 * 1000 * 1000;
-
-  inputLag = newInputLag;
-}
-
-static uint8_t counter;
-
-const uint16_t MultiModuleSyncStatus::getAdjustedRefreshRate()
-{
-  if (!isValid() || refreshRate == 0)
-    return 18000;
-
-  counter = (uint8_t) (counter + 1 % 10);
-  uint16_t rate = (uint16_t) ((adjustedRefreshRate + counter * 50) / 500);
-  // Check how far off we are from our target, positive means we are too slow, negative we are too fast
-  if (inputLag > target * 10 + 30)
-    return (uint16_t) (rate - 1);
-  else if (inputLag < target * 10 - 30)
-    return (uint16_t) (rate + 1);
-  else
-    return rate;
-}
-
-void MultiModuleSyncStatus::getRefreshString(char * statusText)
-{
-  if (!isValid()) {
-    return;
-  }
-
-  char * tmp = statusText;
-#if defined(DEBUG)
-  *tmp++ = 'L';
-  tmp = strAppendUnsigned(tmp, inputLag, 5);
-  tmp = strAppend(tmp, "us R ");
-  tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000), 5);
-  tmp = strAppend(tmp, "us");
-#else
-  tmp = strAppend(tmp, "Sync at ");
-  tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000000));
-  tmp = strAppend(tmp, " ms");
-#endif
-}
-
 void MultiModuleStatus::getStatusString(char * statusText) const
 {
   if (!isValid()) {

radio/src/telemetry/multi.h

@@ -94,28 +94,6 @@ void processMultiTelemetryData(uint8_t data, uint8_t module);
 
 #define MULTI_SCANNER_MAX_CHANNEL 249
 
-// This should be put into the Module definition if other modules gain this functionality
-struct MultiModuleSyncStatus {
-  uint32_t adjustedRefreshRate = 9000 * 1000;    // in ps
-  tmr10ms_t lastUpdate;
-  uint16_t refreshRate;
-  uint16_t inputLag;
-  uint8_t interval;
-  uint8_t target;
-
-  inline bool isValid() const
-  {
-    return (get_tmr10ms()  - lastUpdate < 100);
-  }
-  
-  void getRefreshString(char * refreshText);
-  const uint16_t getAdjustedRefreshRate();
-  void calcAdjustedRefreshRate(uint16_t newRefreshRate, uint16_t newInputLag);
-};
-
-MultiModuleSyncStatus& getMultiSyncStatus(uint8_t module);
-
-
 struct MultiModuleStatus {
 
   uint8_t major;

radio/src/telemetry/telemetry.cpp

@@ -337,3 +337,110 @@ OutputTelemetryBuffer outputTelemetryBuffer __DMA;
 #if defined(LUA)
 Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
 #endif
+
+#define MIN_REFRESH_RATE      4000 /* us */
+#define MAX_REFRESH_RATE     25000 /* us */
+#define SAFE_SYNC_LAG          800 /* us */
+
+#if defined(HARDWARE_INTERNAL_MODULE)
+
+static ModuleSyncStatus moduleSyncStatus[NUM_MODULES];
+
+ModuleSyncStatus &getModuleSyncStatus(uint8_t moduleIdx)
+{
+  return moduleSyncStatus[moduleIdx];
+}
+
+#else
+
+static ModuleSyncStatus moduleSyncStatus;
+
+ModuleSyncStatus &getModuleSyncStatus(uint8_t moduleIdx)
+{
+  return moduleSyncStatus;
+}
+
+#endif
+
+ModuleSyncStatus::ModuleSyncStatus()
+{
+  memset(this, 0, sizeof(ModuleSyncStatus));
+}
+
+void ModuleSyncStatus::update(uint16_t newRefreshRate, uint16_t newInputLag)
+{
+  if (!newRefreshRate)
+    return;
+  
+  if (newRefreshRate < MIN_REFRESH_RATE)
+    newRefreshRate = newRefreshRate * (MIN_REFRESH_RATE / (newRefreshRate + 1));
+  else if (newRefreshRate > MAX_REFRESH_RATE)
+    newRefreshRate = MAX_REFRESH_RATE;
+
+  refreshRate = newRefreshRate;
+  inputLag    = newInputLag;
+  currentLag  = newInputLag;
+  lastUpdate  = get_tmr10ms();
+}
+
+uint16_t ModuleSyncStatus::getAdjustedRefreshRate()
+{
+  int16_t lag = currentLag - SAFE_SYNC_LAG;
+  int32_t newRefreshRate = refreshRate;
+
+  newRefreshRate += lag/2;
+  
+  if (newRefreshRate < MIN_REFRESH_RATE) {
+      newRefreshRate = MIN_REFRESH_RATE;
+  }
+  else if (newRefreshRate > MAX_REFRESH_RATE) {
+    newRefreshRate = MAX_REFRESH_RATE;
+  }
+  
+  currentLag -= newRefreshRate - refreshRate;
+  return (uint16_t)newRefreshRate;
+}
+
+// sprintf does not work AVR ARM
+// use a small helper function
+static void appendInt(char * buf, uint32_t val)
+{
+  while (*buf)
+    buf++;
+
+  strAppendUnsigned(buf, val);
+}
+
+static void prependSpaces(char * buf, int val)
+{
+  while (*buf)
+    buf++;
+
+  int k = 10000;
+  while (val / k == 0 && k > 0) {
+    *buf = ' ';
+    buf++;
+    k /= 10;
+  }
+  *buf = '\0';
+}
+
+void ModuleSyncStatus::getRefreshString(char * statusText)
+{
+  if (!isValid()) {
+    return;
+  }
+
+  char * tmp = statusText;
+#if defined(DEBUG)
+  *tmp++ = 'L';
+  tmp = strAppendUnsigned(tmp, inputLag, 5);
+  tmp = strAppend(tmp, "us R ");
+  tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000), 5);
+  tmp = strAppend(tmp, "us");
+#else
+  tmp = strAppend(tmp, "Sync at ");
+  tmp = strAppendUnsigned(tmp, (uint32_t) (adjustedRefreshRate / 1000000));
+  tmp = strAppend(tmp, " ms");
+#endif
+}

radio/src/telemetry/telemetry.h

@@ -257,4 +257,32 @@ extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
 
 void processPXX2Frame(uint8_t module, const uint8_t *frame);
 
+// Module pulse synchronization
+struct ModuleSyncStatus
+{
+  // feedback input: last received values
+  uint16_t  refreshRate; // in us
+  int16_t   inputLag;    // in us
+
+  tmr10ms_t lastUpdate;  // in 10ms
+  int16_t   currentLag;  // in us
+  
+  inline bool isValid() {
+    return (get_tmr10ms() - lastUpdate < 100);
+  }
+
+  // Set feedback from RF module
+  void update(uint16_t newRefreshRate, uint16_t newInputLag);
+
+  // Get computed settings for scheduler
+  uint16_t getAdjustedRefreshRate();
+
+  // Status string for the UI
+  void getRefreshString(char* refreshText);
+
+  ModuleSyncStatus();
+};
+
+ModuleSyncStatus& getModuleSyncStatus(uint8_t moduleIdx);
+
 #endif // _TELEMETRY_H_