From ccd07cf03e669437f2fafd30db8c13525189b6e0 Mon Sep 17 00:00:00 2001 From: blckmn Date: Mon, 9 Jan 2017 22:43:49 +1100 Subject: [PATCH] F3 and F7 updates to remove DSHOT interrupt. --- src/main/drivers/dma.c | 10 ++++++++ src/main/drivers/dma.h | 2 +- src/main/drivers/pwm_output.h | 5 ++++ src/main/drivers/pwm_output_stm32f3xx.c | 16 +++---------- src/main/drivers/pwm_output_stm32f4xx.c | 3 +-- src/main/drivers/pwm_output_stm32f7xx.c | 31 +++---------------------- 6 files changed, 23 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/dma.c b/src/main/drivers/dma.c index f2efc67f66..672e73a20b 100644 --- a/src/main/drivers/dma.c +++ b/src/main/drivers/dma.c @@ -104,4 +104,14 @@ dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel) } } return 0; +} + +dmaChannelDescriptor_t* getDmaDescriptor(const DMA_Channel_TypeDef* channel) +{ + for (int i = 0; i < DMA_MAX_DESCRIPTORS; i++) { + if (dmaDescriptors[i].channel == channel) { + return &dmaDescriptors[i]; + } + } + return NULL; } \ No newline at end of file diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 3ae5665c59..4274486a00 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -128,7 +128,7 @@ typedef enum { #define DMA_IT_TEIF ((uint32_t)0x00000008) dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel); - +dmaChannelDescriptor_t* getDmaDescriptor(const DMA_Channel_TypeDef* channel); #endif void dmaInit(dmaIdentifier_e identifier, resourceOwner_e owner, uint8_t resourceIndex); diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 66202299e8..8ae0ae1948 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -20,6 +20,7 @@ #include "io/motors.h" #include "io/servos.h" #include "drivers/timer.h" +#include "drivers/dma.h" typedef enum { PWM_TYPE_STANDARD = 0, @@ -86,7 +87,11 @@ typedef struct { #else uint8_t dmaBuffer[MOTOR_DMA_BUFFER_SIZE]; #endif +#ifdef STM32F3 + dmaChannelDescriptor_t* dmaDescriptor; +#else uint32_t dmaFlag; +#endif #if defined(STM32F7) TIM_HandleTypeDef TimHandle; DMA_HandleTypeDef hdma_tim; diff --git a/src/main/drivers/pwm_output_stm32f3xx.c b/src/main/drivers/pwm_output_stm32f3xx.c index ffaa5f80d4..ddd80825c0 100644 --- a/src/main/drivers/pwm_output_stm32f3xx.c +++ b/src/main/drivers/pwm_output_stm32f3xx.c @@ -84,7 +84,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value) packet <<= 1; } + TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE); DMA_SetCurrDataCounter(motor->timerHardware->dmaChannel, MOTOR_DMA_BUFFER_SIZE); + DMA_CLEAR_FLAG(motor->dmaDescriptor, DMA_IT_TCIF); DMA_Cmd(motor->timerHardware->dmaChannel, ENABLE); } @@ -102,16 +104,6 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) } } -static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) -{ - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { - motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; - DMA_Cmd(descriptor->channel, DISABLE); - TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE); - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - } -} - void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) { TIM_OCInitTypeDef TIM_OCInitStructure; @@ -178,7 +170,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t } dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); - dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); + motor->dmaDescriptor = getDmaDescriptor(channel); DMA_Cmd(channel, DISABLE); DMA_DeInit(channel); @@ -196,8 +188,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; DMA_Init(channel, &DMA_InitStructure); - - DMA_ITConfig(channel, DMA_IT_TC, ENABLE); } #endif diff --git a/src/main/drivers/pwm_output_stm32f4xx.c b/src/main/drivers/pwm_output_stm32f4xx.c index 6515fdce05..dc76fbb08b 100644 --- a/src/main/drivers/pwm_output_stm32f4xx.c +++ b/src/main/drivers/pwm_output_stm32f4xx.c @@ -192,8 +192,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t DMA_Init(stream, &DMA_InitStructure); - motor->dmaFlag = dmaFlag_IT_TCIF(timerHardware->dmaStream); - DMA_ClearITPendingBit(stream, motor->dmaFlag); + motor->dmaFlag = dmaFlag_IT_TCIF(stream); } #endif diff --git a/src/main/drivers/pwm_output_stm32f7xx.c b/src/main/drivers/pwm_output_stm32f7xx.c index b5cc043283..30ccd983e2 100644 --- a/src/main/drivers/pwm_output_stm32f7xx.c +++ b/src/main/drivers/pwm_output_stm32f7xx.c @@ -52,7 +52,6 @@ uint8_t getTimerIndex(TIM_TypeDef *timer) void pwmWriteDigital(uint8_t index, uint16_t value) { - if (!pwmMotorsEnabled) { return; } @@ -82,6 +81,9 @@ void pwmWriteDigital(uint8_t index, uint16_t value) packet <<= 1; } + /* may not be required */ + HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]); + if(HAL_TIM_PWM_Start_DMA(&motor->TimHandle, motor->timerHardware->channel, motor->dmaBuffer, MOTOR_DMA_BUFFER_SIZE) != HAL_OK) { /* Starting PWM generation Error */ @@ -92,34 +94,8 @@ void pwmWriteDigital(uint8_t index, uint16_t value) void pwmCompleteDigitalMotorUpdate(uint8_t motorCount) { UNUSED(motorCount); - - if (!pwmMotorsEnabled) { - return; - } - - for (uint8_t i = 0; i < dmaMotorTimerCount; i++) { - //TIM_SetCounter(dmaMotorTimers[i].timer, 0); - //TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE); - } } - -static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) -{ - motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; - HAL_DMA_IRQHandler(motor->TimHandle.hdma[motor->timerDmaSource]); -} - -/*static void motor_DMA_IRQHandler(dmaChannelDescriptor_t *descriptor) -{ - if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { - motorDmaOutput_t * const motor = &dmaMotors[descriptor->userParam]; - DMA_Cmd(descriptor->stream, DISABLE); - TIM_DMACmd(motor->timerHardware->tim, motor->timerDmaSource, DISABLE); - DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); - } -}*/ - void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType) { motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; @@ -186,7 +162,6 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t __HAL_LINKDMA(&motor->TimHandle, hdma[motor->timerDmaSource], motor->hdma_tim); dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex)); - dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex); /* Initialize TIMx DMA handle */ if(HAL_DMA_Init(motor->TimHandle.hdma[motor->timerDmaSource]) != HAL_OK)