diff --git a/Makefile b/Makefile index 6c63d5d48a..b7374d7f58 100644 --- a/Makefile +++ b/Makefile @@ -641,6 +641,7 @@ STM32F7xx_COMMON_SRC = \ drivers/inverter.c \ drivers/bus_spi_hal.c \ drivers/timer_hal.c \ + drivers/timer_stm32f7xx.c \ drivers/pwm_output_hal.c \ drivers/system_stm32f7xx.c \ drivers/serial_uart_stm32f7xx.c \ @@ -653,7 +654,6 @@ F7EXCLUDES = drivers/bus_spi.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ - io/ledstrip.c \ drivers/serial_uart.c # check if target.mk supplied diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index bd935432dc..c7110799c9 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -39,7 +39,7 @@ #include "dma.h" #include "light_ws2811strip.h" -#if defined(STM32F4) +#if defined(STM32F4) || defined(STM32F7) uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #else uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index c24712502c..c980353abb 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -28,6 +28,9 @@ #if defined(STM32F40_41xxx) #define BIT_COMPARE_1 67 // timer compare value for logical 1 #define BIT_COMPARE_0 33 // timer compare value for logical 0 +#elif defined(STM32F7) +#define BIT_COMPARE_1 76 // timer compare value for logical 1 +#define BIT_COMPARE_0 38 // timer compare value for logical 0 #else #define BIT_COMPARE_1 17 // timer compare value for logical 1 #define BIT_COMPARE_0 9 // timer compare value for logical 0 @@ -51,7 +54,7 @@ void setStripColors(const hsvColor_t *colors); bool isWS2811LedStripReady(void); -#if defined(STM32F4) +#if defined(STM32F4) || defined(STM32F7) extern uint32_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; #else extern uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; diff --git a/src/main/drivers/light_ws2811strip_hal.c b/src/main/drivers/light_ws2811strip_hal.c index 7d74bfb39d..2a950330f2 100644 --- a/src/main/drivers/light_ws2811strip_hal.c +++ b/src/main/drivers/light_ws2811strip_hal.c @@ -19,37 +19,119 @@ #include #include "platform.h" -#include "gpio.h" + +#ifdef LED_STRIP #include "common/color.h" -#include "drivers/light_ws2811strip.h" +#include "light_ws2811strip.h" #include "nvic.h" +#include "dma.h" +#include "io.h" +#include "system.h" +#include "rcc.h" +#include "timer.h" -static TIM_HandleTypeDef ledTimHandle; -static DMA_HandleTypeDef ledDmaHandle; +#if !defined(WS2811_PIN) +#define WS2811_PIN PA0 +#define WS2811_TIMER TIM5 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream2 +#define WS2811_DMA_IT DMA_IT_TCIF2 +#define WS2811_DMA_CHANNEL DMA_Channel_6 +#define WS2811_TIMER_CHANNEL TIM_Channel_1 +#endif + +static IO_t ws2811IO = IO_NONE; +static uint16_t timDMASource = 0; +bool ws2811Initialised = false; + +TIM_HandleTypeDef TimHandle; + +void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) +{ + if(htim->Instance==WS2811_TIMER) + { + HAL_TIM_PWM_Stop_DMA(&TimHandle,WS2811_TIMER_CHANNEL); + } +} + +void WS2811_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor) +{ + ws2811LedDataTransferInProgress = 0; + HAL_DMA_IRQHandler(TimHandle.hdma[2]); +} + +void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim) +{ + static DMA_HandleTypeDef hdma_tim; + + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerGPIOAF(WS2811_TIMER)); + + __TIM5_CLK_ENABLE(); + __DMA1_CLK_ENABLE(); + + + /* Set the parameters to be configured */ + hdma_tim.Init.Channel = WS2811_DMA_CHANNEL; + hdma_tim.Init.Direction = DMA_MEMORY_TO_PERIPH; + hdma_tim.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_tim.Init.MemInc = DMA_MINC_ENABLE; + hdma_tim.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ; + hdma_tim.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ; + hdma_tim.Init.Mode = DMA_NORMAL; + hdma_tim.Init.Priority = DMA_PRIORITY_HIGH; + hdma_tim.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + hdma_tim.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + hdma_tim.Init.MemBurst = DMA_MBURST_SINGLE; + hdma_tim.Init.PeriphBurst = DMA_PBURST_SINGLE; + + /* Set hdma_tim instance */ + hdma_tim.Instance = WS2811_DMA_STREAM; + + uint32_t channelAddress = 0; + switch (WS2811_TIMER_CHANNEL) { + case TIM_CHANNEL_1: + timDMASource = TIM_DMA_ID_CC1; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR1); + break; + case TIM_CHANNEL_2: + timDMASource = TIM_DMA_ID_CC2; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR2); + break; + case TIM_CHANNEL_3: + timDMASource = TIM_DMA_ID_CC3; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR3); + break; + case TIM_CHANNEL_4: + timDMASource = TIM_DMA_ID_CC4; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR4); + break; + } + + /* Link hdma_tim to hdma[3] (channel3) */ + __HAL_LINKDMA(htim, hdma[timDMASource], hdma_tim); + + /* Initialize TIMx DMA handle */ + HAL_DMA_Init(htim->hdma[timDMASource]); + + dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, WS2811_DMA_IRQHandler, NVIC_PRIO_WS2811_DMA, timDMASource); +} void ws2811LedStripHardwareInit(void) { - GPIO_InitTypeDef GPIO_InitStructure; + TimHandle.Instance = WS2811_TIMER; - GPIO_InitStructure.Pin = LED_STRIP_PIN; - GPIO_InitStructure.Mode = GPIO_MODE_AF_PP; - GPIO_InitStructure.Pull = GPIO_PULLUP; - GPIO_InitStructure.Speed = GPIO_SPEED_HIGH; - GPIO_InitStructure.Alternate = LED_STRIP_AF; - HAL_GPIO_Init(LED_STRIP_GPIO, &GPIO_InitStructure); - - - ledTimHandle.Instance = LED_STRIP_TIMER; - // Stop timer - HAL_TIM_Base_Stop(&ledTimHandle); - - ledTimHandle.Init.Prescaler = (uint16_t) (SystemCoreClock / 2 / 84000000) - 1; - ledTimHandle.Init.Period = 104; // 800kHz - ledTimHandle.Init.ClockDivision = 0; - ledTimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - - HAL_TIM_PWM_Init(&ledTimHandle); + TimHandle.Init.Prescaler = 1; + TimHandle.Init.Period = 135; // 800kHz + TimHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; + if(HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) + { + /* Initialization Error */ + } TIM_OC_InitTypeDef TIM_OCInitStructure; @@ -60,51 +142,40 @@ void ws2811LedStripHardwareInit(void) TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET; TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET; TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; - - HAL_TIM_PWM_ConfigChannel(&ledTimHandle, &TIM_OCInitStructure, LED_STRIP_TIMER_CHANNEL); - HAL_TIM_Base_Start(&ledTimHandle); - - /* Set the parameters to be configured */ - ledDmaHandle.Instance = LED_STRIP_DMA_STREAM; - ledDmaHandle.Init.Channel = LED_STRIP_DMA_CHANNEL; - ledDmaHandle.Init.Direction = DMA_MEMORY_TO_PERIPH; - ledDmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; - ledDmaHandle.Init.MemInc = DMA_MINC_ENABLE; - ledDmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD ; - ledDmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_WORD ; - ledDmaHandle.Init.Mode = DMA_NORMAL; - ledDmaHandle.Init.Priority = DMA_PRIORITY_VERY_HIGH; - ledDmaHandle.Init.FIFOMode = DMA_FIFOMODE_ENABLE; - ledDmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL; - ledDmaHandle.Init.MemBurst = DMA_MBURST_SINGLE; - ledDmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; - - __HAL_LINKDMA(&ledTimHandle, hdma[LED_STRIP_TIMER_DMA_RQ], ledDmaHandle); - - /* Initialize TIMx DMA handle */ - HAL_DMA_Init(ledTimHandle.hdma[LED_STRIP_TIMER_DMA_RQ]); - - HAL_NVIC_SetPriority(LED_STRIP_DMA_STREAM_IRQn, NVIC_PRIORITY_BASE(NVIC_PRIO_WS2811_DMA), NVIC_PRIORITY_SUB(NVIC_PRIO_WS2811_DMA)); - HAL_NVIC_EnableIRQ(LED_STRIP_DMA_STREAM_IRQn); - -// DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(TIM5->CCR1); -// DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer; -// DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; + if(HAL_TIM_PWM_ConfigChannel(&TimHandle, &TIM_OCInitStructure, WS2811_TIMER_CHANNEL) != HAL_OK) + { + /* Configuration Error */ + } + const hsvColor_t hsv_white = { 0, 255, 255}; + ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); } -void LED_STRIP_DMA_IRQHandler(void) -{ - HAL_DMA_IRQHandler(ledTimHandle.hdma[LED_STRIP_TIMER_DMA_RQ]); - ws2811LedDataTransferInProgress = 0; -} void ws2811LedStripDMAEnable(void) { - __HAL_TIM_SET_COUNTER(&ledTimHandle, 0); - HAL_TIM_PWM_Start_DMA(&ledTimHandle, LED_STRIP_TIMER_CHANNEL, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE); -} + if (!ws2811Initialised) + { + ws2811LedDataTransferInProgress = 0; + return; + } + if(HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) + { + /* Initialization Error */ + ws2811LedDataTransferInProgress = 0; + return; + } + + if( HAL_TIM_PWM_Start_DMA(&TimHandle, WS2811_TIMER_CHANNEL, ledStripDMABuffer, WS2811_DMA_BUFFER_SIZE) != HAL_OK) + { + /* Starting PWM generation Error */ + ws2811LedDataTransferInProgress = 0; + return; + } + +} +#endif diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index d3bec2e7c5..c8045a6cab 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -115,6 +115,8 @@ typedef enum { #define HARDWARE_TIMER_DEFINITION_COUNT 10 #elif defined(STM32F4) #define HARDWARE_TIMER_DEFINITION_COUNT 14 +#elif defined(STM32F7) +#define HARDWARE_TIMER_DEFINITION_COUNT 14 #endif extern const timerHardware_t timerHardware[]; diff --git a/src/main/drivers/timer_hal.c b/src/main/drivers/timer_hal.c index 92dd08ec73..c8167dfb84 100644 --- a/src/main/drivers/timer_hal.c +++ b/src/main/drivers/timer_hal.c @@ -17,7 +17,6 @@ #include #include -#include #include #include "platform.h" @@ -27,6 +26,7 @@ #include "nvic.h" #include "gpio.h" +#include "rcc.h" #include "system.h" #include "timer.h" @@ -204,6 +204,29 @@ static inline uint8_t lookupChannelIndex(const uint16_t channel) return channel >> 2; } +rccPeriphTag_t timerRCC(TIM_TypeDef *tim) +{ + for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].rcc; + } + } + return 0; +} + +#if defined(STM32F7) +uint8_t timerGPIOAF(TIM_TypeDef *tim) +{ + for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].alternateFunction; + } + } + return 0; +} +#endif + + void timerNVICConfigure(uint8_t irq) { HAL_NVIC_SetPriority(irq, NVIC_PRIORITY_BASE(NVIC_PRIO_TIMER), NVIC_PRIORITY_SUB(NVIC_PRIO_TIMER)); diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c new file mode 100644 index 0000000000..6bdc63a298 --- /dev/null +++ b/src/main/drivers/timer_stm32f7xx.c @@ -0,0 +1,84 @@ +/* + modified version of StdPeriph function is located here. + TODO - what license does apply here? + original file was lincesed under MCD-ST Liberty SW License Agreement V2 + http://www.st.com/software_license_agreement_liberty_v2 +*/ + +#include "stm32f7xx.h" +#include "timer.h" +#include "rcc.h" + +/** + * @brief Selects the TIM Output Compare Mode. + * @note This function does NOT disable the selected channel before changing the Output + * Compare Mode. + * @param TIMx: where x can be 1 to 17 except 6 and 7 to select the TIM peripheral. + * @param TIM_Channel: specifies the TIM Channel + * This parameter can be one of the following values: + * @arg TIM_Channel_1: TIM Channel 1 + * @arg TIM_Channel_2: TIM Channel 2 + * @arg TIM_Channel_3: TIM Channel 3 + * @arg TIM_Channel_4: TIM Channel 4 + * @param TIM_OCMode: specifies the TIM Output Compare Mode. + * This parameter can be one of the following values: + * @arg TIM_OCMode_Timing + * @arg TIM_OCMode_Active + * @arg TIM_OCMode_Toggle + * @arg TIM_OCMode_PWM1 + * @arg TIM_OCMode_PWM2 + * @arg TIM_ForcedAction_Active + * @arg TIM_ForcedAction_InActive + * @retval None + */ + +#define CCMR_Offset ((uint16_t)0x0018) + +const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF1_TIM1 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF1_TIM2 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF2_TIM3 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF2_TIM4 }, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF2_TIM5 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF3_TIM8 }, + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF3_TIM9 }, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), GPIO_AF3_TIM10 }, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), GPIO_AF3_TIM11 }, + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), GPIO_AF9_TIM12 }, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), GPIO_AF9_TIM13 }, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), GPIO_AF9_TIM14 }, +}; + +void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) +{ + uint32_t tmp = 0; + + /* Check the parameters */ + assert_param(IS_TIM_LIST8_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); + + tmp = (uint32_t) TIMx; + tmp += CCMR_Offset; + + if((TIM_Channel == TIM_CHANNEL_1) ||(TIM_Channel == TIM_CHANNEL_3)) { + tmp += (TIM_Channel>>1); + + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); + + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } else { + tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1; + + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); + + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); + } +} + diff --git a/src/main/drivers/timer_stm32f7xx.h b/src/main/drivers/timer_stm32f7xx.h new file mode 100644 index 0000000000..286291aab8 --- /dev/null +++ b/src/main/drivers/timer_stm32f7xx.h @@ -0,0 +1,6 @@ + +#pragma once + +#include "stm32f7xx.h" + +void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode); diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 1c83169bfe..ad81178c00 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -139,18 +139,18 @@ #define CURRENT_METER_ADC_PIN PC1 #define RSSI_ADC_GPIO_PIN PC2 -//#define LED_STRIP +#define LED_STRIP // LED Strip can run off Pin 6 (PA0) of the ESC outputs. -#define WS2811_PIN PA0 +#define WS2811_PIN PA1 #define WS2811_TIMER TIM5 -#define WS2811_TIMER_CHANNEL TIM_Channel_1 -#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER -#define WS2811_DMA_STREAM DMA1_Stream2 -#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 -#define WS2811_DMA_IT DMA_IT_TCIF2 +#define WS2811_TIMER_CHANNEL TIM_CHANNEL_2 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST4_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream4 +#define WS2811_DMA_FLAG DMA_FLAG_TCIF4 +#define WS2811_DMA_IT DMA_IT_TCIF4 #define WS2811_DMA_CHANNEL DMA_CHANNEL_6 -#define WS2811_DMA_IRQ DMA1_Stream2_IRQn +#define WS2811_DMA_IRQ DMA1_Stream4_IRQn #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/ANYFCF7/target.mk b/src/main/target/ANYFCF7/target.mk index 7aa0aaecd4..158958769f 100644 --- a/src/main/target/ANYFCF7/target.mk +++ b/src/main/target/ANYFCF7/target.mk @@ -3,6 +3,7 @@ FEATURES += SDCARD VCP TARGET_SRC = \ drivers/accgyro_spi_mpu6000.c \ - drivers/barometer_ms5611.c - + drivers/barometer_ms5611.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_hal.c