Merge remote-tracking branch 'px4/main' into pr-bidirectional-dshot

Signed-off-by: Julian Oes <julian@oes.ch>

platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c

@@ -1,7 +1,8 @@
 /****************************************************************************
  *
- * Copyright (C) 2019-2021 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2019-2023 PX4 Development Team. All rights reserved.
  * Author: Igor Misic <igy1000mb@gmail.com>
+ * Author: Julian Oes <julian@oes.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,12 +40,17 @@
 
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/micro_hal.h>
+#include <px4_platform_common/log.h>
 #include <stm32_dma.h>
 #include <stm32_tim.h>
 #include <px4_arch/dshot.h>
 #include <px4_arch/io_timer.h>
 #include <drivers/drv_dshot.h>
 
+#include <stdio.h>
+#include <drivers/drv_input_capture.h>
+#include <drivers/drv_hrt.h>
+
 
 #define MOTOR_PWM_BIT_1				14u
 #define MOTOR_PWM_BIT_0				7u
@@ -63,8 +69,10 @@
 #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)
 
+#define DSHOT_BIDIRECTIONAL_DMA_SCR (DMA_SCR_PRIHI | DMA_SCR_MSIZE_16BITS | DMA_SCR_PSIZE_16BITS | DMA_SCR_MINC | \
+				     DMA_SCR_DIR_P2M | DMA_SCR_TCIE | DMA_SCR_TEIE | DMA_SCR_DMEIE)
+
 typedef struct dshot_handler_t {
-	bool			init;
 	DMA_HANDLE		dma_handle;
 	uint32_t		dma_size;
 } dshot_handler_t;
@@ -82,36 +90,199 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M
 px4_cache_aligned_data() = {};
 static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {};
 
-int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
+static uint16_t dshot_capture_buffer[32] px4_cache_aligned_data() = {};
+
+static struct hrt_call _call;
+
+static void do_capture(DMA_HANDLE handle, uint8_t status, void *arg);
+static void process_capture_results(void *arg);
+static unsigned calculate_period(void);
+static int dshot_output_timer_init(unsigned channel);
+static int dshot_output_timer_deinit(unsigned channel);
+
+static uint32_t read_ok = 0;
+static uint32_t read_fail_nibble = 0;
+static uint32_t read_fail_crc = 0;
+static uint32_t read_fail_zero = 0;
+
+static bool bidirectional_dshot_enabled = true;
+
+static uint32_t _dshot_frequency = 0;
+static int _timers_init_mask = 0;
+static int _channels_init_mask = 0;
+
+// We only support capture on the first timer (usually 4 channels) for now.
+static uint32_t _motor_to_capture = 0;
+static int32_t _erpms[4] = {};
+
+static void(*_erpm_callback)(int32_t[], size_t, void *) = NULL;
+static void *_erpm_callback_context = NULL;
+
+uint8_t nibbles_from_mapped(uint8_t mapped)
 {
-	unsigned buffer_offset = 0;
+	switch (mapped) {
+	case 0x19:
+		return 0x00;
 
-	for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) {
-		dshot_handler[timer_index].init = false;
+	case 0x1B:
+		return 0x01;
+
+	case 0x12:
+		return 0x02;
+
+	case 0x13:
+		return 0x03;
+
+	case 0x1D:
+		return 0x04;
+
+	case 0x15:
+		return 0x05;
+
+	case 0x16:
+		return 0x06;
+
+	case 0x17:
+		return 0x07;
+
+	case 0x1a:
+		return 0x08;
+
+	case 0x09:
+		return 0x09;
+
+	case 0x0A:
+		return 0x0A;
+
+	case 0x0B:
+		return 0x0B;
+
+	case 0x1E:
+		return 0x0C;
+
+	case 0x0D:
+		return 0x0D;
+
+	case 0x0E:
+		return 0x0E;
+
+	case 0x0F:
+		return 0x0F;
+
+	default:
+		// Unknown mapped
+		return 0xFF;
 	}
+}
 
-	for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
-		if (io_timers[timer].base == 0) { // no more timers configured
-			break;
+unsigned calculate_period(void)
+{
+	uint32_t value = 0;
+
+	// We start off with high
+	uint32_t high = 1;
+
+	unsigned shifted = 0;
+	unsigned previous = 0;
+
+	for (unsigned i = 1; i < (32); ++i) {
+
+		// We can ignore the very first data point as it's the pulse before it starts.
+		if (i > 1) {
+
+			if (dshot_capture_buffer[i] == 0) {
+				// Once we get zeros we're through.
+				break;
+			}
+
+			// This seemss to work with dshot 150, 300, 600, 1200
+			// The values were found by trial and error to get the quantization just right.
+			const uint32_t bits = (dshot_capture_buffer[i] - previous + 5) / 20;
+
+			for (unsigned bit = 0; bit < bits; ++bit) {
+				value = value << 1;
+				value |= high;
+				++shifted;
+			}
+
+			// The next edge toggles.
+			high = !high;
 		}
 
-		// we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size
-#pragma GCC diagnostic push
-#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_channel_mapping.element[timer].channel_count_including_gaps);
-
-		if (buffer_offset > sizeof(dshot_burst_buffer_array)) {
-			return -EINVAL; // something is wrong with the board configuration or some other logic
-		}
+		previous = dshot_capture_buffer[i];
 	}
 
-	/* Init channels */
-	int ret_val = OK;
-	int channels_init_mask = 0;
+	if (shifted == 0) {
+		// no data yet, or this time
+		++read_fail_zero;
+		return 0;
+	}
 
-	for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) {
+	// We need to make sure we shifted 21 times. We might have missed some low "pulses" at the very end.
+	value = value << (21 - shifted);
+
+	// Note: At 0 throttle, the value is 0x1AD6AE, so 0b110101101011010101110
+
+	// From GCR to eRPM according to:
+	// https://brushlesswhoop.com/dshot-and-bidirectional-dshot/#erpm-transmission
+	unsigned gcr = (value ^ (value >> 1));
+
+	uint32_t data = 0;
+
+	// 20bits -> 5 mapped -> 4 nibbles
+	for (unsigned i = 0; i < 4; ++i) {
+		uint32_t nibble = nibbles_from_mapped(gcr & (0x1F)) << (4 * i);
+
+		if (nibble == 0xff) {
+			++read_fail_nibble;
+			return 0;
+		}
+
+		data |= nibble;
+		gcr = gcr >> 5;
+	}
+
+	unsigned shift = (data & 0xE000) >> 13;
+	unsigned period = ((data & 0x1FF0) >> 4) << shift;
+	unsigned crc = (data & 0xf);
+
+	unsigned payload = (data & 0xFFF0) >> 4;
+	unsigned calculated_crc = (~(payload ^ (payload >> 4) ^ (payload >> 8))) & 0x0F;
+
+	if (crc != calculated_crc) {
+		++read_fail_crc;
+		return 0;
+	}
+
+	++read_ok;
+	return period;
+}
+
+int dshot_output_timer_deinit(unsigned channel)
+{
+	return io_timer_unallocate_channel(channel);
+}
+
+int dshot_output_timer_init(unsigned channel)
+{
+	int ret = io_timer_channel_init(channel,
+					bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot, NULL, NULL);
+
+	if (ret == -EBUSY) {
+		// either timer or channel already used - this is not fatal
+		return OK;
+
+	} else {
+		return ret;
+	}
+}
+
+int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
+{
+	bidirectional_dshot_enabled = enable_bidirectional_dshot;
+	_dshot_frequency = dshot_pwm_freq;
+
+	for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) {
 		if (channel_mask & (1 << channel)) {
 			uint8_t timer = timer_io_channels[channel].timer_index;
 
@@ -119,49 +290,113 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 				continue;
 			}
 
-			ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL);
-			channel_mask &= ~(1 << channel);
+			int ret = dshot_output_timer_init(channel);
 
-			if (OK == ret_val) {
-				dshot_handler[timer].init = true;
-				channels_init_mask |= 1 << channel;
+			if (ret != OK) {
+				return ret;
+			}
 
-			} else if (ret_val == -EBUSY) {
-				/* either timer or channel already used - this is not fatal */
-				ret_val = 0;
+			_channels_init_mask |= (1 << channel);
+			_timers_init_mask |= (1 << timer);
+		}
+	}
+
+	for (uint8_t timer = 0; timer < MAX_IO_TIMERS; ++timer) {
+		if (_timers_init_mask & (1 << timer)) {
+			if (dshot_handler[timer].dma_handle == NULL) {
+				dshot_handler[timer].dma_size = io_timers_channel_mapping.element[timer].channel_count_including_gaps *
+								ONE_MOTOR_BUFF_SIZE;
+
+				io_timer_set_dshot_mode(timer, _dshot_frequency,
+							io_timers_channel_mapping.element[timer].channel_count_including_gaps);
+
+				dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap);
+
+				if (NULL == dshot_handler[timer].dma_handle) {
+					// TODO: how to log this?
+					return -ENOSR;
+				}
+
+				// We have tested this now but will anyway initialize it again during a trigger, so we can free it again.
+				stm32_dmafree(dshot_handler[timer].dma_handle);
+				dshot_handler[timer].dma_handle = NULL;
 			}
 		}
 	}
 
-	for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) {
+	unsigned buffer_offset = 0;
 
-		if (true == dshot_handler[timer_index].init) {
-			dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps *
-							      ONE_MOTOR_BUFF_SIZE;
-			io_timer_set_dshot_mode(timer_index, dshot_pwm_freq,
-						io_timers_channel_mapping.element[timer_index].channel_count_including_gaps);
+	for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
+		if (_timers_init_mask & (1 << timer)) {
+			if (io_timers[timer].base == 0) { // no more timers configured
+				break;
+			}
 
-			dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap);
+			// we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size
+#pragma GCC diagnostic push
+#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_channel_mapping.element[timer].channel_count_including_gaps);
 
-			if (NULL == dshot_handler[timer_index].dma_handle) {
-				ret_val = ERROR;
+			if (buffer_offset > sizeof(dshot_burst_buffer_array)) {
+				return -EINVAL; // something is wrong with the board configuration or some other logic
 			}
 		}
 	}
 
-	return ret_val == OK ? channels_init_mask : ret_val;
+	return _channels_init_mask;
 }
 
 void up_dshot_trigger(void)
 {
-	for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) {
 
-		if (true == dshot_handler[timer].init) {
+	for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) {
+		if (_channels_init_mask & (1 << channel)) {
+
+			// For bidirectional dshot we need to re-initialize the timers every time here.
+			// In normal mode, we just do it once.
+			int ret = OK;
+
+			if (bidirectional_dshot_enabled) {
+				dshot_output_timer_deinit(channel);
+				ret = dshot_output_timer_init(channel);
+			}
+
+			if (ret != OK) {
+				// TODO: what to do here?
+				return;
+			}
+		}
+	}
+
+	io_timer_set_enable(true, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
+			    IO_TIMER_ALL_MODES_CHANNELS);
+
+	for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
+		if (_timers_init_mask & (1 << timer)) {
+
+			if (dshot_handler[timer].dma_handle == NULL) {
+				dshot_handler[timer].dma_size = io_timers_channel_mapping.element[timer].channel_count_including_gaps *
+								ONE_MOTOR_BUFF_SIZE;
+
+				io_timer_set_dshot_mode(timer, _dshot_frequency,
+							io_timers_channel_mapping.element[timer].channel_count_including_gaps);
+
+				dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap);
+
+				if (NULL == dshot_handler[timer].dma_handle) {
+					// TODO: how to log this?
+					return;
+				}
+			}
 
 			// 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(io_timers_channel_mapping.element[timer].channel_count_including_gaps));
+			up_clean_dcache((uintptr_t) dshot_burst_buffer[timer],
+					(uintptr_t) dshot_burst_buffer[timer] +
+					DSHOT_BURST_BUFFER_SIZE(
+						io_timers_channel_mapping.element[timer].channel_count_including_gaps));
 
 			px4_stm32_dmasetup(dshot_handler[timer].dma_handle,
 					   io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
@@ -172,13 +407,132 @@ void up_dshot_trigger(void)
 			// Clean UDE flag before DMA is started
 			io_timer_update_dma_req(timer, false);
 			// Trigger DMA (DShot Outputs)
-			stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false);
+			stm32_dmastart(dshot_handler[timer].dma_handle, bidirectional_dshot_enabled ? do_capture : NULL, NULL, false);
 			io_timer_update_dma_req(timer, true);
-
 		}
 	}
 }
 
+void do_capture(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+	(void)handle;
+	(void)status;
+	(void)arg;
+
+	for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
+		if (_timers_init_mask & (1 << timer)) {
+			if (dshot_handler[timer].dma_handle != NULL) {
+				stm32_dmastop(dshot_handler[timer].dma_handle);
+				stm32_dmafree(dshot_handler[timer].dma_handle);
+				dshot_handler[timer].dma_handle = NULL;
+			}
+
+			// TODO: this doesn't scale to more than 4 motors yet
+			unsigned capture_channel = _motor_to_capture;
+
+			dshot_handler[timer].dma_size = sizeof(dshot_capture_buffer);
+
+			// Instead of using the UP DMA channel, we need to use the CH1-4 channels.
+			// It turns out that we can infer the DMA channel index by starting from the UP channel -5.
+			unsigned timer_channel = timer_io_channels[capture_channel].timer_channel;
+
+			switch (timer_channel) {
+			case 1:
+				dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 1);
+				break;
+
+			case 2:
+				dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 2);
+				break;
+
+			case 3:
+				dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 3);
+				break;
+
+			case 4:
+				dshot_handler[timer].dma_handle = stm32_dmachannel(io_timers[timer].dshot.dmamap - 5 + 4);
+				break;
+			}
+
+			memset(dshot_capture_buffer, 0, sizeof(dshot_capture_buffer));
+			up_clean_dcache((uintptr_t) dshot_capture_buffer,
+					(uintptr_t) dshot_capture_buffer +
+					sizeof(dshot_capture_buffer));
+
+			px4_stm32_dmasetup(dshot_handler[timer].dma_handle,
+					   io_timers[timer].base + STM32_GTIM_DMAR_OFFSET,
+					   (uint32_t) dshot_capture_buffer,
+					   sizeof(dshot_capture_buffer),
+					   DSHOT_BIDIRECTIONAL_DMA_SCR);
+
+			io_timer_unallocate_channel(capture_channel);
+			io_timer_channel_init(capture_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
+			io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, 1 << capture_channel);
+
+			up_input_capture_set(capture_channel, Both, 0, NULL, NULL);
+
+			io_timer_capture_update_dma_req(timer, false);
+			io_timer_set_capture_mode(timer, _dshot_frequency, capture_channel);
+			stm32_dmastart(dshot_handler[timer].dma_handle, NULL, NULL, false);
+			io_timer_capture_update_dma_req(timer, true);
+		}
+	}
+
+	// It takes around 85 us for the ESC to respond, so we should have a result after 150 us, surely.
+	hrt_call_after(&_call, 150, process_capture_results, NULL);
+}
+
+void process_capture_results(void *arg)
+{
+	(void)arg;
+
+	// In case DMA is still set up from the last capture, we clear that.
+	for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) {
+		if (_timers_init_mask & (1 << timer)) {
+			if (dshot_handler[timer].dma_handle != NULL) {
+				stm32_dmastop(dshot_handler[timer].dma_handle);
+				stm32_dmafree(dshot_handler[timer].dma_handle);
+				dshot_handler[timer].dma_handle = NULL;
+			}
+		}
+	}
+
+	up_invalidate_dcache((uintptr_t)dshot_capture_buffer,
+			     (uintptr_t)dshot_capture_buffer +
+			     sizeof(dshot_capture_buffer));
+
+	const unsigned period = calculate_period();
+
+	if (period == 0) {
+		// If the parsing failed, we get 0.
+		_erpms[_motor_to_capture] = 0;
+
+	} else if (period == 65408) {
+		// For still, we get this magic 65408 value.
+		_erpms[_motor_to_capture] = 0;
+
+	} else {
+		// from period in us to eRPM
+		_erpms[_motor_to_capture] = 1000000 * 60 / period;
+	}
+
+	for (unsigned channel = 0; channel < MAX_TIMER_IO_CHANNELS; channel++) {
+		if (_channels_init_mask & (1 << channel)) {
+			io_timer_unallocate_channel(channel);
+			io_timer_channel_init(channel, IOTimerChanMode_DshotInverted, NULL, NULL);
+		}
+	}
+
+	if (_erpm_callback != NULL) {
+		// Only publish every 4th time once all measurements have come in.
+		if (_motor_to_capture == 3) {
+			_erpm_callback(_erpms, 4, _erpm_callback_context);
+		}
+	}
+
+	_motor_to_capture = (_motor_to_capture + 1) % 4;
+}
+
 /**
 * bits 	1-11	- throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution)
 * bit 	12		- dshot telemetry enable/disable
@@ -202,9 +556,20 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
 		csum_data >>= NIBBLES_SIZE;
 	}
 
-	packet |= (checksum & 0x0F);
+	if (bidirectional_dshot_enabled) {
+		packet |= ((~checksum) & 0x0F);
+
+	} else {
+		packet |= ((checksum) & 0x0F);
+	}
 
 	unsigned timer = timer_io_channels[motor_number].timer_index;
+
+	// If this timer is not initialized, we give up here.
+	if (!(_timers_init_mask & (1 << timer))) {
+		return;
+	}
+
 	uint32_t *buffer = dshot_burst_buffer[timer];
 	const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer];
 	unsigned num_motors = mapping->channel_count_including_gaps;
@@ -219,7 +584,20 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
 
 int up_dshot_arm(bool armed)
 {
-	return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS);
+	return io_timer_set_enable(armed, bidirectional_dshot_enabled ? IOTimerChanMode_DshotInverted : IOTimerChanMode_Dshot,
+				   IO_TIMER_ALL_MODES_CHANNELS);
+}
+
+void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context)
+{
+	_erpm_callback = callback;
+	_erpm_callback_context = context;
+}
+
+void print_driver_stats()
+{
+	PX4_INFO("dshot driver stats: %lu read, %lu failed nibble, %lu failed CRC, %lu invalid/zero",
+		 read_ok, read_fail_nibble, read_fail_crc, read_fail_zero);
 }
 
 #endif

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -80,6 +80,8 @@ typedef enum io_timer_channel_mode_t {
 	IOTimerChanMode_LED     = 7,
 	IOTimerChanMode_PPS     = 8,
 	IOTimerChanMode_Other   = 9,
+	IOTimerChanMode_DshotInverted = 10,
+	IOTimerChanMode_CaptureDMA = 11,
 	IOTimerChanModeSize
 } io_timer_channel_mode_t;
 
@@ -159,6 +161,9 @@ __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(unsigned channels_mask);
 __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable);
+__EXPORT void io_timer_capture_update_dma_req(uint8_t timer, bool enable);
+__EXPORT int io_timer_set_enable_capture_dma(bool state, io_timer_channel_allocation_t masks);
+__EXPORT int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_rate, unsigned channel);
 
 /**
  * Reserve a timer

platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -365,8 +365,10 @@ int up_input_capture_set_trigger(unsigned channel,  input_capture_edge edge)
 		rv = -ENXIO;
 
 		/* Any pins in capture mode */
+		int mode = io_timer_get_channel_mode(channel);
 
-		if (io_timer_get_channel_mode(channel) == IOTimerChanMode_Capture) {
+		if (mode == IOTimerChanMode_Capture ||
+		    mode == IOTimerChanMode_CaptureDMA) {
 
 			uint16_t edge_bits = 0xffff;
 

platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012, 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -131,6 +131,7 @@ static int io_timer_handler7(int irq, void *context, void *arg);
 	(GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT)
 
 #define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
+#define CCMR_C1_PWMOUT_INVERTED_INIT (GTIM_CCMR_MODE_PWM2 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
 
 #define CCMR_C1_PWMIN_INIT 0 // TBD
 
@@ -519,6 +520,18 @@ void io_timer_update_dma_req(uint8_t timer, bool enable)
 	}
 }
 
+void io_timer_capture_update_dma_req(uint8_t timer, bool enable)
+{
+	if (enable) {
+		rDIER(timer) |= ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE;
+		rEGR(timer)  |= (ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G | ATIM_EGR_CC3G | ATIM_EGR_CC4G);
+
+	} else {
+		rEGR(timer)  &= ~(ATIM_EGR_UG | ATIM_EGR_CC1G | ATIM_EGR_CC2G | ATIM_EGR_CC3G | ATIM_EGR_CC4G);
+		rDIER(timer) &= ~(ATIM_DIER_CC1DE | ATIM_DIER_CC2DE | ATIM_DIER_CC3DE | ATIM_DIER_CC4DE);
+	}
+}
+
 int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length)
 {
 	int ret_val = OK;
@@ -545,7 +558,7 @@ 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;
 
-		// find the lowest channel index for the timer (they are not necesarily in ascending order)
+		// find the lowest channel index for the timer (they are not necessarily in ascending order)
 		unsigned lowest_timer_channel = 4;
 		uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
 		uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
@@ -574,6 +587,54 @@ int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_
 	return ret_val;
 }
 
+int io_timer_set_capture_mode(uint8_t timer, unsigned dshot_pwm_freq, unsigned channel)
+{
+	rARR(timer)  = -1;
+	rEGR(timer)  = ATIM_EGR_UG | GTIM_EGR_CC1G | GTIM_EGR_CC2G | GTIM_EGR_CC3G | GTIM_EGR_CC4G;
+
+	rPSC(timer) = ((int)(io_timers[timer].clock_freq / (dshot_pwm_freq * 5 / 4)) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1;
+
+
+
+	switch (timer_io_channels[channel].timer_channel) {
+	case 1:
+		// We need to disable CC1E before we can switch to CC1S to input
+		rCCER(timer) &= ~(GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP);
+		rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1  << GTIM_CCMR1_CC1S_SHIFT);
+		rCR1(timer) |= GTIM_CR1_CEN;
+		rCCER(timer) |= (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP);
+// We need to pass the offset of the register to read by DMA divided by 4.
+		rDCR(timer)  = 0xD; // 0x34 / 4, offset for CC1
+		break;
+
+	case 2:
+		rCCER(timer) &= ~(GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP);
+		rCCMR1(timer) |= (GTIM_CCMR_CCS_CCIN1  << GTIM_CCMR1_CC2S_SHIFT);
+		rCR1(timer) |= GTIM_CR1_CEN;
+		rCCER(timer) |= (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP);
+		rDCR(timer)  = 0xE; // 0x38 / 4, offset for CC2
+		break;
+
+	case 3:
+		rCCER(timer) &= ~(GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP);
+		rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1  << GTIM_CCMR2_CC3S_SHIFT);
+		rCR1(timer) |= GTIM_CR1_CEN;
+		rCCER(timer) |= (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP);
+		rDCR(timer)  = 0xF; // 0x3C / 4, offset for CC3
+		break;
+
+	case 4:
+		rCCER(timer) &= ~(GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP);
+		rCCMR2(timer) |= (GTIM_CCMR_CCS_CCIN1  << GTIM_CCMR2_CC4S_SHIFT);
+		rCR1(timer) |= GTIM_CR1_CEN;
+		rCCER(timer) |= (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP);
+		rDCR(timer)  = 0x10; // 0x40 / 4, offset for CC4
+		break;
+	}
+
+	return 0;
+}
+
 static inline void io_timer_set_PWM_mode(unsigned timer)
 {
 	rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1;
@@ -773,6 +834,12 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 		setbits = CCMR_C1_PWMOUT_INIT;
 		break;
 
+	case IOTimerChanMode_DshotInverted:
+		ccer_setbits = 0;
+		dier_setbits = 0;
+		setbits = CCMR_C1_PWMOUT_INVERTED_INIT;
+		break;
+
 	case IOTimerChanMode_PWMIn:
 		setbits = CCMR_C1_PWMIN_INIT;
 		gpio = timer_io_channels[channel].gpio_in;
@@ -781,6 +848,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 #if !defined(BOARD_HAS_NO_CAPTURE)
 
 	case IOTimerChanMode_Capture:
+	case IOTimerChanMode_CaptureDMA:
 		setbits = CCMR_C1_CAPTURE_INIT;
 		gpio = timer_io_channels[channel].gpio_in;
 		break;
@@ -805,6 +873,13 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
 
 		rv = io_timer_init_timer(timer, mode);
 
+		if (rv == -16) {
+			// FIXME: I don't understand why exactly this is the way it is.
+			// With this hack I'm able to to toggle the dshot pins from output
+			// to input without problem. But there should be a nicer way.
+			rv = 0;
+		}
+
 		if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) {
 			/* free the channel if it was not used before */
 			io_timer_unallocate_channel(channel);
@@ -897,12 +972,14 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 		break;
 
 	case IOTimerChanMode_Dshot:
+	case IOTimerChanMode_DshotInverted:
 		dier_bit = 0;
 		cr1_bit  = state ? GTIM_CR1_CEN : 0;
 		break;
 
 	case IOTimerChanMode_PWMIn:
 	case IOTimerChanMode_Capture:
+	case IOTimerChanMode_CaptureDMA:
 		break;
 
 	default:
@@ -944,6 +1021,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann
 			     (mode == IOTimerChanMode_PWMOut ||
 			      mode == IOTimerChanMode_OneShot ||
 			      mode == IOTimerChanMode_Dshot ||
+			      mode == IOTimerChanMode_DshotInverted ||
 			      mode == IOTimerChanMode_Trigger))) {
 				action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
 			}
@@ -1004,6 +1082,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value)
 		if ((mode != IOTimerChanMode_PWMOut) &&
 		    (mode != IOTimerChanMode_OneShot) &&
 		    (mode != IOTimerChanMode_Dshot) &&
+		    (mode != IOTimerChanMode_DshotInverted) &&
 		    (mode != IOTimerChanMode_Trigger)) {
 
 			rv = -EIO;

src/drivers/drv_dshot.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -88,10 +88,11 @@ typedef enum {
  * @param channel_mask		Bitmask of channels (LSB = channel 0) to enable.
  *				This allows some of the channels to remain configured
  *				as GPIOs or as another function. Already used channels/timers will not be configured as DShot
- * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
+ * @param dshot_freq		Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
+ * @param enable_bidirectional_dshot Whether to use bidirectional dshot
  * @return <0 on error, the initialized channels mask.
  */
-__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
+__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_freq, bool enable_bidirectional_dshot);
 
 /**
  * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
@@ -137,4 +138,10 @@ __EXPORT extern void up_dshot_trigger(void);
  */
 __EXPORT extern int up_dshot_arm(bool armed);
 
+__EXPORT extern bool up_dshot_get_periods(uint32_t periods[], size_t num_periods);
+
+__EXPORT extern void up_dshot_set_erpm_callback(void(*callback)(int32_t[], size_t, void *), void *context);
+
+__EXPORT extern void print_driver_stats(void);
+
 __END_DECLS

src/drivers/dshot/DShot.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -67,6 +67,13 @@ int DShot::init()
 	// Getting initial parameter values
 	update_params();
 
+	// We can't advertise in interrupt context later.
+	_esc_status_pub.advertise();
+	esc_status_s &esc_status = _esc_status_pub.get();
+	esc_status = {};
+	_esc_status_pub.update();
+	up_dshot_set_erpm_callback(&DShot::erpm_trampoline, this);
+
 	ScheduleNow();
 
 	return OK;
@@ -144,7 +151,7 @@ void DShot::enable_dshot_outputs(const bool enabled)
 			}
 		}
 
-		int ret = up_dshot_init(_output_mask, dshot_frequency);
+		int ret = up_dshot_init(_output_mask, dshot_frequency, _param_bidirectional_enable.get());
 
 		if (ret < 0) {
 			PX4_ERR("up_dshot_init failed (%i)", ret);
@@ -607,6 +614,30 @@ void DShot::update_params()
 	}
 }
 
+void DShot::erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context)
+{
+	DShot *self = static_cast<DShot *>(context);
+	self->erpm(erpms, num_erpms);
+}
+
+void DShot::erpm(int32_t erpms[], size_t num_erpms)
+{
+	esc_status_s &esc_status = _esc_status_pub.get();
+	esc_status = {};
+	esc_status.timestamp = hrt_absolute_time();
+	esc_status.counter = _esc_status_counter++;
+	esc_status.esc_count = num_erpms;
+	esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
+	esc_status.esc_armed_flags = _outputs_on;
+
+	for (unsigned i = 0; i < num_erpms && i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
+		esc_status.esc[i].timestamp = hrt_absolute_time();
+		esc_status.esc[i].esc_rpm = erpms[i] / (_param_mot_pole_count.get() / 2);
+	}
+
+	_esc_status_pub.update();
+}
+
 int DShot::custom_command(int argc, char *argv[])
 {
 	const char *verb = argv[0];
@@ -707,6 +738,7 @@ int DShot::print_status()
 	PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no");
 	perf_print_counter(_cycle_perf);
 	_mixing_output.printStatus();
+	print_driver_stats();
 
 	if (_telemetry) {
 		PX4_INFO("telemetry on: %s", _telemetry_device);

src/drivers/dshot/DShot.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -143,6 +143,9 @@ private:
 
 	void handle_vehicle_commands();
 
+	static void erpm_trampoline(int32_t erpms[], size_t num_erpms, void *context);
+	void erpm(int32_t erpms[], size_t num_erpms);
+
 	MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 	uint32_t _reversible_outputs{};
 
@@ -170,11 +173,15 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
 
+	uORB::PublicationData<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
+	uint16_t _esc_status_counter{0};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::DSHOT_MIN>)    _param_dshot_min,
 		(ParamBool<px4::params::DSHOT_3D_ENABLE>) _param_dshot_3d_enable,
 		(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
 		(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
-		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
+		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count,
+		(ParamBool<px4::params::DSHOT_BIDIR_EN>) _param_bidirectional_enable
 	)
 };

src/drivers/dshot/module.yaml

@@ -33,6 +33,16 @@ parameters:
                     When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
             type: boolean
             default: 0
+        DSHOT_BIDIR_EN:
+            description:
+                short: Enable bidirectional DShot
+                long: |
+                    This parameter enables bidirectional DShot which provides RPM feedback.
+                    Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32.
+                    This is not the same as DShot telemetry which requires an additional serial connection.
+            type: boolean
+            default: 0
+            reboot_required: true
         DSHOT_3D_DEAD_H:
             description:
                 short: DSHOT 3D deadband high