From f00f3d1a27df492e48c25b4d18a0040311f3ca40 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 16 Aug 2021 13:30:56 +0200 Subject: [PATCH] dshot: improve performance by removing extra motor_buffer array Reduces CPU load by almost 1.5% @ 2khz on F4 and F7. This changes the motor ordering on boards where the timer ordering does not match the order of the timer usage in the channels defintion. Only omnibus f4sd is affected. --- boards/omnibus/f4sd/src/board_config.h | 2 +- .../src/px4/stm/stm32_common/dshot/dshot.c | 51 ++++--------------- 2 files changed, 12 insertions(+), 41 deletions(-) diff --git a/boards/omnibus/f4sd/src/board_config.h b/boards/omnibus/f4sd/src/board_config.h index 782334daf2..f250828a54 100644 --- a/boards/omnibus/f4sd/src/board_config.h +++ b/boards/omnibus/f4sd/src/board_config.h @@ -157,7 +157,7 @@ #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {2, 3, 1, 0}; +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {0, 1, 3, 2}; __BEGIN_DECLS 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 8951f601af..6286c7959a 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -78,29 +78,16 @@ typedef struct dshot_handler_t { #define DSHOT_BURST_BUFFER_SIZE(motors_number) (DMA_ALIGN_UP(sizeof(uint32_t)*ONE_MOTOR_BUFF_SIZE*motors_number)) static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; -static uint16_t *motor_buffer = NULL; static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] -px4_cache_aligned_data(); +px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; #ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT static const uint8_t motor_assignment[MOTORS_NUMBER] = 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); - int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { - // Alloc buffers if they do not exist. We don't use channel_mask so that potential future re-init calls can - // use the same buffer. - if (!motor_buffer) { - motor_buffer = (uint16_t *)malloc(sizeof(uint16_t) * ALL_MOTORS_BUF_SIZE); - - if (!motor_buffer) { - return -ENOMEM; - } - } - unsigned buffer_offset = 0; for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { @@ -164,20 +151,14 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) void up_dshot_trigger(void) { - uint8_t first_motor = 0; - for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { if (true == dshot_handler[timer].init) { - uint8_t motors_number = io_timers_channel_mapping.element[timer].channel_count; - dshot_dmar_data_prepare(timer, first_motor, motors_number); - // Flush cache so DMA sees the data up_clean_dcache((uintptr_t)dshot_burst_buffer[timer], - (uintptr_t)dshot_burst_buffer[timer] + DSHOT_BURST_BUFFER_SIZE(motors_number)); - - first_motor += motors_number; + (uintptr_t)dshot_burst_buffer[timer] + + DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count)); px4_stm32_dmasetup(dshot_handler[timer].dma_handle, io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, @@ -212,37 +193,27 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet packet |= throttle << DSHOT_THROTTLE_POSITION; packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; - uint32_t i; uint16_t csum_data = packet; /* XOR checksum calculation */ csum_data >>= NIBBLES_SIZE; - for (i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { + for (unsigned i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { checksum ^= (csum_data & 0x0F); // XOR data by nibbles csum_data >>= NIBBLES_SIZE; } packet |= (checksum & 0x0F); - for (i = 0; i < ONE_MOTOR_DATA_SIZE; i++) { - motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + i] = (packet & 0x8000) ? MOTOR_PWM_BIT_1 : - MOTOR_PWM_BIT_0; // MSB first - packet <<= 1; - } - - motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_END_OF_STREAM] = 0; -} - -void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number) -{ + unsigned timer = timer_io_channels[motor_number].timer_index; uint32_t *buffer = dshot_burst_buffer[timer]; + unsigned num_motors = io_timers_channel_mapping.element[timer].channel_count; + unsigned timer_channel_index = motor_number - io_timers_channel_mapping.element[timer].first_channel_index; - for (uint32_t motor_data_index = 0; motor_data_index < ONE_MOTOR_BUFF_SIZE ; motor_data_index++) { - for (uint32_t motor_index = 0; motor_index < motors_number; motor_index++) { - buffer[motor_data_index * motors_number + motor_index] = motor_buffer[(motor_index + - first_motor) * ONE_MOTOR_BUFF_SIZE + motor_data_index]; - } + for (unsigned motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) { + buffer[motor_data_index * num_motors + timer_channel_index] = + (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first + packet <<= 1; } }