diff --git a/radio/src/pulses/pulses.cpp b/radio/src/pulses/pulses.cpp index 21c4d7eee..8579d0c42 100755 --- a/radio/src/pulses/pulses.cpp +++ b/radio/src/pulses/pulses.cpp @@ -317,7 +317,6 @@ 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); break; #endif default: diff --git a/radio/src/rtos.h b/radio/src/rtos.h index 6e42ebf30..56de40825 100644 --- a/radio/src/rtos.h +++ b/radio/src/rtos.h @@ -230,6 +230,8 @@ 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) #ifdef __cplusplus template 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 e2f01f586..39efa3478 100755 --- a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp +++ b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp @@ -39,12 +39,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() @@ -93,28 +87,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) { @@ -187,12 +159,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/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index ef9b75c42..6b25b8942 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -190,6 +190,7 @@ set(TARGET_SRC led_driver.cpp extmodule_driver.cpp trainer_driver.cpp + 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 ac6e4e1f8..e026b3ad5 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -98,6 +98,7 @@ void boardInit() AUDIO_RCC_APB1Periph | INTMODULE_RCC_APB1Periph | EXTMODULE_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 c6cb49e87..e1772dd8b 100644 --- a/radio/src/targets/horus/board.h +++ b/radio/src/targets/horus/board.h @@ -174,9 +174,6 @@ 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(); @@ -193,6 +190,14 @@ void stop_trainer_ppm(); void init_trainer_capture(); void stop_trainer_capture(); +// Mixer scheduler driver +void mixerSchedulerInit(); +void mixerSchedulerStart(uint16_t periodUs); +void mixerSchedulerStop(); +void mixerSchedulerWaitForTrigger(uint8_t timeoutMs); +void mixerSchedulerEnableTrigger(); +void mixerSchedulerDisableTrigger(); + // Keys driver enum EnumKeys { diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 6f226d7b9..48bc922ac 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -365,7 +365,7 @@ // Telemetry #define TELEMETRY_RCC_AHB1Periph (RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_DMA1) #define TELEMETRY_RCC_APB1Periph RCC_APB1Periph_USART2 -#define TELEMETRY_RCC_APB2Periph RCC_APB2Periph_TIM10 +#define TELEMETRY_RCC_APB2Periph RCC_APB2Periph_TIM11 #define TELEMETRY_DIR_GPIO GPIOD #define TELEMETRY_DIR_GPIO_PIN GPIO_Pin_4 // PD.04 #define TELEMETRY_GPIO GPIOD @@ -754,6 +754,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 #define BT_RCC_APB2Periph RCC_APB2Periph_USART6 diff --git a/radio/src/targets/horus/mixer_scheduler_driver.cpp b/radio/src/targets/horus/mixer_scheduler_driver.cpp new file mode 100644 index 000000000..548bfef97 --- /dev/null +++ b/radio/src/targets/horus/mixer_scheduler_driver.cpp @@ -0,0 +1,80 @@ +/* + * 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" + +RTOS_FLAG_HANDLE mixerFlag; + +void mixerSchedulerInit() +{ + RTOS_CREATE_FLAG(mixerFlag); +} + +void mixerSchedulerStart(uint16_t periodUs) +{ + MIXER_SCHEDULER_TIMER->CR1 = TIM_CR1_URS; // do not generate interrupt on soft update + MIXER_SCHEDULER_TIMER->PSC = INTMODULE_TIMER_FREQ / 2000000 - 1; // 0.5uS (2Mhz) + MIXER_SCHEDULER_TIMER->CCER = 0; + MIXER_SCHEDULER_TIMER->CCMR1 = 0; + MIXER_SCHEDULER_TIMER->ARR = 2 * periodUs - 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 mixerSchedulerEnableTrigger() +{ + MIXER_SCHEDULER_TIMER->DIER |= TIM_DIER_UIE; // enable interrupt +} + +void mixerSchedulerDisableTrigger() +{ + MIXER_SCHEDULER_TIMER->DIER &= ~TIM_DIER_UIE; // disable interrupt +} + +void mixerSchedulerWaitForTrigger(uint8_t timeoutMs) +{ + RTOS_CLEAR_FLAG(mixerFlag); + RTOS_WAIT_FLAG(mixerFlag, timeoutMs); +} + +extern "C" void MIXER_SCHEDULER_TIMER_IRQHandler(void) +{ + MIXER_SCHEDULER_TIMER->SR &= ~TIM_SR_UIF; // clear flag + mixerSchedulerDisableTrigger(); + + // trigger mixer start + CoEnterISR(); + CoSchedLock(); + isr_SetFlag(mixerFlag); + CoSchedUnlock(); + CoExitISR(); +} diff --git a/radio/src/tasks.cpp b/radio/src/tasks.cpp index 9cbe9e2bd..4f803e554 100644 --- a/radio/src/tasks.cpp +++ b/radio/src/tasks.cpp @@ -75,13 +75,21 @@ bool isForcePowerOffRequested() bool isModuleSynchronous(uint8_t module) { - uint8_t protocol = moduleState[module].protocol; - if (protocol == PROTOCOL_CHANNELS_PXX2_HIGHSPEED || protocol == PROTOCOL_CHANNELS_PXX2_LOWSPEED || protocol == PROTOCOL_CHANNELS_CROSSFIRE || protocol == PROTOCOL_CHANNELS_NONE) - return true; -#if defined(INTMODULE_USART) || defined(EXTMODULE_USART) - if (protocol == PROTOCOL_CHANNELS_PXX1_SERIAL) - return true; + switch(moduleState[module].protocol) { + + case PROTOCOL_CHANNELS_PXX2_HIGHSPEED: + case PROTOCOL_CHANNELS_PXX2_LOWSPEED: + case PROTOCOL_CHANNELS_CROSSFIRE: + 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 + return true; + } return false; } @@ -100,13 +108,17 @@ void sendSynchronousPulses() } } +#define DEBUG_MIXER_SCHEDULER + uint32_t nextMixerTime[NUM_MODULES]; TASK_FUNCTION(mixerTask) { - static uint32_t lastRunTime; s_pulses_paused = true; + mixerSchedulerInit(); + mixerSchedulerStart(6666); // 150 Hz + while (true) { #if defined(PCBTARANIS) && defined(SBUS) // SBUS trainer @@ -121,7 +133,22 @@ TASK_FUNCTION(mixerTask) bluetooth.wakeup(); #endif - RTOS_WAIT_TICKS(1); + // TODO: + // - add trigger based on heartbeat driver + + // run mixer at least every 10ms + mixerSchedulerWaitForTrigger(10); + +#if defined(DEBUG_MIXER_SCHEDULER) + GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); + GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN); +#endif + + // TODO: + // - compute next trigger + + // re-enable trigger + mixerSchedulerEnableTrigger(); #if defined(SIMU) if (pwrCheck() == e_power_off) { @@ -133,36 +160,6 @@ TASK_FUNCTION(mixerTask) } #endif - uint32_t now = RTOS_GET_MS(); - bool run = false; - - if (now - lastRunTime >= 10) { - // run at least every 10ms - run = true; - } - -#if defined(INTMODULE_USART) && defined(INTMODULE_HEARTBEAT) - if ((moduleState[INTERNAL_MODULE].protocol == PROTOCOL_CHANNELS_PXX2_HIGHSPEED || moduleState[INTERNAL_MODULE].protocol == PROTOCOL_CHANNELS_PXX1_SERIAL) && heartbeatCapture.valid && heartbeatCapture.timestamp > lastRunTime) { - run = true; - } -#endif - - if (now == nextMixerTime[0]) { - run = true; - } - -#if NUM_MODULES >= 2 - if (now == nextMixerTime[1]) { - run = true; - } -#endif - - if (!run) { - continue; // go back to sleep - } - - lastRunTime = now; - if (!s_pulses_paused) { uint16_t t0 = getTmr2MHz();