From 02660c763ae1388e02fd7606b795269e522d8e10 Mon Sep 17 00:00:00 2001 From: Raphael Coeffic Date: Sat, 19 Oct 2019 23:08:51 +0200 Subject: [PATCH] mixer scheduler --- radio/src/CMakeLists.txt | 1 + radio/src/gui/gui_common.h | 3 +- radio/src/mixer_scheduler.cpp | 79 +++++++++++ radio/src/mixer_scheduler.h | 84 +++++++++++ radio/src/opentx.h | 1 - radio/src/pulses/dsm2.cpp | 6 +- radio/src/pulses/multi.cpp | 2 - radio/src/pulses/pulses.cpp | 109 ++++++++++---- radio/src/pulses/pulses.h | 9 +- radio/src/pulses/pxx.h | 12 +- radio/src/pulses/sbus.cpp | 2 - radio/src/rtos.h | 32 ++++- .../common/arm/stm32/heartbeat_driver.cpp | 3 + .../arm/stm32/intmodule_serial_driver.cpp | 37 ----- .../arm/stm32/mixer_scheduler_driver.cpp | 81 +++++++++++ radio/src/targets/horus/CMakeLists.txt | 1 + radio/src/targets/horus/board.cpp | 1 + radio/src/targets/horus/board.h | 5 +- radio/src/targets/horus/extmodule_driver.cpp | 44 ++++-- radio/src/targets/horus/hal.h | 7 + radio/src/targets/taranis/CMakeLists.txt | 1 + radio/src/targets/taranis/board.cpp | 3 +- radio/src/targets/taranis/board.h | 2 +- .../src/targets/taranis/extmodule_driver.cpp | 39 +++-- radio/src/targets/taranis/hal.h | 7 + radio/src/tasks.cpp | 89 ++++++------ radio/src/telemetry/crossfire.cpp | 20 +++ radio/src/telemetry/crossfire.h | 6 +- radio/src/telemetry/multi.cpp | 133 ++---------------- radio/src/telemetry/multi.h | 22 --- radio/src/telemetry/telemetry.cpp | 86 +++++++++++ radio/src/telemetry/telemetry.h | 31 ++++ 32 files changed, 644 insertions(+), 314 deletions(-) create mode 100644 radio/src/mixer_scheduler.cpp create mode 100644 radio/src/mixer_scheduler.h create mode 100644 radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp diff --git a/radio/src/CMakeLists.txt b/radio/src/CMakeLists.txt index 47a1fed2a..39ea7e52e 100644 --- a/radio/src/CMakeLists.txt +++ b/radio/src/CMakeLists.txt @@ -378,6 +378,7 @@ set(SRC strhelpers.cpp switches.cpp mixer.cpp + mixer_scheduler.cpp stamp.cpp timers.cpp trainer.cpp diff --git a/radio/src/gui/gui_common.h b/radio/src/gui/gui_common.h index dd473eee8..3894bf633 100644 --- a/radio/src/gui/gui_common.h +++ b/radio/src/gui/gui_common.h @@ -185,6 +185,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)) @@ -276,7 +277,7 @@ inline uint8_t MULTIMODULE_HASOPTIONS(uint8_t moduleIdx) #define MULTIMODULE_MODULE_ROWS(moduleIdx) (MULTIMODULE_PROTOCOL_KNOWN(moduleIdx) && !IS_RX_MULTI(moduleIdx)) ? (uint8_t) 0 : HIDDEN_ROW, (MULTIMODULE_PROTOCOL_KNOWN(moduleIdx) && !IS_RX_MULTI(moduleIdx)) ? (uint8_t) 0 : HIDDEN_ROW, MULTI_DISABLE_CHAN_MAP_ROW(moduleIdx), // AUTOBIND, DISABLE TELEM, DISABLE CN.MAP #define MULTIMODULE_TYPE_ROW(moduleIdx) isModuleMultimodule(moduleIdx) ? MULTIMODULE_RFPROTO_COLUMNS(moduleIdx) : HIDDEN_ROW, -#define MULTIMODULE_STATUS_ROWS(moduleIdx) isModuleMultimodule(moduleIdx) ? TITLE_ROW : HIDDEN_ROW, (isModuleMultimodule(moduleIdx) && getMultiSyncStatus(moduleIdx).isValid()) ? TITLE_ROW : HIDDEN_ROW, +#define MULTIMODULE_STATUS_ROWS(moduleIdx) isModuleMultimodule(moduleIdx) ? TITLE_ROW : HIDDEN_ROW, (isModuleMultimodule(moduleIdx) && getModuleSyncStatus(moduleIdx).isValid()) ? TITLE_ROW : HIDDEN_ROW, #define MULTIMODULE_MODE_ROWS(moduleIdx) (g_model.moduleData[moduleIdx].multi.customProto) ? (uint8_t) 3 : MULTIMODULE_HAS_SUBTYPE(moduleIdx) ? (uint8_t)2 : (uint8_t)1 #define MULTIMODULE_TYPE_ROWS(moduleIdx) isModuleMultimodule(moduleIdx) ? (uint8_t) 0 : HIDDEN_ROW, #define MULTIMODULE_SUBTYPE_ROWS(moduleIdx) isModuleMultimodule(moduleIdx) ? MULTIMODULE_RFPROTO_COLUMNS(moduleIdx) : HIDDEN_ROW, diff --git a/radio/src/mixer_scheduler.cpp b/radio/src/mixer_scheduler.cpp new file mode 100644 index 000000000..b29c821c8 --- /dev/null +++ b/radio/src/mixer_scheduler.cpp @@ -0,0 +1,79 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * 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 "opentx.h" +#include "mixer_scheduler.h" + +#if !defined(SIMU) + +// Global trigger flag +RTOS_FLAG_HANDLE mixerFlag; + +// Mixer schedule +struct MixerSchedule { + + // period in us + volatile uint16_t period; +}; + +static MixerSchedule mixerSchedules[NUM_MODULES]; + +uint16_t getMixerSchedulerPeriod() +{ + if (mixerSchedules[INTERNAL_MODULE].period) { + return mixerSchedules[INTERNAL_MODULE].period; + } + else if (mixerSchedules[EXTERNAL_MODULE].period) { + return mixerSchedules[EXTERNAL_MODULE].period; + } + + return MIXER_SCHEDULER_DEFAULT_PERIOD_US; +} + +void mixerSchedulerInit() +{ + RTOS_CREATE_FLAG(mixerFlag); + memset(mixerSchedules, 0, sizeof(mixerSchedules)); +} + +void mixerSchedulerSetPeriod(uint8_t moduleIdx, uint16_t periodUs) +{ + if ((periodUs > 0) && (periodUs < MIN_REFRESH_RATE)) { + periodUs = MIN_REFRESH_RATE; + } + else if ((periodUs > 0) && (periodUs > MAX_REFRESH_RATE)) { + periodUs = MAX_REFRESH_RATE; + } + + mixerSchedules[moduleIdx].period = periodUs; +} + +bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs) +{ + RTOS_CLEAR_FLAG(mixerFlag); + return RTOS_WAIT_FLAG(mixerFlag, timeoutMs); +} + +void mixerSchedulerISRTrigger() +{ + RTOS_ISR_SET_FLAG(mixerFlag); +} + +#endif diff --git a/radio/src/mixer_scheduler.h b/radio/src/mixer_scheduler.h new file mode 100644 index 000000000..ccded9a5e --- /dev/null +++ b/radio/src/mixer_scheduler.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * 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. + */ + +#ifndef _MIXER_SCHEDULER_H_ +#define _MIXER_SCHEDULER_H_ + +#define MIXER_SCHEDULER_DEFAULT_PERIOD_US 4000u // 4ms + +#define MIN_REFRESH_RATE 4000 /* us */ +#define MAX_REFRESH_RATE 25000 /* us */ + +#if !defined(SIMU) + +// Call once to initialize the mixer scheduler +void mixerSchedulerInit(); + +// Configure and start the scheduler timer +void mixerSchedulerStart(); + +// Stop the scheduler timer +void mixerSchedulerStop(); + +// Set the timer counter to 0 +void mixerSchedulerResetTimer(); + +// Set the scheduling period for a given module +void mixerSchedulerSetPeriod(uint8_t moduleIdx, uint16_t periodUs); + +// Wait for the scheduler timer to trigger +// returns true if timeout, false otherwise +bool mixerSchedulerWaitForTrigger(uint8_t timeoutMs); + +// Enable the timer trigger +void mixerSchedulerEnableTrigger(); + +// Disable the timer trigger +void mixerSchedulerDisableTrigger(); + +// Fetch the current scheduling period +uint16_t getMixerSchedulerPeriod(); + +// Trigger mixer from an ISR +void mixerSchedulerISRTrigger(); + +#else + +#define mixerSchedulerInit() +#define mixerSchedulerStart() +#define mixerSchedulerStop() +#define mixerSchedulerResetTimer() +#define mixerSchedulerSetPeriod(m,p) + +static inline bool mixerSchedulerWaitForTrigger(uint8_t timeout) +{ + simuSleep(timeout); + return false; +} + +#define mixerSchedulerEnableTrigger() +#define mixerSchedulerDisableTrigger() + +#define getMixerSchedulerPeriod() (MIXER_SCHEDULER_DEFAULT_PERIOD_US) +#define mixerSchedulerISRTrigger() + +#endif + +#endif diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 52a51719b..394a0734f 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -483,7 +483,6 @@ extern uint32_t nextMixerTime[NUM_MODULES]; void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms); void evalMixes(uint8_t tick10ms); void doMixerCalculations(); -void scheduleNextMixerCalculation(uint8_t module, uint32_t period_ms); void checkTrims(); extern uint8_t currentBacklightBright; diff --git a/radio/src/pulses/dsm2.cpp b/radio/src/pulses/dsm2.cpp index 3612f60db..34a10b3d6 100644 --- a/radio/src/pulses/dsm2.cpp +++ b/radio/src/pulses/dsm2.cpp @@ -76,7 +76,6 @@ void _send_1(uint8_t v) *extmodulePulsesData.dsm2.ptr++ = v - 1; extmodulePulsesData.dsm2.index += 1; - extmodulePulsesData.dsm2.rest -= v; } void sendByteDsm2(uint8_t b) // max 10 changes 0 10 10 10 10 1 @@ -101,9 +100,9 @@ void sendByteDsm2(uint8_t b) // max 10 changes 0 10 10 10 10 1 void putDsm2Flush() { if (extmodulePulsesData.dsm2.index & 1) - *extmodulePulsesData.dsm2.ptr++ = extmodulePulsesData.dsm2.rest; + *extmodulePulsesData.dsm2.ptr++ = 60000; else - *(extmodulePulsesData.dsm2.ptr - 1) = extmodulePulsesData.dsm2.rest; + *(extmodulePulsesData.dsm2.ptr - 1) = 60000; } #endif @@ -119,7 +118,6 @@ void setupPulsesDSM2() extmodulePulsesData.dsm2.serialBitCount = 0 ; #else extmodulePulsesData.dsm2.index = 0; - extmodulePulsesData.dsm2.rest = DSM2_PERIOD * 2000; #endif extmodulePulsesData.dsm2.ptr = extmodulePulsesData.dsm2.pulses; diff --git a/radio/src/pulses/multi.cpp b/radio/src/pulses/multi.cpp index dcc7c7f9c..4f61caf72 100644 --- a/radio/src/pulses/multi.cpp +++ b/radio/src/pulses/multi.cpp @@ -209,10 +209,8 @@ void setupPulsesMultiExternalModule() extmodulePulsesData.dsm2.serialByte = 0 ; extmodulePulsesData.dsm2.serialBitCount = 0 ; #else - extmodulePulsesData.dsm2.rest = getMultiSyncStatus(EXTERNAL_MODULE).getAdjustedRefreshRate(); extmodulePulsesData.dsm2.index = 0; #endif - extmodulePulsesData.dsm2.ptr = extmodulePulsesData.dsm2.pulses; setupPulsesMulti(EXTERNAL_MODULE); diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp index d6833bfe9..1681157bc 100755 --- a/radio/src/pulses/pulses.cpp +++ b/radio/src/pulses/pulses.cpp @@ -21,6 +21,7 @@ #include "opentx.h" #include "io/frsky_pxx2.h" #include "pulses/pxx2.h" +#include "mixer_scheduler.h" uint8_t s_pulses_paused = 0; ModuleState moduleState[NUM_MODULES]; @@ -62,7 +63,7 @@ void getModuleSyncStatusString(uint8_t moduleIdx, char * statusText) *statusText = 0; #if defined(MULTIMODULE) if (isModuleMultimodule(moduleIdx)) { - getMultiSyncStatus(moduleIdx).getRefreshString(statusText); + getModuleSyncStatus(moduleIdx).getRefreshString(statusText); } #endif #if defined(AFHDS3) @@ -225,12 +226,14 @@ void enablePulsesExternalModule(uint8_t protocol) #if defined(PXX1) case PROTOCOL_CHANNELS_PXX1_PULSES: extmodulePxx1PulsesStart(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, PXX_PULSES_PERIOD); break; #endif #if defined(PXX1) && defined(HARDWARE_EXTERNAL_MODULE_SIZE_SML) case PROTOCOL_CHANNELS_PXX1_SERIAL: extmodulePxx1SerialStart(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, EXTMODULE_PXX1_SERIAL_PERIOD); break; #endif @@ -238,59 +241,69 @@ void enablePulsesExternalModule(uint8_t protocol) case PROTOCOL_CHANNELS_DSM2_LP45: case PROTOCOL_CHANNELS_DSM2_DSM2: case PROTOCOL_CHANNELS_DSM2_DSMX: - extmoduleSerialStart(DSM2_BAUDRATE, DSM2_PERIOD * 2000, false); + extmoduleSerialStart(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, DSM2_PERIOD); break; #endif #if defined(CROSSFIRE) case PROTOCOL_CHANNELS_CROSSFIRE: EXTERNAL_MODULE_ON(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, CROSSFIRE_PERIOD); break; #endif #if defined(GHOST) case PROTOCOL_CHANNELS_GHOST: EXTERNAL_MODULE_ON(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, GHOST_PERIOD); break; #endif #if defined(PXX2) && defined(EXTMODULE_USART) case PROTOCOL_CHANNELS_PXX2_HIGHSPEED: extmoduleInvertedSerialStart(PXX2_HIGHSPEED_BAUDRATE); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, PXX2_PERIOD); break; case PROTOCOL_CHANNELS_PXX2_LOWSPEED: extmoduleInvertedSerialStart(PXX2_LOWSPEED_BAUDRATE); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, PXX2_PERIOD); break; #endif #if defined(MULTIMODULE) case PROTOCOL_CHANNELS_MULTIMODULE: - extmoduleSerialStart(MULTIMODULE_BAUDRATE, MULTIMODULE_PERIOD * 2000, true); + extmoduleSerialStart(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, MULTIMODULE_PERIOD); break; #endif #if defined(SBUS) case PROTOCOL_CHANNELS_SBUS: - extmoduleSerialStart(SBUS_BAUDRATE, SBUS_PERIOD_HALF_US, false); + extmoduleSerialStart(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, SBUS_PERIOD); break; #endif #if defined(PPM) case PROTOCOL_CHANNELS_PPM: extmodulePpmStart(); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, PPM_PERIOD(EXTERNAL_MODULE)); break; #endif #if defined(AFHDS3) case PROTOCOL_CHANNELS_AFHDS3: extmodulePulsesData.afhds3.init(EXTERNAL_MODULE); - extmoduleSerialStart(AFHDS3_BAUDRATE, AFHDS3_COMMAND_TIMEOUT * 2000, false); + extmoduleSerialStart(/*AFHDS3_BAUDRATE, AFHDS3_COMMAND_TIMEOUT * 2000, false*/); + mixerSchedulerSetPeriod(EXTERNAL_MODULE, AFHDS3_COMMAND_TIMEOUT * 1000 /* us */); break; #endif - default: + // external module stopped, set period to 50ms (necessary for USB Joystick, for instance) + mixerSchedulerSetPeriod(EXTERNAL_MODULE, 50000/*us*/); break; } } @@ -301,14 +314,12 @@ bool setupPulsesExternalModule(uint8_t protocol) #if defined(PXX1) case PROTOCOL_CHANNELS_PXX1_PULSES: extmodulePulsesData.pxx.setupFrame(EXTERNAL_MODULE); - scheduleNextMixerCalculation(EXTERNAL_MODULE, PXX_PULSES_PERIOD); return true; #endif #if defined(PXX1) && defined(HARDWARE_EXTERNAL_MODULE_SIZE_SML) case PROTOCOL_CHANNELS_PXX1_SERIAL: extmodulePulsesData.pxx_uart.setupFrame(EXTERNAL_MODULE); - scheduleNextMixerCalculation(EXTERNAL_MODULE, EXTMODULE_PXX1_SERIAL_PERIOD); return true; #endif @@ -316,14 +327,12 @@ bool setupPulsesExternalModule(uint8_t protocol) case PROTOCOL_CHANNELS_PXX2_HIGHSPEED: case PROTOCOL_CHANNELS_PXX2_LOWSPEED: extmodulePulsesData.pxx2.setupFrame(EXTERNAL_MODULE); - scheduleNextMixerCalculation(EXTERNAL_MODULE, PXX2_PERIOD); return true; #endif #if defined(SBUS) case PROTOCOL_CHANNELS_SBUS: setupPulsesSbus(); - scheduleNextMixerCalculation(EXTERNAL_MODULE, SBUS_PERIOD); return true; #endif @@ -332,47 +341,61 @@ bool setupPulsesExternalModule(uint8_t protocol) case PROTOCOL_CHANNELS_DSM2_DSM2: case PROTOCOL_CHANNELS_DSM2_DSMX: setupPulsesDSM2(); - scheduleNextMixerCalculation(EXTERNAL_MODULE, DSM2_PERIOD); return true; #endif #if defined(CROSSFIRE) case PROTOCOL_CHANNELS_CROSSFIRE: + { + ModuleSyncStatus& status = getModuleSyncStatus(EXTERNAL_MODULE); + if (status.isValid()) + mixerSchedulerSetPeriod(EXTERNAL_MODULE, status.getAdjustedRefreshRate()); + else + mixerSchedulerSetPeriod(EXTERNAL_MODULE, CROSSFIRE_PERIOD); setupPulsesCrossfire(); - scheduleNextMixerCalculation(EXTERNAL_MODULE, CROSSFIRE_PERIOD); return true; + } #endif #if defined(GHOST) case PROTOCOL_CHANNELS_GHOST: + { + ModuleSyncStatus& status = getModuleSyncStatus(EXTERNAL_MODULE); + if (status.isValid()) + mixerSchedulerSetPeriod(EXTERNAL_MODULE, status.getAdjustedRefreshRate()); + else + mixerSchedulerSetPeriod(EXTERNAL_MODULE, GHOST_PERIOD); setupPulsesGhost(); - scheduleNextMixerCalculation(EXTERNAL_MODULE, GHOST_PERIOD); return true; + } #endif #if defined(MULTIMODULE) case PROTOCOL_CHANNELS_MULTIMODULE: + { + ModuleSyncStatus& status = getModuleSyncStatus(EXTERNAL_MODULE); + if (status.isValid()) + mixerSchedulerSetPeriod(EXTERNAL_MODULE, status.getAdjustedRefreshRate()); + else + mixerSchedulerSetPeriod(EXTERNAL_MODULE, MULTIMODULE_PERIOD); setupPulsesMultiExternalModule(); - scheduleNextMixerCalculation(EXTERNAL_MODULE, MULTIMODULE_PERIOD); return true; + } #endif #if defined(PPM) case PROTOCOL_CHANNELS_PPM: setupPulsesPPMExternalModule(); - scheduleNextMixerCalculation(EXTERNAL_MODULE, PPM_PERIOD(EXTERNAL_MODULE)); return true; #endif #if defined(AFHDS3) case PROTOCOL_CHANNELS_AFHDS3: extmodulePulsesData.afhds3.setupFrame(); - scheduleNextMixerCalculation(EXTERNAL_MODULE, AFHDS3_COMMAND_TIMEOUT); return true; #endif default: - scheduleNextMixerCalculation(EXTERNAL_MODULE, 50/*ms*/); return false; } } @@ -386,12 +409,24 @@ static void enablePulsesInternalModule(uint8_t protocol) #if defined(PXX1) && !defined(INTMODULE_USART) case PROTOCOL_CHANNELS_PXX1_PULSES: intmodulePxx1PulsesStart(); +#if defined(INTMODULE_HEARTBEAT) + // use backup trigger (1 ms later) + mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD + 1000/*us*/); +#else + mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD); +#endif break; #endif #if defined(PXX1) && defined(INTMODULE_USART) case PROTOCOL_CHANNELS_PXX1_SERIAL: intmodulePxx1SerialStart(); +#if defined(INTMODULE_HEARTBEAT) + // use backup trigger (1 ms later) + mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD + 1000/*us*/); +#else + mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD); +#endif break; #endif @@ -399,6 +434,13 @@ static void enablePulsesInternalModule(uint8_t protocol) case PROTOCOL_CHANNELS_PXX2_HIGHSPEED: intmoduleSerialStart(PXX2_HIGHSPEED_BAUDRATE, true, USART_Parity_No, USART_StopBits_1, USART_WordLength_8b); resetAccessAuthenticationCount(); + +#if defined(INTMODULE_HEARTBEAT) + // use backup trigger (1 ms later) + mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_PERIOD + 1000/*us*/); +#else + mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_PERIOD); +#endif break; #endif @@ -406,17 +448,20 @@ static void enablePulsesInternalModule(uint8_t protocol) case PROTOCOL_CHANNELS_MULTIMODULE: intmodulePulsesData.multi.initFrame(); intmoduleSerialStart(MULTIMODULE_BAUDRATE, true, USART_Parity_Even, USART_StopBits_2, USART_WordLength_9b); - intmoduleTimerStart(MULTIMODULE_PERIOD); + mixerSchedulerSetPeriod(INTERNAL_MODULE, MULTIMODULE_PERIOD); break; #endif #if defined(INTERNAL_MODULE_PPM) case PROTOCOL_CHANNELS_PPM: intmodulePpmStart(); + mixerSchedulerSetPeriod(INTERNAL_MODULE, PPM_PERIOD(INTERNAL_MODULE)); break; #endif default: + // internal module stopped, set internal period to 0 and start the scheduler + mixerSchedulerSetPeriod(INTERNAL_MODULE, 0); break; } } @@ -427,18 +472,18 @@ bool setupPulsesInternalModule(uint8_t protocol) #if defined(HARDWARE_INTERNAL_MODULE) && defined(PXX1) && !defined(INTMODULE_USART) case PROTOCOL_CHANNELS_PXX1_PULSES: intmodulePulsesData.pxx.setupFrame(INTERNAL_MODULE); - scheduleNextMixerCalculation(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD); +#if defined(INTMODULE_HEARTBEAT) + mixerSchedulerResetTimer(); + mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD + 1000 /* backup */); +#else + mixerSchedulerSetPeriod(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD); +#endif return true; #endif #if defined(PXX1) && defined(INTMODULE_USART) case PROTOCOL_CHANNELS_PXX1_SERIAL: intmodulePulsesData.pxx_uart.setupFrame(INTERNAL_MODULE); -#if defined(INTMODULE_HEARTBEAT) - scheduleNextMixerCalculation(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD + 1 /* backup */); -#else - scheduleNextMixerCalculation(INTERNAL_MODULE, INTMODULE_PXX1_SERIAL_PERIOD); -#endif return true; #endif @@ -447,15 +492,16 @@ bool setupPulsesInternalModule(uint8_t protocol) { bool result = intmodulePulsesData.pxx2.setupFrame(INTERNAL_MODULE); if (moduleState[INTERNAL_MODULE].mode == MODULE_MODE_SPECTRUM_ANALYSER || moduleState[INTERNAL_MODULE].mode == MODULE_MODE_POWER_METER) { - scheduleNextMixerCalculation(INTERNAL_MODULE, PXX2_TOOLS_PERIOD); + mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_TOOLS_PERIOD); } else { #if defined(INTMODULE_HEARTBEAT) - scheduleNextMixerCalculation(INTERNAL_MODULE, PXX2_PERIOD + 1 /* backup */); + mixerSchedulerResetTimer(); + mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_PERIOD + 1000 /* backup */); #else - scheduleNextMixerCalculation(INTERNAL_MODULE, PXX2_PERIOD); + mixerSchedulerSetPeriod(INTERNAL_MODULE, PXX2_PERIOD); #endif - } + } return result; } #endif @@ -463,19 +509,20 @@ bool setupPulsesInternalModule(uint8_t protocol) #if defined(PCBTARANIS) && defined(INTERNAL_MODULE_PPM) case PROTOCOL_CHANNELS_PPM: setupPulsesPPMInternalModule(); - scheduleNextMixerCalculation(INTERNAL_MODULE, PPM_PERIOD(INTERNAL_MODULE)); + // probably useless, as the interval did not change since "enable" function + mixerSchedulerSetPeriod(INTERNAL_MODULE, PPM_PERIOD(INTERNAL_MODULE)); return true; #endif #if defined(INTERNAL_MODULE_MULTI) case PROTOCOL_CHANNELS_MULTIMODULE: setupPulsesMultiInternalModule(); - scheduleNextMixerCalculation(INTERNAL_MODULE, MULTIMODULE_PERIOD); + mixerSchedulerSetPeriod(INTERNAL_MODULE, MULTIMODULE_PERIOD); return true; #endif default: - scheduleNextMixerCalculation(INTERNAL_MODULE, 10 /*ms*/); // used for USB sim for example + mixerSchedulerSetPeriod(INTERNAL_MODULE, 10000 /*us*/); // used for USB sim for example return false; } } diff --git a/radio/src/pulses/pulses.h b/radio/src/pulses/pulses.h index 84ecee015..4a1fafe65 100644 --- a/radio/src/pulses/pulses.h +++ b/radio/src/pulses/pulses.h @@ -216,21 +216,20 @@ typedef Dsm2SerialPulsesData Dsm2PulsesData; PACK(struct Dsm2TimerPulsesData { pulse_duration_t pulses[MAX_PULSES_TRANSITIONS]; pulse_duration_t * ptr; - uint16_t rest; uint8_t index; }); typedef Dsm2TimerPulsesData Dsm2PulsesData; #endif #define PPM_PERIOD_HALF_US(module) ((g_model.moduleData[module].ppm.frameLength * 5 + 225) * 200) /*half us*/ -#define PPM_PERIOD(module) (PPM_PERIOD_HALF_US(module) / 2000) /*ms*/ +#define PPM_PERIOD(module) (PPM_PERIOD_HALF_US(module) / 2) /*us*/ #define DSM2_BAUDRATE 125000 -#define DSM2_PERIOD 22 /*ms*/ +#define DSM2_PERIOD 22000 /*us*/ #define SBUS_BAUDRATE 100000 #define SBUS_PERIOD_HALF_US ((g_model.moduleData[EXTERNAL_MODULE].sbus.refreshRate * 5 + 225) * 200) /*half us*/ -#define SBUS_PERIOD (SBUS_PERIOD_HALF_US / 2000) /*ms*/ +#define SBUS_PERIOD (SBUS_PERIOD_HALF_US / 2) /*us*/ #define MULTIMODULE_BAUDRATE 100000 -#define MULTIMODULE_PERIOD 7 /*ms*/ +#define MULTIMODULE_PERIOD 7000 /*us*/ #define CROSSFIRE_FRAME_MAXLEN 64 PACK(struct CrossfirePulsesData { diff --git a/radio/src/pulses/pxx.h b/radio/src/pulses/pxx.h index 5d0bfaf05..ae84d77b2 100644 --- a/radio/src/pulses/pxx.h +++ b/radio/src/pulses/pxx.h @@ -29,20 +29,20 @@ #define PXX2_LOWSPEED_BAUDRATE 230400 #define PXX2_HIGHSPEED_BAUDRATE 450000 -#define PXX2_PERIOD 4 // 4ms -#define PXX2_TOOLS_PERIOD 1 // 1ms +#define PXX2_PERIOD 4000/*us*/ +#define PXX2_TOOLS_PERIOD 1000/*us*/ #define PXX2_FRAME_MAXLENGTH 64 -#define PXX_PULSES_PERIOD 9/*ms*/ -#define EXTMODULE_PXX1_SERIAL_PERIOD 4/*ms*/ +#define PXX_PULSES_PERIOD 9000/*us*/ +#define EXTMODULE_PXX1_SERIAL_PERIOD 4000/*us*/ #define EXTMODULE_PXX1_SERIAL_BAUDRATE 420000 #if defined(PXX_FREQUENCY_HIGH) #define INTMODULE_PXX1_SERIAL_BAUDRATE 450000 - #define INTMODULE_PXX1_SERIAL_PERIOD 4/*ms*/ + #define INTMODULE_PXX1_SERIAL_PERIOD 4000/*us*/ #else #define INTMODULE_PXX1_SERIAL_BAUDRATE 115200 - #define INTMODULE_PXX1_SERIAL_PERIOD 9/*ms*/ + #define INTMODULE_PXX1_SERIAL_PERIOD 9000/*us*/ #endif // Used by the Sky9x family boards diff --git a/radio/src/pulses/sbus.cpp b/radio/src/pulses/sbus.cpp index a881730fc..e44abd642 100644 --- a/radio/src/pulses/sbus.cpp +++ b/radio/src/pulses/sbus.cpp @@ -55,7 +55,6 @@ static void _send_level(uint8_t v) *extmodulePulsesData.dsm2.ptr++ = v - 1; extmodulePulsesData.dsm2.index+=1; - extmodulePulsesData.dsm2.rest -=v; } void sendByteSbus(uint8_t b) // max 11 changes 0 10 10 10 10 P 1 @@ -113,7 +112,6 @@ void setupPulsesSbus() extmodulePulsesData.dsm2.serialByte = 0; extmodulePulsesData.dsm2.serialBitCount = 0; #else - extmodulePulsesData.dsm2.rest = SBUS_PERIOD_HALF_US; extmodulePulsesData.dsm2.index = 0; #endif diff --git a/radio/src/rtos.h b/radio/src/rtos.h index 6e42ebf30..a5dbd11b9 100644 --- a/radio/src/rtos.h +++ b/radio/src/rtos.h @@ -36,7 +36,9 @@ extern "C++" { typedef pthread_t RTOS_TASK_HANDLE; typedef pthread_mutex_t RTOS_MUTEX_HANDLE; + typedef uint32_t RTOS_FLAG_HANDLE; + typedef sem_t * RTOS_EVENT_HANDLE; extern uint64_t simuTimerMicros(void); @@ -76,16 +78,26 @@ extern "C++" { pthread_mutex_unlock(&mutex); } - static inline void RTOS_CREATE_FLAG(uint32_t &flag) + static inline void RTOS_CREATE_FLAG(RTOS_FLAG_HANDLE flag) { - flag = 0; // TODO: real flags (use semaphores?) } - static inline void RTOS_SET_FLAG(uint32_t &flag) + static inline void RTOS_SET_FLAG(RTOS_FLAG_HANDLE flag) { - flag = 1; } + static inline void RTOS_CLEAR_FLAG(RTOS_FLAG_HANDLE flag) + { + } + + static inline bool RTOS_WAIT_FLAG(RTOS_FLAG_HANDLE flag, uint32_t timeout) + { + simuSleep(timeout); + return false; + } + + #define RTOS_ISR_SET_FLAG RTOS_SET_FLAG + template class FakeTaskStack { @@ -146,6 +158,7 @@ template { return (uint32_t)(simuTimerMicros() / 1000); } + #elif defined(RTOS_COOS) #ifdef __cplusplus extern "C" { @@ -230,7 +243,18 @@ 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) (CoWaitForSingleFlag(flag,timeout) == E_TIMEOUT) + static inline void RTOS_ISR_SET_FLAG(RTOS_FLAG_HANDLE flag) + { + CoEnterISR(); + CoSchedLock(); + isr_SetFlag(flag); + CoSchedUnlock(); + CoExitISR(); + } + #ifdef __cplusplus template class TaskStack diff --git a/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp b/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp index 25544c33a..269b40854 100644 --- a/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp +++ b/radio/src/targets/common/arm/stm32/heartbeat_driver.cpp @@ -19,6 +19,7 @@ */ #include "opentx.h" +#include "mixer_scheduler.h" #if defined(INTMODULE_HEARTBEAT_GPIO) volatile HeartbeatCapture heartbeatCapture; @@ -77,6 +78,8 @@ void check_intmodule_heartbeat() heartbeatCapture.count++; #endif EXTI_ClearITPendingBit(INTMODULE_HEARTBEAT_EXTI_LINE); + + mixerSchedulerISRTrigger(); } } #endif diff --git a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp index 56b71d97c..794c49765 100755 --- a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp @@ -45,12 +45,6 @@ void intmoduleStop() USART_DeInit(INTMODULE_USART); GPIO_ResetBits(INTMODULE_GPIO, INTMODULE_TX_GPIO_PIN | INTMODULE_RX_GPIO_PIN); - -#if defined(INTERNAL_MODULE_MULTI) - // stop pulses timer - INTMODULE_TIMER->DIER &= ~TIM_DIER_CC2IE; - INTMODULE_TIMER->CR1 &= ~TIM_CR1_CEN; -#endif } void intmodulePxx1SerialStart() @@ -103,28 +97,6 @@ void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable, uint16_t parity, } } -#if defined(INTERNAL_MODULE_MULTI) -void intmoduleTimerStart(uint32_t periodMs) -{ - // Timer - INTMODULE_TIMER->CR1 &= ~TIM_CR1_CEN; - INTMODULE_TIMER->PSC = INTMODULE_TIMER_FREQ / 2000000 - 1; // 0.5uS (2Mhz) - INTMODULE_TIMER->ARR = periodMs * 2000; - INTMODULE_TIMER->CCR2 = (periodMs - 1) * 2000; - INTMODULE_TIMER->CCER = TIM_CCER_CC3E; - INTMODULE_TIMER->CCMR2 = 0; - INTMODULE_TIMER->EGR = 1; // Restart - - INTMODULE_TIMER->CCMR2 = TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_0; // Toggle CC1 o/p - INTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag - INTMODULE_TIMER->DIER |= TIM_DIER_CC2IE; // Enable this interrupt - INTMODULE_TIMER->CR1 |= TIM_CR1_CEN; - - NVIC_EnableIRQ(INTMODULE_TIMER_IRQn); - NVIC_SetPriority(INTMODULE_TIMER_IRQn, 7); -} -#endif - #define USART_FLAG_ERRORS (USART_FLAG_ORE | USART_FLAG_NE | USART_FLAG_FE | USART_FLAG_PE) extern "C" void INTMODULE_USART_IRQHandler(void) { @@ -217,12 +189,3 @@ void intmoduleSendNextFrame() #endif } } - -#if defined(INTERNAL_MODULE_MULTI) -extern "C" void INTMODULE_TIMER_IRQHandler() -{ - INTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // clear flag - setupPulsesInternalModule(); - intmoduleSendNextFrame(); -} -#endif diff --git a/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp b/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp new file mode 100644 index 000000000..7f7b23a1c --- /dev/null +++ b/radio/src/targets/common/arm/stm32/mixer_scheduler_driver.cpp @@ -0,0 +1,81 @@ +/* + * Copyright (C) OpenTX + * + * Based on code named + * 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 "opentx.h" +#include "mixer_scheduler.h" + +#if !defined(SIMU) + +// Start scheduler with default period +void mixerSchedulerStart() +{ + MIXER_SCHEDULER_TIMER->CR1 &= ~TIM_CR1_CEN; + + MIXER_SCHEDULER_TIMER->CR1 = TIM_CR1_URS; // do not generate interrupt on soft update + MIXER_SCHEDULER_TIMER->PSC = MIXER_SCHEDULER_TIMER_FREQ / 2000000 - 1; // 0.5uS (2Mhz) + MIXER_SCHEDULER_TIMER->CCER = 0; + MIXER_SCHEDULER_TIMER->CCMR1 = 0; + MIXER_SCHEDULER_TIMER->ARR = 2 * getMixerSchedulerPeriod() - 1; + MIXER_SCHEDULER_TIMER->EGR = TIM_EGR_UG; // reset timer + + NVIC_EnableIRQ(MIXER_SCHEDULER_TIMER_IRQn); + NVIC_SetPriority(MIXER_SCHEDULER_TIMER_IRQn, 8); + + MIXER_SCHEDULER_TIMER->SR &= TIM_SR_UIF; // clear interrupt flag + MIXER_SCHEDULER_TIMER->DIER |= TIM_DIER_UIE; // enable interrupt + MIXER_SCHEDULER_TIMER->CR1 |= TIM_CR1_CEN; +} + +void mixerSchedulerStop() +{ + MIXER_SCHEDULER_TIMER->CR1 &= ~TIM_CR1_CEN; + NVIC_DisableIRQ(MIXER_SCHEDULER_TIMER_IRQn); +} + +void mixerSchedulerResetTimer() +{ + mixerSchedulerDisableTrigger(); + MIXER_SCHEDULER_TIMER->CNT = 0; + mixerSchedulerEnableTrigger(); +} + +void mixerSchedulerEnableTrigger() +{ + MIXER_SCHEDULER_TIMER->DIER |= TIM_DIER_UIE; // enable interrupt +} + +void mixerSchedulerDisableTrigger() +{ + MIXER_SCHEDULER_TIMER->DIER &= ~TIM_DIER_UIE; // disable interrupt +} + +extern "C" void MIXER_SCHEDULER_TIMER_IRQHandler(void) +{ + MIXER_SCHEDULER_TIMER->SR &= ~TIM_SR_UIF; // clear flag + mixerSchedulerDisableTrigger(); + + // set next period + MIXER_SCHEDULER_TIMER->ARR = 2 * getMixerSchedulerPeriod() - 1; + + // trigger mixer start + mixerSchedulerISRTrigger(); +} + +#endif diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index fbf963b71..460a24701 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -240,6 +240,7 @@ set(TARGET_SRC led_driver.cpp extmodule_driver.cpp trainer_driver.cpp + ../common/arm/stm32/mixer_scheduler_driver.cpp ../common/arm/stm32/heartbeat_driver.cpp ../common/arm/stm32/timers_driver.cpp ../common/arm/stm32/intmodule_serial_driver.cpp diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index cb379e6fe..0f3f9df4a 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -137,6 +137,7 @@ void boardInit() INTMODULE_RCC_APB1Periph | EXTMODULE_RCC_APB1Periph | I2C_RCC_APB1Periph | + MIXER_SCHEDULER_TIMER_RCC_APB1Periph | GPS_RCC_APB1Periph | BACKLIGHT_RCC_APB1Periph, ENABLE); diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h index 38daec33c..f1c47ab9f 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -180,14 +180,11 @@ void init_intmodule_heartbeat(); void check_intmodule_heartbeat(); void intmoduleSerialStart(uint32_t baudrate, uint8_t rxEnable, uint16_t parity, uint16_t stopBits, uint16_t wordLength); -#if defined(INTERNAL_MODULE_MULTI) -void intmoduleTimerStart(uint32_t periodMs); -#endif void intmoduleSendByte(uint8_t byte); void intmoduleSendBuffer(const uint8_t * data, uint8_t size); void intmoduleSendNextFrame(); -void extmoduleSerialStart(uint32_t baudrate, uint32_t period_half_us, bool inverted); +void extmoduleSerialStart(); void extmoduleInvertedSerialStart(uint32_t baudrate); void extmoduleSendBuffer(const uint8_t * data, uint8_t size); void extmoduleSendNextFrame(); diff --git a/radio/src/targets/horus/extmodule_driver.cpp b/radio/src/targets/horus/extmodule_driver.cpp index 47a40ae02..e73d5c3d3 100644 --- a/radio/src/targets/horus/extmodule_driver.cpp +++ b/radio/src/targets/horus/extmodule_driver.cpp @@ -143,7 +143,7 @@ void extmodulePxx1PulsesStart() } #endif -void extmoduleSerialStart(uint32_t /*baudrate*/, uint32_t period_half_us, bool inverted) +void extmoduleSerialStart() { EXTERNAL_MODULE_ON(); @@ -176,10 +176,9 @@ void extmoduleSerialStart(uint32_t /*baudrate*/, uint32_t period_half_us, bool i EXTMODULE_TIMER->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_0; #endif - EXTMODULE_TIMER->ARR = period_half_us; - EXTMODULE_TIMER->CCR2 = period_half_us - 4000; + EXTMODULE_TIMER->ARR = 40000; // dummy value until the DMA request kicks in EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag - EXTMODULE_TIMER->DIER |= TIM_DIER_UDE | TIM_DIER_CC2IE; + EXTMODULE_TIMER->DIER |= TIM_DIER_UDE; EXTMODULE_TIMER->CR1 |= TIM_CR1_CEN; NVIC_EnableIRQ(EXTMODULE_TIMER_DMA_STREAM_IRQn); @@ -342,23 +341,36 @@ void extmoduleSendNextFrame() #if defined(DSM2) case PROTOCOL_CHANNELS_SBUS: -#if defined(PCBX10) || PCBREV >= 13 - EXTMODULE_TIMER->CCER = TIM_CCER_CC3E | (GET_SBUS_POLARITY(EXTERNAL_MODULE) ? TIM_CCER_CC3P : 0); // reverse polarity for Sbus if needed -#else - EXTMODULE_TIMER->CCER = TIM_CCER_CC1E | (GET_SBUS_POLARITY(EXTERNAL_MODULE) ? TIM_CCER_CC1P : 0); // reverse polarity for Sbus if needed -#endif - // no break case PROTOCOL_CHANNELS_DSM2_LP45: case PROTOCOL_CHANNELS_DSM2_DSM2: case PROTOCOL_CHANNELS_DSM2_DSMX: case PROTOCOL_CHANNELS_MULTIMODULE: - EXTMODULE_TIMER->CCR2 = *(extmodulePulsesData.dsm2.ptr - 1) - 4000; // 2mS in advance + + if (EXTMODULE_TIMER_DMA_STREAM->CR & DMA_SxCR_EN) + return; + + if (PROTOCOL_CHANNELS_SBUS == moduleState[EXTERNAL_MODULE].protocol) { +#if defined(PCBX10) || PCBREV >= 13 + EXTMODULE_TIMER->CCER = TIM_CCER_CC3E | (GET_SBUS_POLARITY(EXTERNAL_MODULE) ? TIM_CCER_CC3P : 0); // reverse polarity for Sbus if needed +#else + EXTMODULE_TIMER->CCER = TIM_CCER_CC1E | (GET_SBUS_POLARITY(EXTERNAL_MODULE) ? TIM_CCER_CC1P : 0); // reverse polarity for Sbus if needed +#endif + } + + // disable timer + EXTMODULE_TIMER->CR1 &= ~TIM_CR1_CEN; + + // send DMA request EXTMODULE_TIMER_DMA_STREAM->CR &= ~DMA_SxCR_EN; // Disable DMA EXTMODULE_TIMER_DMA_STREAM->CR |= EXTMODULE_TIMER_DMA_CHANNEL | DMA_SxCR_DIR_0 | DMA_SxCR_MINC | EXTMODULE_TIMER_DMA_SIZE | DMA_SxCR_PL_0 | DMA_SxCR_PL_1; EXTMODULE_TIMER_DMA_STREAM->PAR = CONVERT_PTR_UINT(&EXTMODULE_TIMER->ARR); EXTMODULE_TIMER_DMA_STREAM->M0AR = CONVERT_PTR_UINT(extmodulePulsesData.dsm2.pulses); EXTMODULE_TIMER_DMA_STREAM->NDTR = extmodulePulsesData.dsm2.ptr - extmodulePulsesData.dsm2.pulses; EXTMODULE_TIMER_DMA_STREAM->CR |= DMA_SxCR_EN | DMA_SxCR_TCIE; // Enable DMA + + // re-init timer + EXTMODULE_TIMER->EGR = 1; + EXTMODULE_TIMER->CR1 |= TIM_CR1_CEN; break; #endif @@ -419,14 +431,20 @@ extern "C" void EXTMODULE_TIMER_DMA_IRQHandler() DMA_ClearITPendingBit(EXTMODULE_TIMER_DMA_STREAM, EXTMODULE_TIMER_DMA_FLAG_TC); - EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag - EXTMODULE_TIMER->DIER |= TIM_DIER_CC2IE; // Enable this interrupt + switch (moduleState[EXTERNAL_MODULE].protocol) { + case PROTOCOL_CHANNELS_PXX1_PULSES: + case PROTOCOL_CHANNELS_PPM: + EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag + EXTMODULE_TIMER->DIER |= TIM_DIER_CC2IE; // Enable this interrupt + break; + } } extern "C" void EXTMODULE_TIMER_IRQHandler() { EXTMODULE_TIMER->DIER &= ~TIM_DIER_CC2IE; // Stop this interrupt EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; + if (setupPulsesExternalModule()) extmoduleSendNextFrame(); } diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 479dd88c5..35a135cf4 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -902,6 +902,13 @@ #define TIMER_2MHz_RCC_APB1Periph RCC_APB1Periph_TIM7 #define TIMER_2MHz_TIMER TIM7 +// Mixer scheduler timer +#define MIXER_SCHEDULER_TIMER_RCC_APB1Periph RCC_APB1Periph_TIM13 +#define MIXER_SCHEDULER_TIMER TIM13 +#define MIXER_SCHEDULER_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) +#define MIXER_SCHEDULER_TIMER_IRQn TIM8_UP_TIM13_IRQn +#define MIXER_SCHEDULER_TIMER_IRQHandler TIM8_UP_TIM13_IRQHandler + // Bluetooth #define STORAGE_BLUETOOTH #if defined(BLUETOOTH) diff --git a/radio/src/targets/taranis/CMakeLists.txt b/radio/src/targets/taranis/CMakeLists.txt index 506f237cd..955a0994c 100644 --- a/radio/src/targets/taranis/CMakeLists.txt +++ b/radio/src/targets/taranis/CMakeLists.txt @@ -401,6 +401,7 @@ set(TARGET_SRC ../common/arm/stm32/audio_dac_driver.cpp ../common/arm/stm32/adc_driver.cpp ../common/arm/stm32/heartbeat_driver.cpp + ../common/arm/stm32/mixer_scheduler_driver.cpp ) if(PCB STREQUAL XLITE OR PCB STREQUAL XLITES) diff --git a/radio/src/targets/taranis/board.cpp b/radio/src/targets/taranis/board.cpp index b038fc0f7..7d8561636 100644 --- a/radio/src/targets/taranis/board.cpp +++ b/radio/src/targets/taranis/board.cpp @@ -110,6 +110,7 @@ void boardInit() AUX_SERIAL_RCC_APB1Periph | INTMODULE_RCC_APB1Periph | TRAINER_MODULE_RCC_APB1Periph | + MIXER_SCHEDULER_TIMER_RCC_APB1Periph | BT_RCC_APB1Periph | GYRO_RCC_APB1Periph, ENABLE); @@ -339,4 +340,4 @@ void initJackDetect(void) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_Init(JACK_DETECT_GPIO, &GPIO_InitStructure); } -#endif \ No newline at end of file +#endif diff --git a/radio/src/targets/taranis/board.h b/radio/src/targets/taranis/board.h index d9092fcdc..29fd50432 100644 --- a/radio/src/targets/taranis/board.h +++ b/radio/src/targets/taranis/board.h @@ -143,7 +143,7 @@ void intmoduleSendByte(uint8_t byte); void intmoduleSendBuffer(const uint8_t * data, uint8_t size); void intmoduleSendNextFrame(); -void extmoduleSerialStart(uint32_t baudrate, uint32_t period_half_us, bool inverted); +void extmoduleSerialStart(); void extmoduleInvertedSerialStart(uint32_t baudrate); void extmoduleSendBuffer(const uint8_t * data, uint8_t size); void extmoduleSendNextFrame(); diff --git a/radio/src/targets/taranis/extmodule_driver.cpp b/radio/src/targets/taranis/extmodule_driver.cpp index 4287c0f75..7a7927a41 100644 --- a/radio/src/targets/taranis/extmodule_driver.cpp +++ b/radio/src/targets/taranis/extmodule_driver.cpp @@ -83,7 +83,7 @@ void extmodulePpmStart() NVIC_SetPriority(EXTMODULE_TIMER_CC_IRQn, 7); } -void extmoduleSerialStart(uint32_t /*baudrate*/, uint32_t period_half_us, bool inverted) +void extmoduleSerialStart() { EXTERNAL_MODULE_ON(); @@ -99,16 +99,17 @@ void extmoduleSerialStart(uint32_t /*baudrate*/, uint32_t period_half_us, bool i EXTMODULE_TIMER->CR1 &= ~TIM_CR1_CEN; EXTMODULE_TIMER->PSC = EXTMODULE_TIMER_FREQ / 2000000 - 1; // 0.5uS from 30MHz - EXTMODULE_TIMER->CCER = EXTMODULE_TIMER_OUTPUT_ENABLE | (inverted ? 0 : EXTMODULE_TIMER_OUTPUT_POLARITY); + + EXTMODULE_TIMER->CCR3 = 0; + EXTMODULE_TIMER->CCER = EXTMODULE_TIMER_OUTPUT_ENABLE | EXTMODULE_TIMER_OUTPUT_POLARITY; EXTMODULE_TIMER->BDTR = TIM_BDTR_MOE; // Enable outputs EXTMODULE_TIMER->CCR1 = 0; EXTMODULE_TIMER->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_0; // Force O/P high EXTMODULE_TIMER->EGR = 1; // Restart EXTMODULE_TIMER->CCMR1 = TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_0; - EXTMODULE_TIMER->ARR = period_half_us; - EXTMODULE_TIMER->CCR2 = 40000; // The first frame will be sent in 20ms + EXTMODULE_TIMER->ARR = 40000; // dummy value until the DMA request kicks in EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag - EXTMODULE_TIMER->DIER |= TIM_DIER_UDE | TIM_DIER_CC2IE; + EXTMODULE_TIMER->DIER |= TIM_DIER_UDE; EXTMODULE_TIMER->CR1 |= TIM_CR1_CEN; NVIC_EnableIRQ(EXTMODULE_TIMER_DMA_STREAM_IRQn); @@ -300,19 +301,33 @@ void extmoduleSendNextFrame() #endif #if defined(SBUS) || defined(DSM2) || defined(MULTIMODULE) case PROTOCOL_CHANNELS_SBUS: - EXTMODULE_TIMER->CCER = EXTMODULE_TIMER_OUTPUT_ENABLE | (GET_SBUS_POLARITY(EXTERNAL_MODULE) ? 0 : EXTMODULE_TIMER_OUTPUT_POLARITY); // reverse polarity for Sbus if needed // no break case PROTOCOL_CHANNELS_DSM2_LP45: case PROTOCOL_CHANNELS_DSM2_DSM2: case PROTOCOL_CHANNELS_DSM2_DSMX: case PROTOCOL_CHANNELS_MULTIMODULE: - EXTMODULE_TIMER->CCR2 = *(extmodulePulsesData.dsm2.ptr - 1) - 4000; // 2mS in advance + + if (EXTMODULE_TIMER_DMA_STREAM->CR & DMA_SxCR_EN) + return; + + if (PROTOCOL_CHANNELS_SBUS == moduleState[EXTERNAL_MODULE].protocol) { + EXTMODULE_TIMER->CCER = EXTMODULE_TIMER_OUTPUT_ENABLE | (GET_SBUS_POLARITY(EXTERNAL_MODULE) ? EXTMODULE_TIMER_OUTPUT_POLARITY : 0); // reverse polarity for Sbus if needed + } + + // disable timer + EXTMODULE_TIMER->CR1 &= ~TIM_CR1_CEN; + + // send DMA request EXTMODULE_TIMER_DMA_STREAM->CR &= ~DMA_SxCR_EN; // Disable DMA EXTMODULE_TIMER_DMA_STREAM->CR |= EXTMODULE_TIMER_DMA_CHANNEL | DMA_SxCR_DIR_0 | DMA_SxCR_MINC | DMA_SxCR_PSIZE_0 | DMA_SxCR_MSIZE_0 | DMA_SxCR_PL_0 | DMA_SxCR_PL_1; EXTMODULE_TIMER_DMA_STREAM->PAR = CONVERT_PTR_UINT(&EXTMODULE_TIMER->ARR); EXTMODULE_TIMER_DMA_STREAM->M0AR = CONVERT_PTR_UINT(extmodulePulsesData.dsm2.pulses); EXTMODULE_TIMER_DMA_STREAM->NDTR = extmodulePulsesData.dsm2.ptr - extmodulePulsesData.dsm2.pulses; EXTMODULE_TIMER_DMA_STREAM->CR |= DMA_SxCR_EN | DMA_SxCR_TCIE; // Enable DMA + + // re-init timer + EXTMODULE_TIMER->EGR = 1; + EXTMODULE_TIMER->CR1 |= TIM_CR1_CEN; break; #endif @@ -373,14 +388,20 @@ extern "C" void EXTMODULE_TIMER_DMA_STREAM_IRQHandler() DMA_ClearITPendingBit(EXTMODULE_TIMER_DMA_STREAM, EXTMODULE_TIMER_DMA_FLAG_TC); - EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag - EXTMODULE_TIMER->DIER |= TIM_DIER_CC2IE; // Enable this interrupt + switch (moduleState[EXTERNAL_MODULE].protocol) { + case PROTOCOL_CHANNELS_PXX1_PULSES: + case PROTOCOL_CHANNELS_PPM: + EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; // Clear flag + EXTMODULE_TIMER->DIER |= TIM_DIER_CC2IE; // Enable this interrupt + break; + } } extern "C" void EXTMODULE_TIMER_CC_IRQHandler() { EXTMODULE_TIMER->DIER &= ~TIM_DIER_CC2IE; // Stop this interrupt EXTMODULE_TIMER->SR &= ~TIM_SR_CC2IF; + if (setupPulsesExternalModule()) { extmoduleSendNextFrame(); } diff --git a/radio/src/targets/taranis/hal.h b/radio/src/targets/taranis/hal.h index b7160cbf9..aebff4c88 100644 --- a/radio/src/targets/taranis/hal.h +++ b/radio/src/targets/taranis/hal.h @@ -2024,4 +2024,11 @@ #define TIMER_2MHz_RCC_APB1Periph RCC_APB1Periph_TIM7 #define TIMER_2MHz_TIMER TIM7 +// Mixer scheduler timer +#define MIXER_SCHEDULER_TIMER_RCC_APB1Periph RCC_APB1Periph_TIM13 +#define MIXER_SCHEDULER_TIMER TIM13 +#define MIXER_SCHEDULER_TIMER_FREQ (PERI1_FREQUENCY * TIMER_MULT_APB1) +#define MIXER_SCHEDULER_TIMER_IRQn TIM8_UP_TIM13_IRQn +#define MIXER_SCHEDULER_TIMER_IRQHandler TIM8_UP_TIM13_IRQHandler + #endif // _HAL_H_ diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 16e120784..e4c209edd 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -19,6 +19,7 @@ */ #include "opentx.h" +#include "mixer_scheduler.h" RTOS_TASK_HANDLE menusTaskId; RTOS_DEFINE_STACK(menusStack, MENUS_STACK_SIZE); @@ -75,13 +76,29 @@ bool isForcePowerOffRequested() bool isModuleSynchronous(uint8_t moduleIdx) { - uint8_t protocol = moduleState[moduleIdx].protocol; - if (protocol == PROTOCOL_CHANNELS_PXX2_HIGHSPEED || protocol == PROTOCOL_CHANNELS_PXX2_LOWSPEED || protocol == PROTOCOL_CHANNELS_CROSSFIRE || protocol == PROTOCOL_CHANNELS_GHOST || protocol == PROTOCOL_CHANNELS_NONE) - return true; -#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) - if (protocol == PROTOCOL_CHANNELS_PXX1_SERIAL) - return true; + switch(moduleState[moduleIdx].protocol) { + + case PROTOCOL_CHANNELS_PXX2_HIGHSPEED: + case PROTOCOL_CHANNELS_PXX2_LOWSPEED: + case PROTOCOL_CHANNELS_CROSSFIRE: + case PROTOCOL_CHANNELS_GHOST: + case PROTOCOL_CHANNELS_AFHDS3: + case PROTOCOL_CHANNELS_NONE: + +#if defined(MULTIMODULE) + case PROTOCOL_CHANNELS_MULTIMODULE: #endif +#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) + case PROTOCOL_CHANNELS_PXX1_SERIAL: +#endif +#if defined(DSM2) + case PROTOCOL_CHANNELS_SBUS: + case PROTOCOL_CHANNELS_DSM2_LP45: + case PROTOCOL_CHANNELS_DSM2_DSM2: + case PROTOCOL_CHANNELS_DSM2_DSMX: +#endif + return true; + } return false; } @@ -100,12 +117,17 @@ void sendSynchronousPulses(uint8_t runMask) } } +//#define DEBUG_MIXER_SCHEDULER + uint32_t nextMixerTime[NUM_MODULES]; TASK_FUNCTION(mixerTask) { s_pulses_paused = true; + mixerSchedulerInit(); + mixerSchedulerStart(); + while (true) { #if defined(SBUS_TRAINER) // SBUS trainer @@ -120,7 +142,16 @@ TASK_FUNCTION(mixerTask) bluetooth.wakeup(); #endif - RTOS_WAIT_TICKS(1); + // run mixer at least every 30ms + bool timeout = mixerSchedulerWaitForTrigger(30); + +#if defined(DEBUG_MIXER_SCHEDULER) + GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); + GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); +#endif + + // re-enable trigger + mixerSchedulerEnableTrigger(); #if defined(SIMU) if (pwrCheck() == e_power_off) { @@ -132,23 +163,6 @@ TASK_FUNCTION(mixerTask) } #endif - uint32_t now = RTOS_GET_MS(); - uint8_t runMask = 0; - - if (now >= nextMixerTime[0]) { - runMask |= (1 << 0); - } - -#if NUM_MODULES >= 2 - if (now >= nextMixerTime[1]) { - runMask |= (1 << 1); - } -#endif - - if (!runMask) { - continue; // go back to sleep - } - if (!s_pulses_paused) { uint16_t t0 = getTmr2MHz(); @@ -183,29 +197,18 @@ TASK_FUNCTION(mixerTask) if (t0 > maxMixerDuration) maxMixerDuration = t0; - sendSynchronousPulses(runMask); + // TODO: + // - check the cause of timeouts when switching + // between protocols with multi-proto RF + if (timeout) + serialPrint("mix sched timeout!"); + + // TODO: fix runMask + sendSynchronousPulses((1 << INTERNAL_MODULE) | (1 << EXTERNAL_MODULE)); } } } -void scheduleNextMixerCalculation(uint8_t module, uint32_t period_ms) -{ - // Schedule next mixer calculation time, - - if (isModuleSynchronous(module)) { - nextMixerTime[module] += period_ms / RTOS_MS_PER_TICK; - if (nextMixerTime[module] < RTOS_GET_TIME()) { - // we are late ... let's add some small delay - nextMixerTime[module] = (uint32_t) RTOS_GET_TIME() + (period_ms / RTOS_MS_PER_TICK); - } - } - else { - // for now assume mixer calculation takes 2 ms. - nextMixerTime[module] = (uint32_t) RTOS_GET_TIME() + (period_ms / RTOS_MS_PER_TICK); - } - - DEBUG_TIMER_STOP(debugTimerMixerCalcToUsage); -} #define MENU_TASK_PERIOD_TICKS (50 / RTOS_MS_PER_TICK) // 50ms diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index b46cc2b44..672dd56f0 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -184,6 +184,26 @@ void processCrossfireTelemetryFrame() break; } + case RADIO_ID: + if (telemetryRxBuffer[3] == 0xEA // radio address + && telemetryRxBuffer[5] == 0x10 // timing correction frame + ) { + + uint32_t update_interval; + int32_t offset; + if (getCrossfireTelemetryValue<4>(6, (int32_t&)update_interval) && getCrossfireTelemetryValue<4>(10, offset)) { + + // values are in 10th of micro-seconds + update_interval /= 10; + offset /= 10; + + TRACE("[XF] Rate: %d, Lag: %d", update_interval, offset); + getModuleSyncStatus(EXTERNAL_MODULE).update(update_interval, offset + SAFE_SYNC_LAG); + } + } + break; + + #if defined(LUA) default: if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(telemetryRxBufferCount-2) ) { diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index 9855b78f4..9940986f9 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -99,15 +99,15 @@ const uint32_t CROSSFIRE_BAUDRATES[] = { 115200, }; const uint8_t CROSSFIRE_PERIODS[] = { - 4, + 4, 16, }; #if SPORT_MAX_BAUDRATE < 400000 #define CROSSFIRE_BAUDRATE CROSSFIRE_BAUDRATES[g_eeGeneral.telemetryBaudrate] -#define CROSSFIRE_PERIOD CROSSFIRE_PERIODS[g_eeGeneral.telemetryBaudrate] +#define CROSSFIRE_PERIOD (CROSSFIRE_PERIODS[g_eeGeneral.telemetryBaudrate]*1000) #else #define CROSSFIRE_BAUDRATE 400000 -#define CROSSFIRE_PERIOD 4 // 4ms +#define CROSSFIRE_PERIOD 6666 /* us; 150 Hz */ #endif #define CROSSFIRE_TELEM_MIRROR_BAUDRATE 115200 diff --git a/radio/src/telemetry/multi.cpp b/radio/src/telemetry/multi.cpp index b1b86eb0a..eb5f6fc08 100644 --- a/radio/src/telemetry/multi.cpp +++ b/radio/src/telemetry/multi.cpp @@ -63,7 +63,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]; @@ -74,11 +73,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]; @@ -111,7 +105,6 @@ uint8_t intTelemetryRxBufferCount; #else // !INTERNAL_MODULE_MULTI static MultiModuleStatus multiModuleStatus; -static MultiModuleSyncStatus multiSyncStatus; static uint8_t multiBindStatus = MULTI_NORMAL_OPERATION; static MultiBufferState multiTelemetryBufferState; @@ -122,11 +115,6 @@ MultiModuleStatus& getMultiModuleStatus(uint8_t) return multiModuleStatus; } -MultiModuleSyncStatus& getMultiSyncStatus(uint8_t) -{ - return multiSyncStatus; -} - uint8_t getMultiBindStatus(uint8_t) { return multiBindStatus; @@ -254,24 +242,17 @@ 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 + uint16_t refreshRate = data[0] << 8 | data[1]; + int16_t inputLag = data[2] << 8 | data[3]; - status.calcAdjustedRefreshRate(data[0] << 8 | data[1], data[2] << 8 | data[3]); + // if (inputLag > refreshRate/2) + // inputLag -= refreshRate; -#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 + status.update(refreshRate, inputLag); + + serialPrint("MP ADJ: R %d, L %04d", refreshRate, inputLag); } #if defined(PCBTARANIS) || defined(PCBHORUS) @@ -434,104 +415,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 70c4fdc74..21e1e6d9b 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -21,6 +21,7 @@ #include "opentx.h" #include "multi.h" #include "pulses/afhds3.h" +#include "mixer_scheduler.h" uint8_t telemetryStreaming = 0; uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1) @@ -374,3 +375,88 @@ OutputTelemetryBuffer outputTelemetryBuffer __DMA; #if defined(LUA) Fifo * luaInputTelemetryFifo = NULL; #endif + +#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; + + if (lag == 0) { + return refreshRate; + } + + newRefreshRate += lag; + + if (newRefreshRate < MIN_REFRESH_RATE) { + newRefreshRate = MIN_REFRESH_RATE; + } + else if (newRefreshRate > MAX_REFRESH_RATE) { + newRefreshRate = MAX_REFRESH_RATE; + } + + TRACE("[SYNC] rate = %dus",newRefreshRate); + + currentLag -= newRefreshRate - refreshRate; + return (uint16_t)newRefreshRate; +} + +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) (refreshRate / 1000), 5); + tmp = strAppend(tmp, "us"); +#else + tmp = strAppend(tmp, "Sync at "); + tmp = strAppendUnsigned(tmp, (uint32_t) (refreshRate / 1000000)); + tmp = strAppend(tmp, " ms"); +#endif +} diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index 6599db749..18f4f98a6 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -289,4 +289,35 @@ extern Fifo * luaInputTelemetryFifo; void processPXX2Frame(uint8_t module, const uint8_t *frame); +// Module pulse synchronization +#define SAFE_SYNC_LAG 800 /* us */ + +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() { + // 2 seconds + return (get_tmr10ms() - lastUpdate < 200); + } + + // 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_