diff --git a/boards/holybro/kakutef7/src/timer_config.c b/boards/holybro/kakutef7/src/timer_config.c index bbe5680bfc..ce511a0003 100644 --- a/boards/holybro/kakutef7/src/timer_config.c +++ b/boards/holybro/kakutef7/src/timer_config.c @@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM3, .dshot = { - .dma_base = DSHOT_DMA1_BASE, - .channel = DShot_Channel5, - .stream = DShot_Stream2, + .dma_base = STM32_DMA1_BASE, + .dmamap = DMAMAP_TIM3_UP, .start_ccr_register = TIM_DMABASE_CCR3, .channels_number = 2u /* CCR3 and CCR4 */ } @@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler1, .vectorno = STM32_IRQ_TIM1CC, .dshot = { - .dma_base = DSHOT_DMA2_BASE, - .channel = DShot_Channel6, - .stream = DShot_Stream5, + .dma_base = STM32_DMA2_BASE, + .dmamap = DMAMAP_TIM1_UP, .start_ccr_register = TIM_DMABASE_CCR1, .channels_number = 2u /* CCR1 and CCR2 */ } @@ -95,9 +93,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler2, .vectorno = STM32_IRQ_TIM8CC, .dshot = { - .dma_base = DSHOT_DMA2_BASE, - .channel = DShot_Channel7, - .stream = DShot_Stream1, + .dma_base = STM32_DMA2_BASE, + .dmamap = DMAMAP_TIM8_UP, .start_ccr_register = TIM_DMABASE_CCR4, .channels_number = 1u /* CCR4 */ } @@ -112,9 +109,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler3, .vectorno = STM32_IRQ_TIM5, .dshot = { - .dma_base = DSHOT_DMA1_BASE, - .channel = DShot_Channel6, - .stream = DShot_Stream6, // alternatively DShot_Stream0 + .dma_base = STM32_DMA1_BASE, + .dmamap = DMAMAP_TIM5_UP_2, .start_ccr_register = TIM_DMABASE_CCR4, .channels_number = 1u /* CCR4 */ } diff --git a/boards/modalai/fc-v1/src/timer_config.c b/boards/modalai/fc-v1/src/timer_config.c index c7766425d0..cc05d34010 100644 --- a/boards/modalai/fc-v1/src/timer_config.c +++ b/boards/modalai/fc-v1/src/timer_config.c @@ -76,9 +76,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, .dshot = { - .dma_base = DSHOT_DMA2_BASE, - .channel = DShot_Channel6, - .stream = DShot_Stream5, + .dma_base = STM32_DMA2_BASE, + .dmamap = DMAMAP_TIM1_UP, .start_ccr_register = TIM_DMABASE_CCR1, .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ } diff --git a/boards/omnibus/f4sd/src/timer_config.c b/boards/omnibus/f4sd/src/timer_config.c index cbfae03556..7b99c523d9 100644 --- a/boards/omnibus/f4sd/src/timer_config.c +++ b/boards/omnibus/f4sd/src/timer_config.c @@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler1, .vectorno = STM32_IRQ_TIM2, .dshot = { - .dma_base = DSHOT_DMA1_BASE, - .channel = DShot_Channel3, - .stream = DShot_Stream1, + .dma_base = STM32_DMA1_BASE, + .dmamap = DMAMAP_TIM2_UP_1, .start_ccr_register = TIM_DMABASE_CCR3, .channels_number = 2u /* CCR3 and CCR4 */ } @@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler2, .vectorno = STM32_IRQ_TIM3, .dshot = { - .dma_base = DSHOT_DMA1_BASE, - .channel = DShot_Channel5, - .stream = DShot_Stream2, + .dma_base = STM32_DMA1_BASE, + .dmamap = DMAMAP_TIM3_UP, .start_ccr_register = TIM_DMABASE_CCR3, .channels_number = 2u /* CCR3 and CCR4 */ } diff --git a/boards/px4/fmu-v2/src/timer_config.c b/boards/px4/fmu-v2/src/timer_config.c index 4d9f7e8524..4b47bd3b5c 100644 --- a/boards/px4/fmu-v2/src/timer_config.c +++ b/boards/px4/fmu-v2/src/timer_config.c @@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, .dshot = { - .dma_base = DSHOT_DMA2_BASE, - .channel = DShot_Channel6, - .stream = DShot_Stream5, + .dma_base = STM32_DMA2_BASE, + .dmamap = DMAMAP_TIM1_UP, .start_ccr_register = TIM_DMABASE_CCR1, .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ } @@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler1, .vectorno = STM32_IRQ_TIM4, .dshot = { - .dma_base = DSHOT_DMA1_BASE, - .channel = DShot_Channel2, - .stream = DShot_Stream6, + .dma_base = STM32_DMA1_BASE, + .dmamap = DMAMAP_TIM4_UP, .start_ccr_register = TIM_DMABASE_CCR2, .channels_number = 2u /* CCR2 and CCR3 */ } diff --git a/boards/px4/fmu-v3/src/timer_config.c b/boards/px4/fmu-v3/src/timer_config.c index 4d9f7e8524..4b47bd3b5c 100644 --- a/boards/px4/fmu-v3/src/timer_config.c +++ b/boards/px4/fmu-v3/src/timer_config.c @@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, .dshot = { - .dma_base = DSHOT_DMA2_BASE, - .channel = DShot_Channel6, - .stream = DShot_Stream5, + .dma_base = STM32_DMA2_BASE, + .dmamap = DMAMAP_TIM1_UP, .start_ccr_register = TIM_DMABASE_CCR1, .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ } @@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler1, .vectorno = STM32_IRQ_TIM4, .dshot = { - .dma_base = DSHOT_DMA1_BASE, - .channel = DShot_Channel2, - .stream = DShot_Stream6, + .dma_base = STM32_DMA1_BASE, + .dmamap = DMAMAP_TIM4_UP, .start_ccr_register = TIM_DMABASE_CCR2, .channels_number = 2u /* CCR2 and CCR3 */ } diff --git a/boards/px4/fmu-v4/src/timer_config.c b/boards/px4/fmu-v4/src/timer_config.c index 97988c3434..f7f1d61ece 100644 --- a/boards/px4/fmu-v4/src/timer_config.c +++ b/boards/px4/fmu-v4/src/timer_config.c @@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, .dshot = { - .dma_base = DSHOT_DMA2_BASE, - .channel = DShot_Channel6, - .stream = DShot_Stream5, + .dma_base = STM32_DMA2_BASE, + .dmamap = DMAMAP_TIM1_UP, .start_ccr_register = TIM_DMABASE_CCR1, .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ } @@ -79,12 +78,12 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler1, .vectorno = STM32_IRQ_TIM4, .dshot = { - .dma_base = DSHOT_DMA1_BASE, - .channel = DShot_Channel2, - .stream = DShot_Stream6, + .dma_base = STM32_DMA1_BASE, + .dmamap = DMAMAP_TIM4_UP, .start_ccr_register = TIM_DMABASE_CCR2, - .channels_number = 2u /* CCR2 and CCR3 */ + .channels_number = 2u // CCR2 and CCR3 } + } }; diff --git a/boards/px4/fmu-v5/src/timer_config.c b/boards/px4/fmu-v5/src/timer_config.c index cde6354cff..37f57f08a0 100644 --- a/boards/px4/fmu-v5/src/timer_config.c +++ b/boards/px4/fmu-v5/src/timer_config.c @@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, .dshot = { - .dma_base = DSHOT_DMA2_BASE, - .channel = DShot_Channel6, - .stream = DShot_Stream5, + .dma_base = STM32_DMA2_BASE, + .dmamap = DMAMAP_TIM1_UP, .start_ccr_register = TIM_DMABASE_CCR1, .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index ec15036d82..3e553b3b35 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -46,76 +46,27 @@ #include -#define REG(_tmr, _reg) (*(volatile uint32_t *)(io_timers[_tmr].dshot.dma_base + _reg)) - -/* DMA registers */ -#define rLIFCR(_tmr) REG(_tmr, STM32_DMA_LIFCR_OFFSET) -#define rHIFCR(_tmr) REG(_tmr, STM32_DMA_HIFCR_OFFSET) - -#define rS0CR(_tmr) REG(_tmr, STM32_DMA_S0CR_OFFSET) -#define rS0NDTR(_tmr) REG(_tmr, STM32_DMA_S0NDTR_OFFSET) -#define rS0PAR(_tmr) REG(_tmr, STM32_DMA_S0PAR_OFFSET) -#define rS0M0AR(_tmr) REG(_tmr, STM32_DMA_S0M0AR_OFFSET) -#define rS0FCR(_tmr) REG(_tmr, STM32_DMA_S0FCR_OFFSET) - -#define rS1CR(_tmr) REG(_tmr, STM32_DMA_S1CR_OFFSET) -#define rS1NDTR(_tmr) REG(_tmr, STM32_DMA_S1NDTR_OFFSET) -#define rS1PAR(_tmr) REG(_tmr, STM32_DMA_S1PAR_OFFSET) -#define rS1M0AR(_tmr) REG(_tmr, STM32_DMA_S1M0AR_OFFSET) -#define rS1FCR(_tmr) REG(_tmr, STM32_DMA_S1FCR_OFFSET) - -#define rS2CR(_tmr) REG(_tmr, STM32_DMA_S2CR_OFFSET) -#define rS2NDTR(_tmr) REG(_tmr, STM32_DMA_S2NDTR_OFFSET) -#define rS2PAR(_tmr) REG(_tmr, STM32_DMA_S2PAR_OFFSET) -#define rS2M0AR(_tmr) REG(_tmr, STM32_DMA_S2M0AR_OFFSET) -#define rS2FCR(_tmr) REG(_tmr, STM32_DMA_S2FCR_OFFSET) - -#define rS3CR(_tmr) REG(_tmr, STM32_DMA_S3CR_OFFSET) -#define rS3NDTR(_tmr) REG(_tmr, STM32_DMA_S3NDTR_OFFSET) -#define rS3PAR(_tmr) REG(_tmr, STM32_DMA_S3PAR_OFFSET) -#define rS3M0AR(_tmr) REG(_tmr, STM32_DMA_S3M0AR_OFFSET) -#define rS3FCR(_tmr) REG(_tmr, STM32_DMA_S3FCR_OFFSET) - -#define rS4CR(_tmr) REG(_tmr, STM32_DMA_S4CR_OFFSET) -#define rS4NDTR(_tmr) REG(_tmr, STM32_DMA_S4NDTR_OFFSET) -#define rS4PAR(_tmr) REG(_tmr, STM32_DMA_S4PAR_OFFSET) -#define rS4M0AR(_tmr) REG(_tmr, STM32_DMA_S4M0AR_OFFSET) -#define rS4FCR(_tmr) REG(_tmr, STM32_DMA_S4FCR_OFFSET) - -#define rS5CR(_tmr) REG(_tmr, STM32_DMA_S5CR_OFFSET) -#define rS5NDTR(_tmr) REG(_tmr, STM32_DMA_S5NDTR_OFFSET) -#define rS5PAR(_tmr) REG(_tmr, STM32_DMA_S5PAR_OFFSET) -#define rS5M0AR(_tmr) REG(_tmr, STM32_DMA_S5M0AR_OFFSET) -#define rS5FCR(_tmr) REG(_tmr, STM32_DMA_S5FCR_OFFSET) - -#define rS6CR(_tmr) REG(_tmr, STM32_DMA_S6CR_OFFSET) -#define rS6NDTR(_tmr) REG(_tmr, STM32_DMA_S6NDTR_OFFSET) -#define rS6PAR(_tmr) REG(_tmr, STM32_DMA_S6PAR_OFFSET) -#define rS6M0AR(_tmr) REG(_tmr, STM32_DMA_S6M0AR_OFFSET) -#define rS6FCR(_tmr) REG(_tmr, STM32_DMA_S6FCR_OFFSET) - -#define rS7CR(_tmr) REG(_tmr, STM32_DMA_S7CR_OFFSET) -#define rS7NDTR(_tmr) REG(_tmr, STM32_DMA_S7NDTR_OFFSET) -#define rS7PAR(_tmr) REG(_tmr, STM32_DMA_S7PAR_OFFSET) -#define rS7M0AR(_tmr) REG(_tmr, STM32_DMA_S7M0AR_OFFSET) -#define rS7FCR(_tmr) REG(_tmr, STM32_DMA_S7FCR_OFFSET) - #define MOTOR_PWM_BIT_1 14u #define MOTOR_PWM_BIT_0 7u #define DSHOT_TIMERS MAX_IO_TIMERS #define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS #define ONE_MOTOR_DATA_SIZE 16u -#define ONE_MOTOR_BUFF_SIZE 18u +#define ONE_MOTOR_BUFF_SIZE 17u #define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * ONE_MOTOR_BUFF_SIZE) #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u #define DSHOT_NUMBER_OF_NIBBLES 3u +#define DSHOT_END_OF_STREAM 16u #define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 +#define DSHOT_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | \ + DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE) + typedef struct dshot_handler_t { bool init; - uint8_t motors_number; + DMA_HANDLE dma_handle; + uint32_t dma_size; } dshot_handler_t; #define DMA_BUFFER_MASK (PX4_ARCH_DCACHE_LINESIZE - 1) @@ -133,7 +84,6 @@ static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNM #endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */ void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number); -int dshot_setup_stream_registers(uint32_t timer); int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { @@ -193,101 +143,20 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) { if (true == dshot_handler[timer_index].init) { - dshot_handler[timer_index].motors_number = io_timers[timer_index].dshot.channels_number; - io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, dshot_handler[timer_index].motors_number); - ret_val = dshot_setup_stream_registers(timer_index); + dshot_handler[timer_index].dma_size = io_timers[timer_index].dshot.channels_number * ONE_MOTOR_BUFF_SIZE; + io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, io_timers[timer_index].dshot.channels_number); + + dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); + + if (NULL == dshot_handler[timer_index].dma_handle) { + ret_val = ERROR; + } } } return ret_val; } -int dshot_setup_stream_registers(uint32_t timer) -{ - int ret_val = OK; - - switch (io_timers[timer].dshot.stream) { - case DShot_Stream0: - rS0CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS0CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS0CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS0PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS0M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS0FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - case DShot_Stream1: - rS1CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS1CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS1CR(timer) |= DMA_SCR_DIR_M2P; - rS1CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS1PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS1M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS1FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - case DShot_Stream2: - rS2CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS2CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS2CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS2PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS2M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS2FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - case DShot_Stream3: - rS3CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS3CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS3CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS3PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS3M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS3FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - case DShot_Stream4: - rS4CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS4CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS4CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS4PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS4M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS4FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - case DShot_Stream5: - rS5CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS5CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS5CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS5PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS5M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS5FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - case DShot_Stream6: - rS6CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS6CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS6CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS6PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS6M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS6FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - case DShot_Stream7: - rS7CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); - rS7CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; - rS7CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; - rS7PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; - rS7M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); - rS7FCR(timer) &= 0x0; /* Disable FIFO */ - break; - - default: - ret_val = ERROR; - break; - } - - return ret_val; -} - void up_dshot_trigger(void) { uint8_t first_motor = 0; @@ -296,76 +165,7 @@ void up_dshot_trigger(void) if (true == dshot_handler[timer].init) { - uint32_t dma_int_streamx_mask; - volatile uint32_t *rSxNDTR; - volatile uint32_t *rSxCR; - volatile uint32_t *rxIFCR; - - switch (io_timers[timer].dshot.stream) { - case DShot_Stream0: - dma_int_streamx_mask = DMA_INT_STREAM0_MASK; - rxIFCR = &rLIFCR(timer); - rSxNDTR = &rS0NDTR(timer); - rSxCR = &rS0CR(timer); - break; - - case DShot_Stream1: - dma_int_streamx_mask = DMA_INT_STREAM1_MASK; - rxIFCR = &rLIFCR(timer); - rSxNDTR = &rS1NDTR(timer); - rSxCR = &rS1CR(timer); - break; - - case DShot_Stream2: - dma_int_streamx_mask = DMA_INT_STREAM2_MASK; - rxIFCR = &rLIFCR(timer); - rSxNDTR = &rS2NDTR(timer); - rSxCR = &rS2CR(timer); - break; - - case DShot_Stream3: - dma_int_streamx_mask = DMA_INT_STREAM3_MASK; - rxIFCR = &rLIFCR(timer); - rSxNDTR = &rS3NDTR(timer); - rSxCR = &rS3CR(timer); - break; - - case DShot_Stream4: - dma_int_streamx_mask = DMA_INT_STREAM4_MASK; - rxIFCR = &rHIFCR(timer); - rSxNDTR = &rS4NDTR(timer); - rSxCR = &rS4CR(timer); - break; - - case DShot_Stream5: - dma_int_streamx_mask = DMA_INT_STREAM5_MASK; - rxIFCR = &rHIFCR(timer); - rSxNDTR = &rS5NDTR(timer); - rSxCR = &rS5CR(timer); - break; - - case DShot_Stream6: - dma_int_streamx_mask = DMA_INT_STREAM6_MASK; - rxIFCR = &rHIFCR(timer); - rSxNDTR = &rS6NDTR(timer); - rSxCR = &rS6CR(timer); - break; - - case DShot_Stream7: - default: - dma_int_streamx_mask = DMA_INT_STREAM7_MASK; - rxIFCR = &rHIFCR(timer); - rSxNDTR = &rS7NDTR(timer); - rSxCR = &rS7CR(timer); - break; - } - - // Check if DMA is still in progress (just to be safe, not expected to happen) - if (*rSxCR & DMA_SCR_EN) { - continue; - } - - uint8_t motors_number = dshot_handler[timer].motors_number; + uint8_t motors_number = io_timers[timer].dshot.channels_number; dshot_dmar_data_prepare(timer, first_motor, motors_number); // Flush cache so DMA sees the data @@ -374,11 +174,18 @@ void up_dshot_trigger(void) first_motor += motors_number; - uint32_t dshot_data_size = motors_number * ONE_MOTOR_BUFF_SIZE; - *rxIFCR |= dma_int_streamx_mask; //clear interrupt flags - *rSxNDTR = dshot_data_size; - io_timer_update_generation(timer); - *rSxCR |= DMA_SCR_EN; // Trigger DMA + stm32_dmasetup(dshot_handler[timer].dma_handle, + io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, + (uint32_t)(dshot_burst_buffer[timer]), + dshot_handler[timer].dma_size, + DSHOT_DMA_SCR); + + // Clean UDE flag before DMA is started + io_timer_update_dma_req(timer, false); + // Trigger DMA (DShot Outputs) + stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false); + io_timer_update_dma_req(timer, true); + } } } @@ -419,8 +226,7 @@ static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool packet <<= 1; } - motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + 16] = 0; - motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + 17] = 0; + motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_END_OF_STREAM] = 0; } void up_dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry) @@ -450,5 +256,4 @@ int up_dshot_arm(bool armed) return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); } - #endif diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h index a944bc65e3..a539adcfc2 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h @@ -35,53 +35,30 @@ #pragma once #include +#include #define DSHOT_MOTOR_PWM_BIT_WIDTH 20u /* Configuration for each timer to setup DShot. Some timers have only one while others have two choices for the stream. * - * TIM1UP - DMA2, Channel6, Stream5 - * TIM2UP - DMA1, Channel3, Stream1 or Stream7 - * TIM3UP - DMA1, Channel5, Stream2 - * TIM4UP - DMA1, Channel2, Stream6 - * TIM5UP - DMA1, Channel6, Stream0 or Stream6 - * TIM6UP - DMA1, Channel7, Stream1 - * TIM7UP - DMA1, Channel1, Stream2 or Stream4 - * TIM8UP - DMA2, Channel7, Stream1 + * DMAMAP_TIM1_UP - DMA2, Channel6, Stream5 + * DMAMAP_TIM2_UP_1 - DMA1, Channel3, Stream1 + * DMAMAP_TIM2_UP_2 - DMA1, Channel3, Stream7 + * DMAMAP_TIM3_UP - DMA1, Channel5, Stream2 + * DMAMAP_TIM4_UP - DMA1, Channel2, Stream6 + * DMAMAP_TIM5_UP_1 - DMA1, Channel6, Stream0 + * DMAMAP_TIM5_UP_2 - DMA1, Channel6, Stream6 + * DMAMAP_TIM6_UP - DMA1, Channel7, Stream1 + * DMAMAP_TIM7_UP_1 - DMA1, Channel1, Stream2 + * DMAMAP_TIM7_UP_2 - DMA1, Channel1, Stream4 + * DMAMAP_TIM8_UP - DMA2, Channel7, Stream1 */ -#define DSHOT_DMA1_BASE STM32_DMA1_BASE -#define DSHOT_DMA2_BASE STM32_DMA2_BASE - -typedef enum dshot_dma_channel_t { - DShot_Channel0 = 0u, - DShot_Channel1 = 1u, - DShot_Channel2 = 2u, - DShot_Channel3 = 3u, - DShot_Channel4 = 4u, - DShot_Channel5 = 5u, - DShot_Channel6 = 6u, - DShot_Channel7 = 7u -} dshot_dma_channel_t; - -typedef enum dshot_dma_stream_t { - DShot_Stream0 = 0u, - DShot_Stream1 = 1u, - DShot_Stream2 = 2u, - DShot_Stream3 = 3u, - DShot_Stream4 = 4u, - DShot_Stream5 = 5u, - DShot_Stream6 = 6u, - DShot_Stream7 = 7u -} dshot_dma_stream_t; - - /* The structure which contains configuration for DShot */ typedef struct dshot_conf_t { uint32_t dma_base; - dshot_dma_channel_t channel; - dshot_dma_stream_t stream; + uint32_t dmamap; uint32_t start_ccr_register; uint8_t channels_number; ///< number of channels/outputs (<=MAX_NUM_CHANNELS_PER_TIMER) } dshot_conf_t; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index a8a319a4a7..29f31e2026 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -143,7 +143,7 @@ __EXPORT int io_timer_free_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); __EXPORT extern void io_timer_trigger(void); -__EXPORT void io_timer_update_generation(uint8_t timer); +__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); __EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 4f6e0d3ef1..2fe5f8ffce 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -492,10 +492,14 @@ static inline void io_timer_set_oneshot_mode(unsigned timer) rEGR(timer) = GTIM_EGR_UG; } -void io_timer_update_generation(uint8_t timer) +void io_timer_update_dma_req(uint8_t timer, bool enable) { - // Re-initialize the counter and generate an update of the registers - rEGR(timer) = ATIM_EGR_UG; + if (enable) { + rDIER(timer) |= ATIM_DIER_UDE; + + } else { + rDIER(timer) &= ~ATIM_DIER_UDE; + } } int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) @@ -524,7 +528,6 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_ rPSC(timer) = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; rEGR(timer) = ATIM_EGR_UG; rDCR(timer) = (io_timers[timer].dshot.start_ccr_register | tim_dma_burst_length); - rDIER(timer) = ATIM_DIER_UDE; } return ret_val;