zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

DShot: updated driver to use NuttX DMA functions (#13233)

sbg
Igor Mišić 5 years ago committed by Beat Küng
parent
7ebaf9a1cb
commit
f4ee9141cb
11 changed files with 79 additions and 307 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      boards/holybro/kakutef7/src/timer_config.c
  2. 5
      boards/modalai/fc-v1/src/timer_config.c
  3. 10
      boards/omnibus/f4sd/src/timer_config.c
  4. 10
      boards/px4/fmu-v2/src/timer_config.c
  5. 10
      boards/px4/fmu-v3/src/timer_config.c
  6. 13
      boards/px4/fmu-v4/src/timer_config.c
  7. 5
      boards/px4/fmu-v5/src/timer_config.c
  8. 251
      platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
  9. 49
      platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h
  10. 2
      platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
  11. 11
      platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c

20
boards/holybro/kakutef7/src/timer_config.c
Unescape View File

@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler0, .handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM3, .vectorno = STM32_IRQ_TIM3,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA1_BASE, .dma_base = STM32_DMA1_BASE,
.channel = DShot_Channel5, .dmamap = DMAMAP_TIM3_UP,
.stream = DShot_Stream2,
.start_ccr_register = TIM_DMABASE_CCR3, .start_ccr_register = TIM_DMABASE_CCR3,
.channels_number = 2u /* CCR3 and CCR4 */ .channels_number = 2u /* CCR3 and CCR4 */
} }
@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler1, .handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM1CC, .vectorno = STM32_IRQ_TIM1CC,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA2_BASE, .dma_base = STM32_DMA2_BASE,
.channel = DShot_Channel6, .dmamap = DMAMAP_TIM1_UP,
.stream = DShot_Stream5,
.start_ccr_register = TIM_DMABASE_CCR1, .start_ccr_register = TIM_DMABASE_CCR1,
.channels_number = 2u /* CCR1 and CCR2 */ .channels_number = 2u /* CCR1 and CCR2 */
} }
@ -95,9 +93,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler2, .handler = io_timer_handler2,
.vectorno = STM32_IRQ_TIM8CC, .vectorno = STM32_IRQ_TIM8CC,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA2_BASE, .dma_base = STM32_DMA2_BASE,
.channel = DShot_Channel7, .dmamap = DMAMAP_TIM8_UP,
.stream = DShot_Stream1,
.start_ccr_register = TIM_DMABASE_CCR4, .start_ccr_register = TIM_DMABASE_CCR4,
.channels_number = 1u /* CCR4 */ .channels_number = 1u /* CCR4 */
} }
@ -112,9 +109,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler3, .handler = io_timer_handler3,
.vectorno = STM32_IRQ_TIM5, .vectorno = STM32_IRQ_TIM5,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA1_BASE, .dma_base = STM32_DMA1_BASE,
.channel = DShot_Channel6, .dmamap = DMAMAP_TIM5_UP_2,
.stream = DShot_Stream6, // alternatively DShot_Stream0
.start_ccr_register = TIM_DMABASE_CCR4, .start_ccr_register = TIM_DMABASE_CCR4,
.channels_number = 1u /* CCR4 */ .channels_number = 1u /* CCR4 */
} }

5
boards/modalai/fc-v1/src/timer_config.c
Unescape View File

@ -76,9 +76,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler0, .handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM1CC, .vectorno = STM32_IRQ_TIM1CC,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA2_BASE, .dma_base = STM32_DMA2_BASE,
.channel = DShot_Channel6, .dmamap = DMAMAP_TIM1_UP,
.stream = DShot_Stream5,
.start_ccr_register = TIM_DMABASE_CCR1, .start_ccr_register = TIM_DMABASE_CCR1,
.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */
} }

10
boards/omnibus/f4sd/src/timer_config.c
Unescape View File

@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler1, .handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM2, .vectorno = STM32_IRQ_TIM2,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA1_BASE, .dma_base = STM32_DMA1_BASE,
.channel = DShot_Channel3, .dmamap = DMAMAP_TIM2_UP_1,
.stream = DShot_Stream1,
.start_ccr_register = TIM_DMABASE_CCR3, .start_ccr_register = TIM_DMABASE_CCR3,
.channels_number = 2u /* CCR3 and CCR4 */ .channels_number = 2u /* CCR3 and CCR4 */
} }
@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler2, .handler = io_timer_handler2,
.vectorno = STM32_IRQ_TIM3, .vectorno = STM32_IRQ_TIM3,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA1_BASE, .dma_base = STM32_DMA1_BASE,
.channel = DShot_Channel5, .dmamap = DMAMAP_TIM3_UP,
.stream = DShot_Stream2,
.start_ccr_register = TIM_DMABASE_CCR3, .start_ccr_register = TIM_DMABASE_CCR3,
.channels_number = 2u /* CCR3 and CCR4 */ .channels_number = 2u /* CCR3 and CCR4 */
} }

10
boards/px4/fmu-v2/src/timer_config.c
Unescape View File

@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler0, .handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM1CC, .vectorno = STM32_IRQ_TIM1CC,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA2_BASE, .dma_base = STM32_DMA2_BASE,
.channel = DShot_Channel6, .dmamap = DMAMAP_TIM1_UP,
.stream = DShot_Stream5,
.start_ccr_register = TIM_DMABASE_CCR1, .start_ccr_register = TIM_DMABASE_CCR1,
.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ .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, .handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM4, .vectorno = STM32_IRQ_TIM4,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA1_BASE, .dma_base = STM32_DMA1_BASE,
.channel = DShot_Channel2, .dmamap = DMAMAP_TIM4_UP,
.stream = DShot_Stream6,
.start_ccr_register = TIM_DMABASE_CCR2, .start_ccr_register = TIM_DMABASE_CCR2,
.channels_number = 2u /* CCR2 and CCR3 */ .channels_number = 2u /* CCR2 and CCR3 */
} }

10
boards/px4/fmu-v3/src/timer_config.c
Unescape View File

@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler0, .handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM1CC, .vectorno = STM32_IRQ_TIM1CC,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA2_BASE, .dma_base = STM32_DMA2_BASE,
.channel = DShot_Channel6, .dmamap = DMAMAP_TIM1_UP,
.stream = DShot_Stream5,
.start_ccr_register = TIM_DMABASE_CCR1, .start_ccr_register = TIM_DMABASE_CCR1,
.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ .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, .handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM4, .vectorno = STM32_IRQ_TIM4,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA1_BASE, .dma_base = STM32_DMA1_BASE,
.channel = DShot_Channel2, .dmamap = DMAMAP_TIM4_UP,
.stream = DShot_Stream6,
.start_ccr_register = TIM_DMABASE_CCR2, .start_ccr_register = TIM_DMABASE_CCR2,
.channels_number = 2u /* CCR2 and CCR3 */ .channels_number = 2u /* CCR2 and CCR3 */
} }

13
boards/px4/fmu-v4/src/timer_config.c
Unescape View File

@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler0, .handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM1CC, .vectorno = STM32_IRQ_TIM1CC,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA2_BASE, .dma_base = STM32_DMA2_BASE,
.channel = DShot_Channel6, .dmamap = DMAMAP_TIM1_UP,
.stream = DShot_Stream5,
.start_ccr_register = TIM_DMABASE_CCR1, .start_ccr_register = TIM_DMABASE_CCR1,
.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ .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, .handler = io_timer_handler1,
.vectorno = STM32_IRQ_TIM4, .vectorno = STM32_IRQ_TIM4,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA1_BASE, .dma_base = STM32_DMA1_BASE,
.channel = DShot_Channel2, .dmamap = DMAMAP_TIM4_UP,
.stream = DShot_Stream6,
.start_ccr_register = TIM_DMABASE_CCR2, .start_ccr_register = TIM_DMABASE_CCR2,
.channels_number = 2u /* CCR2 and CCR3 */ .channels_number = 2u // CCR2 and CCR3
} }
} }
}; };

5
boards/px4/fmu-v5/src/timer_config.c
Unescape View File

@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
.handler = io_timer_handler0, .handler = io_timer_handler0,
.vectorno = STM32_IRQ_TIM1CC, .vectorno = STM32_IRQ_TIM1CC,
.dshot = { .dshot = {
.dma_base = DSHOT_DMA2_BASE, .dma_base = STM32_DMA2_BASE,
.channel = DShot_Channel6, .dmamap = DMAMAP_TIM1_UP,
.stream = DShot_Stream5,
.start_ccr_register = TIM_DMABASE_CCR1, .start_ccr_register = TIM_DMABASE_CCR1,
.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */
} }

251
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
Unescape View File

@ -46,76 +46,27 @@
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#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_1 14u
#define MOTOR_PWM_BIT_0 7u #define MOTOR_PWM_BIT_0 7u
#define DSHOT_TIMERS MAX_IO_TIMERS #define DSHOT_TIMERS MAX_IO_TIMERS
#define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS #define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS
#define ONE_MOTOR_DATA_SIZE 16u #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 ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * ONE_MOTOR_BUFF_SIZE)
#define DSHOT_THROTTLE_POSITION 5u #define DSHOT_THROTTLE_POSITION 5u
#define DSHOT_TELEMETRY_POSITION 4u #define DSHOT_TELEMETRY_POSITION 4u
#define NIBBLES_SIZE 4u #define NIBBLES_SIZE 4u
#define DSHOT_NUMBER_OF_NIBBLES 3u #define DSHOT_NUMBER_OF_NIBBLES 3u
#define DSHOT_END_OF_STREAM 16u
#define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 #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 { typedef struct dshot_handler_t {
bool init; bool init;
uint8_t motors_number; DMA_HANDLE dma_handle;
uint32_t dma_size;
} dshot_handler_t; } dshot_handler_t;
#define DMA_BUFFER_MASK (PX4_ARCH_DCACHE_LINESIZE - 1) #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 */ #endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */
void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number); 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) int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
{ {
@ -193,96 +143,15 @@ 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++) { for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) {
if (true == dshot_handler[timer_index].init) { if (true == dshot_handler[timer_index].init) {
dshot_handler[timer_index].motors_number = io_timers[timer_index].dshot.channels_number; 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, dshot_handler[timer_index].motors_number); io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, io_timers[timer_index].dshot.channels_number);
ret_val = dshot_setup_stream_registers(timer_index);
}
}
return ret_val;
}
int dshot_setup_stream_registers(uint32_t timer) dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap);
{
int ret_val = OK;
switch (io_timers[timer].dshot.stream) { if (NULL == dshot_handler[timer_index].dma_handle) {
case DShot_Stream0: ret_val = ERROR;
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; return ret_val;
@ -296,76 +165,7 @@ void up_dshot_trigger(void)
if (true == dshot_handler[timer].init) { if (true == dshot_handler[timer].init) {
uint32_t dma_int_streamx_mask; uint8_t motors_number = io_timers[timer].dshot.channels_number;
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;
dshot_dmar_data_prepare(timer, first_motor, motors_number); dshot_dmar_data_prepare(timer, first_motor, motors_number);
// Flush cache so DMA sees the data // Flush cache so DMA sees the data
@ -374,11 +174,18 @@ void up_dshot_trigger(void)
first_motor += motors_number; first_motor += motors_number;
uint32_t dshot_data_size = motors_number * ONE_MOTOR_BUFF_SIZE; stm32_dmasetup(dshot_handler[timer].dma_handle,
*rxIFCR |= dma_int_streamx_mask; //clear interrupt flags io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
*rSxNDTR = dshot_data_size; (uint32_t)(dshot_burst_buffer[timer]),
io_timer_update_generation(timer); dshot_handler[timer].dma_size,
*rSxCR |= DMA_SCR_EN; // Trigger DMA 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; packet <<= 1;
} }
motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + 16] = 0; motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_END_OF_STREAM] = 0;
motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + 17] = 0;
} }
void up_dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry) 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); return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS);
} }
#endif #endif

49
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h
Unescape View File

@ -35,53 +35,30 @@
#pragma once #pragma once
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <stm32_dma.h>
#define DSHOT_MOTOR_PWM_BIT_WIDTH 20u #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. /* Configuration for each timer to setup DShot. Some timers have only one while others have two choices for the stream.
* *
* TIM1UP - DMA2, Channel6, Stream5 * DMAMAP_TIM1_UP - DMA2, Channel6, Stream5
* TIM2UP - DMA1, Channel3, Stream1 or Stream7 * DMAMAP_TIM2_UP_1 - DMA1, Channel3, Stream1
* TIM3UP - DMA1, Channel5, Stream2 * DMAMAP_TIM2_UP_2 - DMA1, Channel3, Stream7
* TIM4UP - DMA1, Channel2, Stream6 * DMAMAP_TIM3_UP - DMA1, Channel5, Stream2
* TIM5UP - DMA1, Channel6, Stream0 or Stream6 * DMAMAP_TIM4_UP - DMA1, Channel2, Stream6
* TIM6UP - DMA1, Channel7, Stream1 * DMAMAP_TIM5_UP_1 - DMA1, Channel6, Stream0
* TIM7UP - DMA1, Channel1, Stream2 or Stream4 * DMAMAP_TIM5_UP_2 - DMA1, Channel6, Stream6
* TIM8UP - DMA2, Channel7, Stream1 * 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 /* The structure which contains configuration for DShot
*/ */
typedef struct dshot_conf_t { typedef struct dshot_conf_t {
uint32_t dma_base; uint32_t dma_base;
dshot_dma_channel_t channel; uint32_t dmamap;
dshot_dma_stream_t stream;
uint32_t start_ccr_register; uint32_t start_ccr_register;
uint8_t channels_number; ///< number of channels/outputs (<=MAX_NUM_CHANNELS_PER_TIMER) uint8_t channels_number; ///< number of channels/outputs (<=MAX_NUM_CHANNELS_PER_TIMER)
} dshot_conf_t; } dshot_conf_t;

2
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
Unescape View File

@ -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_channel_mode(unsigned channel);
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
__EXPORT extern void io_timer_trigger(void); __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); __EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);

11
platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c
Unescape View File

@ -492,10 +492,14 @@ static inline void io_timer_set_oneshot_mode(unsigned timer)
rEGR(timer) = GTIM_EGR_UG; 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 if (enable) {
rEGR(timer) = ATIM_EGR_UG; 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) 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; rPSC(timer) = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1;
rEGR(timer) = ATIM_EGR_UG; rEGR(timer) = ATIM_EGR_UG;
rDCR(timer) = (io_timers[timer].dshot.start_ccr_register | tim_dma_burst_length); rDCR(timer) = (io_timers[timer].dshot.start_ccr_register | tim_dma_burst_length);
rDIER(timer) = ATIM_DIER_UDE;
} }
return ret_val; return ret_val;

Write Preview
Loading…
Cancel
Save