dshot: reduce static buffer size

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.

boards/av/x-v1/src/board_config.h

@@ -328,6 +328,8 @@
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
 
+#define BOARD_NUM_IO_TIMERS 5
+
 __BEGIN_DECLS
 
 /****************************************************************************************************

boards/holybro/kakutef7/src/board_config.h

@@ -216,6 +216,8 @@
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
 
+#define BOARD_NUM_IO_TIMERS 4
+
 __BEGIN_DECLS
 
 /****************************************************************************************************

boards/mro/ctrl-zero-f7/src/board_config.h

@@ -336,6 +336,8 @@
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
 
+#define BOARD_NUM_IO_TIMERS 3
+
 __BEGIN_DECLS
 
 #ifndef __ASSEMBLY__

boards/nxp/fmuk66-v3/src/board_config.h

@@ -520,6 +520,8 @@ __BEGIN_DECLS
 
 #define BOARD_HAS_NOISY_FXOS8700_MAG 1 // Disable internal MAG
 
+#define BOARD_NUM_IO_TIMERS 3
+
 /************************************************************************************
  * Public data
  ************************************************************************************/

boards/px4/fmu-v5/src/board_config.h

@@ -671,6 +671,8 @@
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
 
+#define BOARD_NUM_IO_TIMERS 5
+
 __BEGIN_DECLS
 
 /****************************************************************************************************

boards/px4/fmu-v5x/src/board_config.h

@@ -672,6 +672,8 @@
 
 #define BOARD_ENABLE_CONSOLE_BUFFER
 
+#define BOARD_NUM_IO_TIMERS 5
+
 __BEGIN_DECLS
 
 /****************************************************************************************************

boards/px4/io-v2/src/board_config.h

@@ -162,3 +162,5 @@
 #define LED_SIGNAL        5  /* LED? + LED? */
 #define LED_ASSERTION     6  /* LED? + LED? + LED? */
 #define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
+
+#define BOARD_NUM_IO_TIMERS 3

platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h

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

@@ -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);
+
+		if (!motor_buffer) {
+			return -ENOMEM;
+		}
+	}
+
+	unsigned buffer_offset = 0;
 
-	uint8_t timer;
+	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

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

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

src/drivers/dshot/dshot.cpp

@@ -531,11 +531,20 @@ DShotOutput::update_dshot_out_state(bool on)
 			break;
 		}
 
-		up_dshot_init(_output_mask, dshot_frequency);
+		int ret = up_dshot_init(_output_mask, dshot_frequency);
+
+		if (ret != 0) {
+			PX4_ERR("up_dshot_init failed (%i)", ret);
+			return;
+		}
+
 		_outputs_initialized = true;
 	}
 
-	up_dshot_arm(on);
+	if (_outputs_initialized) {
+		up_dshot_arm(on);
+		_outputs_on = on;
+	}
 }
 
 void DShotOutput::updateTelemetryNumMotors()
@@ -747,7 +756,6 @@ DShotOutput::Run()
 	bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers();
 
 	if (_outputs_on != outputs_on) {
-		_outputs_on = outputs_on;
 		update_dshot_out_state(outputs_on);
 	}