dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Beat Küng 2021-08-16 13:30:56 +02:00 committed by Daniel Agar
parent afed10618b
commit f00f3d1a27
2 changed files with 12 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/omnibus/f4sd/src/board_config.h
View File

@ -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

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

@ -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;
}
}