Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 15:04:38 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

dshot: reduce static buffer size

Browse Source 
And handle failures of up_dshot_init().

On Omnibus: reduces memory usage if dshot is enabled by ~1.0KB.
The buffer is roughly 1KB in size.
This commit is contained in:
Beat Küng
2019-09-07 14:23:32 +02:00
parent e78250ab8d
commit 182efaa757
12 changed files with 91 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h
+5 -1
View File
@@ -45,7 +45,11 @@
#pragma once
__BEGIN_DECLS
/* configuration limits */
#define MAX_IO_TIMERS 4
#ifdef BOARD_NUM_IO_TIMERS
#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS
#else
#define MAX_IO_TIMERS 2
#endif
#define MAX_TIMER_IO_CHANNELS 16
#define MAX_LED_TIMERS 2
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
+55 -28
View File
@@ -111,6 +111,7 @@
#define DSHOT_TELEMETRY_POSITION 4u
#define NIBBLES_SIZE 4u
#define DSHOT_NUMBER_OF_NIBBLES 3u
#define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4
typedef struct dshot_handler_t {
bool init;
@@ -119,11 +120,13 @@ typedef struct dshot_handler_t {
#define DMA_BUFFER_MASK (PX4_ARCH_DCACHE_LINESIZE - 1)
#define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK)
#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[MOTORS_NUMBER][ONE_MOTOR_BUFF_SIZE] = {};
static uint8_t dshot_burst_buffer[DSHOT_TIMERS][DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE)] __attribute__((
aligned(PX4_ARCH_DCACHE_LINESIZE))) = {};
static uint16_t *motor_buffer = NULL;
static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)]
__attribute__((aligned(PX4_ARCH_DCACHE_LINESIZE))); // DMA buffer
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;
@@ -134,11 +137,37 @@ int dshot_setup_stream_registers(uint32_t timer);
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
{
int ret_val = OK;
// 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);
uint8_t timer;
if (!motor_buffer) {
return -ENOMEM;
}
}
unsigned buffer_offset = 0;
for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
if (io_timers[timer].base == 0) { // no more timers configured
break;
}
// 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"
dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset];
#pragma GCC diagnostic pop
buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers[timer].dshot.channels_number);
if (buffer_offset > sizeof(dshot_burst_buffer_array)) {
return -EINVAL; // something is wrong with the board configuration or some other logic
}
}
/* Init channels */
int ret_val = OK;
for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) {
if (channel_mask & (1 << channel)) {
@@ -151,7 +180,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
if (OK == success) {
channel_mask &= ~(1 << channel);
timer = timer_io_channels[channel].timer_index;
uint8_t timer = timer_io_channels[channel].timer_index;
if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot
io_timer_free_channel(channel);
@@ -189,7 +218,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS0M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS0FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -199,7 +228,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS1M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS1FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -208,7 +237,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS2M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS2FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -217,7 +246,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS3M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS3FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -226,7 +255,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS4M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS4FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -235,7 +264,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS5M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS5FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -244,7 +273,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS6M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS6FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -253,7 +282,7 @@ int dshot_setup_stream_registers(uint32_t timer)
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]));
rS7M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]);
rS7FCR(timer) &= 0x0; /* Disable FIFO */
break;
@@ -272,16 +301,16 @@ void up_dshot_trigger(void)
for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) {
if (true == dshot_handler[timer].init) {
dshot_dmar_data_prepare(timer, first_motor, dshot_handler[timer].motors_number);
uint8_t motors_number = dshot_handler[timer].motors_number;
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] + DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE));
(uintptr_t)dshot_burst_buffer[timer] + DSHOT_BURST_BUFFER_SIZE(motors_number));
first_motor += dshot_handler[timer].motors_number;
first_motor += motors_number;
uint32_t dshot_data_size = dshot_handler[timer].motors_number * ONE_MOTOR_BUFF_SIZE;
uint32_t dshot_data_size = motors_number * ONE_MOTOR_BUFF_SIZE;
switch (io_timers[timer].dshot.stream) {
case DShot_Stream0:
@@ -370,12 +399,13 @@ static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool
packet |= (checksum & 0x0F);
for (i = 0; i < ONE_MOTOR_DATA_SIZE; i++) {
motor_buffer[motor_number][i] = (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first
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][16] = 0;
motor_buffer[motor_number][17] = 0;
motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + 16] = 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)
@@ -390,15 +420,12 @@ 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)
{
// 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
uint32_t *buffer = dshot_burst_buffer[timer];
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][motor_data_index];
buffer[motor_data_index * motors_number + motor_index] = motor_buffer[(motor_index +
first_motor) * ONE_MOTOR_BUFF_SIZE + motor_data_index];
}
}
}
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h
+1 -1
View File
@@ -83,5 +83,5 @@ typedef struct dshot_conf_t {
dshot_dma_channel_t channel;
dshot_dma_stream_t stream;
uint32_t start_ccr_register;
uint8_t channels_number;
uint8_t channels_number; ///< number of channels/outputs (<=MAX_NUM_CHANNELS_PER_TIMER)
} dshot_conf_t;
platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
+5 -1
View File
@@ -44,7 +44,11 @@
#pragma once
__BEGIN_DECLS
/* configuration limits */
#define MAX_IO_TIMERS 5
#ifdef BOARD_NUM_IO_TIMERS
#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS
#else
#define MAX_IO_TIMERS 2
#endif
#define MAX_TIMER_IO_CHANNELS 8
#define MAX_LED_TIMERS 2