Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

dshot: handle dcache, flush cache before DMA transfer

Browse Source
This commit is contained in:
Beat Küng
2019-09-02 15:41:59 +02:00
parent 3234aca53b
commit 23dbd9426a
1 changed files with 16 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c
+16 -2
View File
@@ -38,6 +38,7 @@
#else #else
#include <px4_config.h> #include <px4_config.h>
#include <px4_micro_hal.h>
#include <stm32_dma.h> #include <stm32_dma.h>
#include <stm32_tim.h> #include <stm32_tim.h>
#include <px4_arch/dshot.h> #include <px4_arch/dshot.h>
@@ -116,9 +117,13 @@ typedef struct dshot_handler_t {
uint8_t motors_number; uint8_t motors_number;
} dshot_handler_t; } dshot_handler_t;
#define DMA_BUFFER_MASK (PX4_ARCH_DCACHE_LINESIZE - 1)
#define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK)
static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {};
static uint16_t motor_buffer[MOTORS_NUMBER][ONE_MOTOR_BUFF_SIZE] = {}; static uint16_t motor_buffer[MOTORS_NUMBER][ONE_MOTOR_BUFF_SIZE] = {};
static uint32_t dshot_burst_buffer[DSHOT_TIMERS][ALL_MOTORS_BUF_SIZE] = {}; // TODO: DMA-able memory static uint8_t dshot_burst_buffer[DSHOT_TIMERS][DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE)] __attribute__((
aligned(PX4_ARCH_DCACHE_LINESIZE))) = {};
#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT #ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT
static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNMENT; static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNMENT;
@@ -270,6 +275,10 @@ void up_dshot_trigger(void)
dshot_dmar_data_prepare(timer, first_motor, dshot_handler[timer].motors_number); dshot_dmar_data_prepare(timer, first_motor, dshot_handler[timer].motors_number);
// Flush cache so DMA sees the data
up_clean_dcache((uintptr_t)dshot_burst_buffer[timer],
(uintptr_t)dshot_burst_buffer[timer] + DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE));
first_motor += dshot_handler[timer].motors_number; first_motor += dshot_handler[timer].motors_number;
uint32_t dshot_data_size = dshot_handler[timer].motors_number * ONE_MOTOR_BUFF_SIZE; uint32_t dshot_data_size = dshot_handler[timer].motors_number * ONE_MOTOR_BUFF_SIZE;
@@ -381,9 +390,14 @@ void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry)
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)
{ {
// we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size
#pragma GCC diagnostic ignored "-Wcast-align"
uint32_t *buffer = (uint32_t *)dshot_burst_buffer[timer];
#pragma GCC diagnostic pop
for (uint32_t motor_data_index = 0; motor_data_index < ONE_MOTOR_BUFF_SIZE ; motor_data_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++) { for (uint32_t motor_index = 0; motor_index < motors_number; motor_index++) {
dshot_burst_buffer[timer][motor_data_index * motors_number + motor_index] = motor_buffer[motor_index + buffer[motor_data_index * motors_number + motor_index] = motor_buffer[motor_index +
first_motor][motor_data_index]; first_motor][motor_data_index];
} }
} }