diff --git a/src/main/drivers/rx/expresslrs_driver.c b/src/main/drivers/rx/expresslrs_driver.c index 610c7ea286..e61d8bd033 100644 --- a/src/main/drivers/rx/expresslrs_driver.c +++ b/src/main/drivers/rx/expresslrs_driver.c @@ -101,13 +101,8 @@ void expressLrsUpdateTimerInterval(uint16_t intervalUs) timerState.intervalUs = intervalUs; expressLrsRecalculatePhaseShiftLimits(); -#ifdef USE_HAL_DRIVER timerReconfigureTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1)); - LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1); -#else - configTimeBase(timer, expressLrsCalculateMaximumExpectedPeriod(timerState.intervalUs), MHZ_TO_HZ(1)); - TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1); -#endif + timerSetPeriod(timer, (timerState.intervalUs / TICK_TOCK_COUNT) - 1); } void expressLrsUpdatePhaseShift(int32_t newPhaseShift) @@ -140,11 +135,7 @@ static void expressLrsOnTimerUpdate(timerOvrHandlerRec_t *cbRec, captureCompare_ uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.frequencyOffsetTicks; -#ifdef USE_HAL_DRIVER - LL_TIM_SetAutoReload(timer, adjustedPeriod - 1); -#else - TIM_SetAutoreload(timer, adjustedPeriod - 1); -#endif + timerSetPeriod(timer, adjustedPeriod - 1); expressLrsOnTimerTickISR(); @@ -154,11 +145,7 @@ static void expressLrsOnTimerUpdate(timerOvrHandlerRec_t *cbRec, captureCompare_ uint32_t adjustedPeriod = (timerState.intervalUs / TICK_TOCK_COUNT) + timerState.phaseShiftUs + timerState.frequencyOffsetTicks; -#ifdef USE_HAL_DRIVER - LL_TIM_SetAutoReload(timer, adjustedPeriod - 1); -#else - TIM_SetAutoreload(timer, adjustedPeriod - 1); -#endif + timerSetPeriod(timer, adjustedPeriod - 1); timerState.phaseShiftUs = 0; @@ -175,15 +162,8 @@ bool expressLrsTimerIsRunning(void) void expressLrsTimerStop(void) { -#ifdef USE_HAL_DRIVER - LL_TIM_DisableIT_UPDATE(timer); - LL_TIM_DisableCounter(timer); - LL_TIM_SetCounter(timer, 0); -#else - TIM_ITConfig(timer, TIM_IT_Update, DISABLE); - TIM_Cmd(timer, DISABLE); - TIM_SetCounter(timer, 0); -#endif + timerDisable(timer); + timerSetCounter(timer, 0); timerState.running = false; } @@ -191,29 +171,12 @@ void expressLrsTimerResume(void) { timerState.tickTock = TOCK; -#ifdef USE_HAL_DRIVER - LL_TIM_SetAutoReload(timer, (timerState.intervalUs / TICK_TOCK_COUNT)); - LL_TIM_SetCounter(timer, 0); - - LL_TIM_ClearFlag_UPDATE(timer); - LL_TIM_EnableIT_UPDATE(timer); -#else - TIM_SetAutoreload(timer, (timerState.intervalUs / TICK_TOCK_COUNT)); - TIM_SetCounter(timer, 0); - - TIM_ClearFlag(timer, TIM_FLAG_Update); - TIM_ITConfig(timer, TIM_IT_Update, ENABLE); -#endif + timerSetPeriod(timer, (timerState.intervalUs / TICK_TOCK_COUNT)); + timerSetCounter(timer, 0); + timerEnableInterrupt(timer); timerState.running = true; - -#ifdef USE_HAL_DRIVER - LL_TIM_EnableCounter(timer); - LL_TIM_GenerateEvent_UPDATE(timer); -#else - TIM_Cmd(timer, ENABLE); - TIM_GenerateEvent(timer, TIM_EventSource_Update); -#endif + timerEnable(timer); } void expressLrsInitialiseTimer(TIM_TypeDef *t, timerOvrHandlerRec_t *timerUpdateCb) diff --git a/src/main/drivers/serial_escserial.c b/src/main/drivers/serial_escserial.c index b0d7424a1b..6067dfde22 100644 --- a/src/main/drivers/serial_escserial.c +++ b/src/main/drivers/serial_escserial.c @@ -134,13 +134,6 @@ enum { #define STOP_BIT_MASK (1 << 0) #define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1)) -#ifdef USE_HAL_DRIVER -static void TIM_DeInit(TIM_TypeDef *tim) -{ - LL_TIM_DeInit(tim); -} -#endif - static void setTxSignalEsc(escSerial_t *escSerial, uint8_t state) { if (escSerial->mode == PROTOCOL_KISSALL) @@ -343,7 +336,7 @@ static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8 { uint32_t clock = SystemCoreClock/2; uint32_t timerPeriod; - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); do { timerPeriod = clock / baud; if (isTimerPeriodTooLarge(timerPeriod)) { @@ -377,11 +370,8 @@ static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t c // Adjust the timing so it will interrupt on the middle. // This is clobbers transmission, but it is okay because we are // always half-duplex. -#ifdef USE_HAL_DRIVER - __HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2); -#else - TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2); -#endif + timerSetCounter(escSerial->txTimerHardware->tim, timerGetPeriod(escSerial->txTimerHardware->tim) / 2); + if (escSerial->isTransmittingData) { escSerial->transmissionErrors++; } @@ -414,7 +404,7 @@ static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t c static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_e options) { // start bit is usually a FALLING signal - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, SystemCoreClock / 2); timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL); @@ -538,7 +528,7 @@ static void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t captur static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { uint32_t timerPeriod = 34; - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); timerReconfigureTimeBase(timerHardwarePtr->tim, timerPeriod, MHZ_TO_HZ(1)); timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc); timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL); @@ -570,12 +560,7 @@ static void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb); - //clear timer -#ifdef USE_HAL_DRIVER - __HAL_TIM_SetCounter(escSerial->rxTimerHandle, 0); -#else - TIM_SetCounter(escSerial->rxTimerHardware->tim,0); -#endif + timerSetCounter(escSerial->rxTimerHardware->tim, 0); if (capture > 40 && capture < 90) { @@ -628,7 +613,7 @@ static void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference) { // start bit is usually a FALLING signal - TIM_DeInit(timerHardwarePtr->tim); + timerReset(timerHardwarePtr->tim); timerReconfigureTimeBase(timerHardwarePtr->tim, 0xFFFF, MHZ_TO_HZ(1)); timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0); timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc); @@ -786,11 +771,11 @@ static void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode) if (mode != PROTOCOL_KISSALL) { escSerialInputPortDeConfig(escSerial->rxTimerHardware); timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL); - TIM_DeInit(escSerial->rxTimerHardware->tim); + timerReset(escSerial->rxTimerHardware->tim); } timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL); - TIM_DeInit(escSerial->txTimerHardware->tim); + timerReset(escSerial->txTimerHardware->tim); } static uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance) diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 13fe8bb682..0f69dee869 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -487,11 +487,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) // Synchronize the bit timing so that it will interrupt at the center // of the bit period. -#ifdef USE_HAL_DRIVER - __HAL_TIM_SetCounter(self->timerHandle, __HAL_TIM_GetAutoreload(self->timerHandle) / 2); -#else - TIM_SetCounter(self->timerHardware->tim, self->timerHardware->tim->ARR / 2); -#endif + timerSetCounter(self->timerHardware->tim, timerGetPeriod(self->timerHardware->tim) / 2); // For a mono-timer full duplex configuration, this may clobber the // transmission because the next callback to the onSerialTimerOverflow diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 28ed1872b3..40bae0689c 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -216,3 +216,12 @@ uint16_t timerGetPeriodByPrescaler(TIM_TypeDef *tim, uint16_t prescaler, uint32_ int8_t timerGetNumberByIndex(uint8_t index); int8_t timerGetTIMNumber(const TIM_TypeDef *tim); uint8_t timerLookupChannelIndex(const uint16_t channel); + +// TODO: replace TIM_TypeDef with alternate +void timerReset(TIM_TypeDef *timer); +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period); +uint32_t timerGetPeriod(TIM_TypeDef *timer); +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter); +void timerDisable(TIM_TypeDef *timer); +void timerEnable(TIM_TypeDef *timer); +void timerEnableInterrupt(TIM_TypeDef *timer); diff --git a/src/platform/APM32/timer_apm32.c b/src/platform/APM32/timer_apm32.c index b113bc5f07..12ef8f197b 100644 --- a/src/platform/APM32/timer_apm32.c +++ b/src/platform/APM32/timer_apm32.c @@ -330,8 +330,9 @@ static void timerNVICConfigure(uint8_t irq) TMR_HandleTypeDef* timerFindTimerHandle(TMR_TypeDef *tim) { uint8_t timerIndex = lookupTimerIndex(tim); - if (timerIndex >= USED_TIMER_COUNT) + if (timerIndex >= USED_TIMER_COUNT) { return NULL; + } return &timerHandle[timerIndex].Handle; } @@ -1196,4 +1197,43 @@ DAL_StatusTypeDef DMA_SetCurrDataCounter(TMR_HandleTypeDef *htim, uint32_t Chann /* Return function status */ return DAL_OK; } + +void timerReset(TIM_TypeDef *timer) +{ + DDL_TMR_DeInit(timer); +} + +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period) +{ + timer->AUTORLD = period; +} + +uint32_t timerGetPeriod(TIM_TypeDef *timer) +{ + return timer->AUTORLD; +} + +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter) +{ + timer->CNT = counter; +} + +void timerDisable(TIM_TypeDef *timer) +{ + DDL_TMR_DisableIT_UPDATE(timer); + DDL_TMR_DisableCounter(timer); +} + +void timerEnable(TIM_TypeDef *timer) +{ + DDL_TMR_EnableCounter(timer); + DDL_TMR_GenerateEvent_UPDATE(timer); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + DDL_TMR_ClearFlag_UPDATE(timer); + DDL_TMR_EnableIT_UPDATE(timer); +} + #endif diff --git a/src/platform/AT32/target/AT32F435G/target.h b/src/platform/AT32/target/AT32F435G/target.h index b76a523cd4..e9dbcebed6 100644 --- a/src/platform/AT32/target/AT32F435G/target.h +++ b/src/platform/AT32/target/AT32F435G/target.h @@ -97,5 +97,6 @@ #undef USE_RX_EXPRESSLRS // #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #undef USE_SERIAL_4WAY_SK_BOOTLOADER +#define USE_ESCSERIAL #define FLASH_PAGE_SIZE ((uint32_t)0x0800) // 2K sectors diff --git a/src/platform/AT32/target/AT32F435M/target.h b/src/platform/AT32/target/AT32F435M/target.h index fb52e7fcea..3ee3ca1928 100644 --- a/src/platform/AT32/target/AT32F435M/target.h +++ b/src/platform/AT32/target/AT32F435M/target.h @@ -97,5 +97,6 @@ #undef USE_RX_EXPRESSLRS // #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #undef USE_SERIAL_4WAY_SK_BOOTLOADER +#define USE_ESCSERIAL #define FLASH_PAGE_SIZE ((uint32_t)0x1000) // 4K sectors diff --git a/src/platform/AT32/timer_at32bsp.c b/src/platform/AT32/timer_at32bsp.c index f76a417152..6336ad1a2a 100644 --- a/src/platform/AT32/timer_at32bsp.c +++ b/src/platform/AT32/timer_at32bsp.c @@ -777,4 +777,50 @@ uint16_t timerGetPrescalerByDesiredHertz(tmr_type *tim, uint32_t hz) } return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; } + +void timerReset(tmr_type *timer) +{ + ATOMIC_BLOCK(NVIC_PRIO_TIMER) { + tmr_counter_enable(timer, FALSE); + } +} + +void timerSetPeriod(tmr_type *timer, uint32_t period) +{ + tmr_period_value_set(timer, period); +} + +uint32_t timerGetPeriod(tmr_type *timer) +{ + return tmr_period_value_get(timer); +} + +void timerSetCounter(tmr_type *timer, uint32_t counter) +{ + tmr_counter_value_set(timer, counter); +} + +void timerReconfigureTimeBase(tmr_type *timer, uint16_t period, uint32_t hz) +{ + configTimeBase(timer, period, hz); +} + +void timerDisable(TIM_TypeDef *timer) +{ + tmr_interrupt_enable(timer, TMR_OVF_INT, FALSE); + tmr_counter_enable(timer, FALSE); +} + +void timerEnable(TIM_TypeDef *timer) +{ + tmr_counter_enable(timer, TRUE); + tmr_overflow_event_disable(timer, TRUE); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + tmr_flag_clear(timer, TMR_OVF_FLAG); + tmr_interrupt_enable(timer, TMR_OVF_INT, TRUE); +} + #endif diff --git a/src/platform/STM32/timer_hal.c b/src/platform/STM32/timer_hal.c index 9f68624eca..c9e9180f6c 100644 --- a/src/platform/STM32/timer_hal.c +++ b/src/platform/STM32/timer_hal.c @@ -1251,4 +1251,43 @@ HAL_StatusTypeDef DMA_SetCurrDataCounter(TIM_HandleTypeDef *htim, uint32_t Chann /* Return function status */ return HAL_OK; } + +void timerReset(TIM_TypeDef *timer) +{ + LL_TIM_DeInit(timer); +} + +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period) +{ + timer->ARR = period; +} + +uint32_t timerGetPeriod(TIM_TypeDef *timer) +{ + return timer->ARR; +} + +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter) +{ + timer->CNT = counter; +} + +void timerDisable(TIM_TypeDef *timer) +{ + LL_TIM_DisableIT_UPDATE(timer); + LL_TIM_DisableCounter(timer); +} + +void timerEnable(TIM_TypeDef *timer) +{ + LL_TIM_EnableCounter(timer); + LL_TIM_GenerateEvent_UPDATE(timer); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + LL_TIM_ClearFlag_UPDATE(timer); + LL_TIM_EnableIT_UPDATE(timer); +} + #endif diff --git a/src/platform/STM32/timer_stdperiph.c b/src/platform/STM32/timer_stdperiph.c index f12fd5547b..8e0c2bc95a 100644 --- a/src/platform/STM32/timer_stdperiph.c +++ b/src/platform/STM32/timer_stdperiph.c @@ -954,4 +954,43 @@ uint16_t timerGetPrescalerByDesiredHertz(TIM_TypeDef *tim, uint32_t hz) } return (uint16_t)((timerClock(tim) + hz / 2 ) / hz) - 1; } + +void timerReset(TIM_TypeDef *timer) +{ + TIM_DeInit(timer); +} + +void timerSetPeriod(TIM_TypeDef *timer, uint32_t period) +{ + timer->ARR = period; +} + +uint32_t timerGetPeriod(TIM_TypeDef *timer) +{ + return timer->ARR; +} + +void timerSetCounter(TIM_TypeDef *timer, uint32_t counter) +{ + timer->CNT = counter; +} + +void timerDisable(TIM_TypeDef *timer) +{ + TIM_ITConfig(timer, TIM_IT_Update, DISABLE); + TIM_Cmd(timer, DISABLE); +} + +void timerEnable(TIM_TypeDef *timer) +{ + TIM_Cmd(timer, ENABLE); + TIM_GenerateEvent(timer, TIM_EventSource_Update); +} + +void timerEnableInterrupt(TIM_TypeDef *timer) +{ + TIM_ClearFlag(timer, TIM_FLAG_Update); + TIM_ITConfig(timer, TIM_IT_Update, ENABLE); +} + #endif