From b5da1990db8010fd0d0c6a014df0b3334d22cb86 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Wed, 6 Nov 2019 16:53:59 +0100 Subject: [PATCH] WiP --- radio/src/gui/128x64/model_setup.cpp | 2 +- radio/src/gui/gui_common.h | 1 + radio/src/pulses/pulses.cpp | 5 + radio/src/rtos.h | 2 +- .../arm/stm32/mixer_scheduler_driver.cpp | 4 +- .../common/arm/stm32/mixer_scheduler_driver.h | 3 +- radio/src/tasks.cpp | 3 +- radio/src/telemetry/crossfire.cpp | 11 ++ radio/src/telemetry/multi.cpp | 157 ++++-------------- radio/src/telemetry/multi.h | 22 --- radio/src/telemetry/telemetry.cpp | 107 ++++++++++++ radio/src/telemetry/telemetry.h | 28 ++++ 12 files changed, 192 insertions(+), 153 deletions(-) diff --git a/radio/src/gui/128x64/model_setup.cpp b/radio/src/gui/128x64/model_setup.cpp index 3a67ba80e..8ae39e18a 100644 --- a/radio/src/gui/128x64/model_setup.cpp +++ b/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; } diff --git a/radio/src/gui/gui_common.h b/radio/src/gui/gui_common.h index ccbf535eb..52b3d8b29 100644 --- a/radio/src/gui/gui_common.h +++ b/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)) diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp index e70b4176b..e1bbf81df 100755 --- a/radio/src/pulses/pulses.cpp +++ b/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) diff --git a/radio/src/rtos.h b/radio/src/rtos.h index caf1afb6a..357e80527 100644 --- a/radio/src/rtos.h +++ b/radio/src/rtos.h @@ -241,7 +241,7 @@ template #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) { diff --git a/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp b/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp index 3e374a0d4..7caceaef5 100644 --- a/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp +++ b/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() diff --git a/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.h b/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.h index c9a056790..98a7f547f 100644 --- a/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.h +++ b/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(); diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index e9b7e70fb..a83a319a7 100644 --- a/radio/src/tasks.cpp +++ b/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(); } } diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index b233583a4..6c544004a 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/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) ) { diff --git a/radio/src/telemetry/multi.cpp b/radio/src/telemetry/multi.cpp index e6b41faae..00a6902e1 100644 --- a/radio/src/telemetry/multi.cpp +++ b/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.interval = data[4]; - status.target = data[5]; -#if !defined(PPM_PIN_SERIAL) - auto oldlag = status.inputLag; - (void) oldlag; -#endif +// status.lastUpdate = get_tmr10ms(); +// status.interval = data[4]; +// status.target = data[5]; +// #if !defined(PPM_PIN_SERIAL) +// auto oldlag = status.inputLag; +// (void) oldlag; +// #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) - 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 +// serialPrint("MP ADJ: R %d, L %04d, T %03d, calc %04d", +// refreshRate, +// data[2] << 8 | data[3], +// status.target, +// status.getAdjustedRefreshRate()/2); + 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()) { diff --git a/radio/src/telemetry/multi.h b/radio/src/telemetry/multi.h index e9b9ada15..ca9d3fc79 100644 --- a/radio/src/telemetry/multi.h +++ b/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; diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 4e7bc8550..2486af929 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -337,3 +337,110 @@ OutputTelemetryBuffer outputTelemetryBuffer __DMA; #if defined(LUA) Fifo * 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 +} diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index 0c0a1c969..0076c91b4 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -257,4 +257,32 @@ extern Fifo * 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_