dshot: fixed DShot stream random bits. Supported boards migrated to n…

…ew drivers.
PX4 · Oct 20, 2019 · 59261e4 · 59261e4
1 parent fd2e32c
commit 59261e4
Show file tree

Hide file tree

Showing 8 changed files with 50 additions and 50 deletions.
diff --git a/boards/holybro/kakutef7/src/timer_config.c b/boards/holybro/kakutef7/src/timer_config.c
@@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler0,
 		.vectorno =  STM32_IRQ_TIM3,
 		.dshot = {
-			.dma_base = DSHOT_DMA1_BASE,
-			.channel = DShot_Channel5,
-			.stream = DShot_Stream2,
+			.dma_base = STM32_DMA1_BASE,
+			.dmamap = DMAMAP_TIM3_UP,
 			.start_ccr_register = TIM_DMABASE_CCR3,
 			.channels_number = 2u /* CCR3 and CCR4 */
 		}
@@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler1,
 		.vectorno =  STM32_IRQ_TIM1CC,
 		.dshot = {
-			.dma_base = DSHOT_DMA2_BASE,
-			.channel = DShot_Channel6,
-			.stream = DShot_Stream5,
+			.dma_base = STM32_DMA2_BASE,
+			.dmamap = DMAMAP_TIM1_UP,
 			.start_ccr_register = TIM_DMABASE_CCR1,
 			.channels_number = 2u /* CCR1 and CCR2 */
 		}
@@ -95,9 +93,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler2,
 		.vectorno =  STM32_IRQ_TIM8CC,
 		.dshot = {
-			.dma_base = DSHOT_DMA2_BASE,
-			.channel = DShot_Channel7,
-			.stream = DShot_Stream1,
+			.dma_base = STM32_DMA2_BASE,
+			.dmamap = DMAMAP_TIM8_UP,
 			.start_ccr_register = TIM_DMABASE_CCR4,
 			.channels_number = 1u /* CCR4 */
 		}
@@ -112,9 +109,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler3,
 		.vectorno =  STM32_IRQ_TIM5,
 		.dshot = {
-			.dma_base = DSHOT_DMA1_BASE,
-			.channel = DShot_Channel6,
-			.stream = DShot_Stream6, // alternatively DShot_Stream0
+			.dma_base = STM32_DMA1_BASE,
+			.dmamap = DMAMAP_TIM5_UP_2,
 			.start_ccr_register = TIM_DMABASE_CCR4,
 			.channels_number = 1u /* CCR4 */
 		}

diff --git a/boards/omnibus/f4sd/src/timer_config.c b/boards/omnibus/f4sd/src/timer_config.c
@@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler1,
 		.vectorno =  STM32_IRQ_TIM2,
 		.dshot = {
-			.dma_base = DSHOT_DMA1_BASE,
-			.channel = DShot_Channel3,
-			.stream = DShot_Stream1,
+			.dma_base = STM32_DMA1_BASE,
+			.dmamap = DMAMAP_TIM2_UP_1,
 			.start_ccr_register = TIM_DMABASE_CCR3,
 			.channels_number = 2u /* CCR3 and CCR4 */
 		}
@@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler2,
 		.vectorno =  STM32_IRQ_TIM3,
 		.dshot = {
-			.dma_base = DSHOT_DMA1_BASE,
-			.channel = DShot_Channel5,
-			.stream = DShot_Stream2,
+			.dma_base = STM32_DMA1_BASE,
+			.dmamap = DMAMAP_TIM3_UP,
 			.start_ccr_register = TIM_DMABASE_CCR3,
 			.channels_number = 2u /* CCR3 and CCR4 */
 		}

diff --git a/boards/px4/fmu-v2/src/timer_config.c b/boards/px4/fmu-v2/src/timer_config.c
@@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler0,
 		.vectorno =  STM32_IRQ_TIM1CC,
 		.dshot = {
-			.dma_base = DSHOT_DMA2_BASE,
-			.channel = DShot_Channel6,
-			.stream = DShot_Stream5,
+			.dma_base = STM32_DMA2_BASE,
+			.dmamap = DMAMAP_TIM1_UP,
 			.start_ccr_register = TIM_DMABASE_CCR1,
 			.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */
 		}
@@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler1,
 		.vectorno =  STM32_IRQ_TIM4,
 		.dshot = {
-			.dma_base = DSHOT_DMA1_BASE,
-			.channel = DShot_Channel2,
-			.stream = DShot_Stream6,
+			.dma_base = STM32_DMA1_BASE,
+			.dmamap = DMAMAP_TIM4_UP,
 			.start_ccr_register = TIM_DMABASE_CCR2,
 			.channels_number = 2u /* CCR2 and CCR3 */
 		}

diff --git a/boards/px4/fmu-v3/src/timer_config.c b/boards/px4/fmu-v3/src/timer_config.c
@@ -61,9 +61,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler0,
 		.vectorno =  STM32_IRQ_TIM1CC,
 		.dshot = {
-			.dma_base = DSHOT_DMA2_BASE,
-			.channel = DShot_Channel6,
-			.stream = DShot_Stream5,
+			.dma_base = STM32_DMA2_BASE,
+			.dmamap = DMAMAP_TIM1_UP,
 			.start_ccr_register = TIM_DMABASE_CCR1,
 			.channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */
 		}
@@ -78,9 +77,8 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.handler = io_timer_handler1,
 		.vectorno =  STM32_IRQ_TIM4,
 		.dshot = {
-			.dma_base = DSHOT_DMA1_BASE,
-			.channel = DShot_Channel2,
-			.stream = DShot_Stream6,
+			.dma_base = STM32_DMA1_BASE,
+			.dmamap = DMAMAP_TIM4_UP,
 			.start_ccr_register = TIM_DMABASE_CCR2,
 			.channels_number = 2u /* CCR2 and CCR3 */
 		}

diff --git a/boards/px4/fmu-v4/src/timer_config.c b/boards/px4/fmu-v4/src/timer_config.c
@@ -77,13 +77,12 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = {
 		.last_channel_index = 5,
 		.handler = io_timer_handler1,
 		.vectorno =  STM32_IRQ_TIM4,
-		/* In conflict with UART8_RX
 		.dshot = {
 			.dma_base = STM32_DMA1_BASE,
 			.dmamap = DMAMAP_TIM4_UP,
 			.start_ccr_register = TIM_DMABASE_CCR2,
 			.channels_number = 2u // CCR2 and CCR3
-		} */
+		}
 
 	}
 };

diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
@@ -51,23 +51,23 @@
 #define DSHOT_TIMERS				MAX_IO_TIMERS
 #define MOTORS_NUMBER				DIRECT_PWM_OUTPUT_CHANNELS
 #define ONE_MOTOR_DATA_SIZE			16u
-#define ONE_MOTOR_BUFF_SIZE			18u
+#define ONE_MOTOR_BUFF_SIZE			17u
 #define ALL_MOTORS_BUF_SIZE			(MOTORS_NUMBER * ONE_MOTOR_BUFF_SIZE)
 #define DSHOT_THROTTLE_POSITION		5u
 #define DSHOT_TELEMETRY_POSITION	4u
 #define NIBBLES_SIZE 				4u
 #define DSHOT_NUMBER_OF_NIBBLES		3u
-#define DSHOT_REDUNDANT_BIT_16 		16u
-#define DSHOT_REDUNDANT_BIT_17 		17u
+#define DSHOT_END_OF_STREAM 		16u
 #define MAX_NUM_CHANNELS_PER_TIMER	4u // CCR1-CCR4
 
 #define DSHOT_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | \
-				 DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE)
+		       DMA_SCR_DIR_M2P | DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE)
 
 typedef struct dshot_handler_t {
 	bool			init;
 	DMA_HANDLE		dma_handle;
 	uint32_t		dma_size;
+	uint8_t			callback_arg;
 } dshot_handler_t;
 
 #define DMA_BUFFER_MASK    (PX4_ARCH_DCACHE_LINESIZE - 1)
@@ -85,6 +85,7 @@ static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNM
 #endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */
 
 void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number);
+void dshot_callback(DMA_HANDLE handle, uint8_t status, void *arg);
 
 int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 {
@@ -175,20 +176,28 @@ void up_dshot_trigger(void)
 
 			first_motor += motors_number;
 
-			io_timer_update_generation(timer);
-
 			stm32_dmasetup(dshot_handler[timer].dma_handle,
-			                io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
-			                (uint32_t)(dshot_burst_buffer[timer]),
-			                dshot_handler[timer].dma_size,
-			                DSHOT_DMA_SCR);
+				       io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
+				       (uint32_t)(dshot_burst_buffer[timer]),
+				       dshot_handler[timer].dma_size,
+				       DSHOT_DMA_SCR);
 
 			// Trigger DMA (DShot Outputs)
-			stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false);
+			dshot_handler[timer].callback_arg = timer;
+			stm32_dmastart(dshot_handler[timer].dma_handle, &dshot_callback, &(dshot_handler[timer].callback_arg), false);
+			io_timer_update_dma_req(timer, true);
+
 		}
 	}
 }
 
+void dshot_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+
+	uint8_t timer = *((uint8_t *)arg);
+	io_timer_update_dma_req(timer, false);
+}
+
 /**
 * bits 	1-11	- throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution)
 * bit 	12		- dshot telemetry enable/disable
@@ -225,8 +234,7 @@ static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool
 		packet <<= 1;
 	}
 
-	motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_REDUNDANT_BIT_16] = 0;
-	motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_REDUNDANT_BIT_17] = 0;
+	motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_END_OF_STREAM] = 0;
 }
 
 void up_dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry)

diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h
@@ -143,7 +143,7 @@ __EXPORT int io_timer_free_channel(unsigned channel);
 __EXPORT int io_timer_get_channel_mode(unsigned channel);
 __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
 __EXPORT extern void io_timer_trigger(void);
-__EXPORT void io_timer_update_generation(uint8_t timer);
+__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
 
 __EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length);
 

diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c
@@ -492,10 +492,14 @@ static inline void io_timer_set_oneshot_mode(unsigned timer)
 	rEGR(timer) = GTIM_EGR_UG;
 }
 
-void io_timer_update_generation(uint8_t timer)
+void io_timer_update_dma_req(uint8_t timer, bool enable)
 {
-	// Re-initialize the counter and generate an update of the registers
-	rEGR(timer) = ATIM_EGR_UG;
+	if (enable) {
+		rDIER(timer) |= ATIM_DIER_UDE;
+
+	} else {
+		rDIER(timer) &= ~ATIM_DIER_UDE;
+	}
 }
 
 int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length)
@@ -524,7 +528,6 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_
 		rPSC(timer)  = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1;
 		rEGR(timer)  = ATIM_EGR_UG;
 		rDCR(timer)  = (io_timers[timer].dshot.start_ccr_register | tim_dma_burst_length);
-		rDIER(timer) = ATIM_DIER_UDE;
 	}
 
 	return ret_val;