dshot: fix clearing out esc status

Tools/module_config/output_groups_from_timer_config.py

@@ -34,33 +34,41 @@ def extract_timer(line):
     if search:
         return search.group(1), 'generic'
 
-    # nxp rt1062 format: initIOPWM(PWM::FlexPWM2),
+    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
-    search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
+    search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,\)]', line, re.IGNORECASE)
     if search:
-        return search.group(1), 'imxrt'
+        return (search.group(1) + '_' +  search.group(2)), 'imxrt'
 
     return None, 'unknown'
 
-def extract_timer_from_channel(line, num_channels_already_found):
+def extract_timer_from_channel(line, timer_names):
     # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
     search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE)
     if search:
         return search.group(1)
 
-    # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
+    # NXP FlexPWM format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
-    search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE)
+    search = re.search('PWM::(PWM[0-9]+).*PWM::Submodule([0-9])', line, re.IGNORECASE)
     if search:
-        # imxrt uses a 1:1 timer group to channel association
+        return str(timer_names.index((search.group(1) + '_' +  search.group(2))))
-        return str(num_channels_already_found)
 
     return None
 
+def imxrt_is_dshot(line):
+
+    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
+    search = re.search('(initIOPWMDshot)', line, re.IGNORECASE)
+    if search:
+        return True
+
+    return False
+
 def get_timer_groups(timer_config_file, verbose=False):
     with open(timer_config_file, 'r') as f:
         timer_config = f.read()
 
     # timers
-    dshot_support = {} # key: timer
+    dshot_support = {str(i): False for i in range(16)}
     timers_start_marker = 'io_timers_t io_timers'
     timers_start = timer_config.find(timers_start_marker)
     if timers_start == -1:
@@ -69,6 +77,7 @@ def get_timer_groups(timer_config_file, verbose=False):
     open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose)
     timers_str = timer_config[open_idx:close_idx]
     timers = []
+    timer_names = []
     for line in timers_str.splitlines():
         line = line.strip()
         if len(line) == 0 or line.startswith('//'):
@@ -77,14 +86,11 @@ def get_timer_groups(timer_config_file, verbose=False):
 
         if timer_type == 'imxrt':
             if verbose: print('imxrt timer found')
-            max_num_channels = 16 # Just add a fixed number of timers
+            timer_names.append(timer)
-            timers = [str(i) for i in range(max_num_channels)]
+            if imxrt_is_dshot(line):
-            dshot_support = {str(i): False for i in range(max_num_channels)}
+                dshot_support[str(len(timers))] = True
-            for i in range(8): # First 8 channels support dshot
+            timers.append(str(len(timers)))
-                dshot_support[str(i)] = True
+        elif timer:
-            break
-
-        if timer:
             if verbose: print('found timer def: {:}'.format(timer))
             dshot_support[timer] = 'DMA' in line
             timers.append(timer)
@@ -111,7 +117,7 @@ def get_timer_groups(timer_config_file, verbose=False):
             continue
 
         if verbose: print('--'+line+'--')
-        timer = extract_timer_from_channel(line, len(channel_timers))
+        timer = extract_timer_from_channel(line, timer_names)
 
         if timer:
             if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line))

boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig

@@ -83,7 +83,6 @@ CONFIG_IMXRT_ENET=y
 CONFIG_IMXRT_FLEXCAN1=y
 CONFIG_IMXRT_FLEXCAN2=y
 CONFIG_IMXRT_FLEXCAN3=y
-CONFIG_IMXRT_FLEXIO1=y
 CONFIG_IMXRT_FLEXSPI2=y
 CONFIG_IMXRT_GPIO13_IRQ=y
 CONFIG_IMXRT_GPIO1_0_15_IRQ=y

boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c

@@ -114,7 +114,7 @@ const struct clock_configuration_s g_initial_clkconfig = {
 			.div           = 1,
 			.mux           = ACMP_CLK_ROOT_OSC_RC_48M_DIV2,
 		},
-		.flexio1_clk_root =
+		.flexio1_clk_root = /* 240 / 2 = 120Mhz */
 		{
 			.enable        = 1,
 			.div           = 2,

boards/px4/fmu-v6xrt/src/timer_config.cpp

@@ -91,14 +91,14 @@
 
 
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO23_1, 23),
+	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0),
-	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO25_1, 25),
+	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1),
-	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO27_1, 27),
+	initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO06_1, 6),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO08_1, 8),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO10_1, 10),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2),
-	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3, GPIO_FLEXIO1_FLEXIO19_1, 19),
+	initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3),
-	initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO29_1, 29),
+	initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0),
 	initIOPWM(PWM::FlexPWM3, PWM::Submodule1),
 	initIOPWM(PWM::FlexPWM3, PWM::Submodule3),
 	initIOPWM(PWM::FlexPWM4, PWM::Submodule0),
@@ -106,14 +106,14 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 };
 
 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
-	/* FMU_CH1  */  initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23),
+	/* FMU_CH1  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23, GPIO_FLEXIO1_FLEXIO23_1, 23),
-	/* FMU_CH2  */  initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25),
+	/* FMU_CH2  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25, GPIO_FLEXIO1_FLEXIO25_1, 25),
-	/* FMU_CH3  */  initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27),
+	/* FMU_CH3  */  initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27, GPIO_FLEXIO1_FLEXIO27_1, 27),
-	/* FMU_CH4  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06),
+	/* FMU_CH4  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06, GPIO_FLEXIO1_FLEXIO06_1, 6),
-	/* FMU_CH5  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08),
+	/* FMU_CH5  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08, GPIO_FLEXIO1_FLEXIO08_1, 8),
-	/* FMU_CH6  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10),
+	/* FMU_CH6  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10, GPIO_FLEXIO1_FLEXIO10_1, 10),
-	/* FMU_CH7  */  initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19),
+	/* FMU_CH7  */  initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19, GPIO_FLEXIO1_FLEXIO19_1, 19),
-	/* FMU_CH8  */  initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29),
+	/* FMU_CH8  */  initIOTimerChannelDshot(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29, GPIO_FLEXIO1_FLEXIO29_1, 29),
 	/* FMU_CH9  */  initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_31),
 	/* FMU_CH10 */  initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_21),
 	/* FMU_CH11 */  initIOTimerChannel(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_00),

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit 693b9e782535f12e6a4ab657c7a0c3bd92b45fb1
+Subproject commit 837e0eccab44a8e4cb44e076111eae43f92f61aa

platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c

@@ -33,145 +33,424 @@
  ****************************************************************************/
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/micro_hal.h>
+#include <px4_platform_common/log.h>
 #include <imxrt_flexio.h>
+#include <hardware/imxrt_flexio.h>
+#include <imxrt_periphclks.h>
 #include <px4_arch/dshot.h>
 #include <px4_arch/io_timer.h>
 #include <drivers/drv_dshot.h>
 #include <stdio.h>
+#include "barriers.h"
 
 #include "arm_internal.h"
 
+#define FLEXIO_BASE			IMXRT_FLEXIO1_BASE
+#define FLEXIO_PREQ			120000000
 #define DSHOT_TIMERS			FLEXIO_SHIFTBUFNIS_COUNT
 #define DSHOT_THROTTLE_POSITION		5u
 #define DSHOT_TELEMETRY_POSITION	4u
 #define NIBBLES_SIZE 			4u
 #define DSHOT_NUMBER_OF_NIBBLES		3u
 
+#if defined(IOMUX_PULL_UP_47K)
+#define IOMUX_PULL_UP IOMUX_PULL_UP_47K
+#endif
+
+static const uint32_t gcr_decode[32] = {
+	0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+	0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF,
+	0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7,
+	0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0
+};
+
+uint32_t erpms[DSHOT_TIMERS];
+
+typedef enum {
+	DSHOT_START = 0,
+	DSHOT_12BIT_TRANSFERRED,
+	DSHOT_TRANSMIT_COMPLETE,
+	BDSHOT_RECEIVE,
+	BDSHOT_RECEIVE_COMPLETE,
+} dshot_state;
+
 typedef struct dshot_handler_t {
 	bool			init;
 	uint32_t 		data_seg1;
 	uint32_t 		irq_data;
+	dshot_state             state;
+	bool			bdshot;
+	uint32_t                raw_response;
+	uint16_t                erpm;
+	uint32_t		crc_error_cnt;
+	uint32_t		frame_error_cnt;
+	uint32_t		no_response_cnt;
 } dshot_handler_t;
 
 static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {};
 
-struct flexio_dev_s *flexio_s;
+static uint32_t dshot_tcmp;
+static uint32_t bdshot_tcmp;
+static uint32_t dshot_mask;
+static uint32_t bdshot_recv_mask;
+static uint32_t bdshot_parsed_recv_mask;
+
+static inline uint32_t flexio_getreg32(uint32_t offset)
+{
+	return getreg32(FLEXIO_BASE + offset);
+}
+
+static inline void flexio_modifyreg32(unsigned int offset,
+				      uint32_t clearbits,
+				      uint32_t setbits)
+{
+	modifyreg32(FLEXIO_BASE + offset, clearbits, setbits);
+}
+
+static inline void flexio_putreg32(uint32_t value, uint32_t offset)
+{
+	putreg32(value, FLEXIO_BASE + offset);
+}
+
+static inline void enable_shifter_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, 0, mask);
+}
+
+static inline void disable_shifter_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, mask, 0);
+}
+
+static inline uint32_t get_shifter_status_flags(void)
+{
+	return flexio_getreg32(IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
+}
+
+static inline void clear_shifter_status_flags(uint32_t mask)
+{
+	flexio_putreg32(mask, IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
+}
+
+static inline void enable_timer_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, 0, mask);
+}
+
+static inline void disable_timer_status_interrupts(uint32_t mask)
+{
+	flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, mask, 0);
+}
+
+static inline uint32_t get_timer_status_flags(void)
+{
+	return flexio_getreg32(IMXRT_FLEXIO_TIMSTAT_OFFSET);
+}
+
+static inline void clear_timer_status_flags(uint32_t mask)
+{
+	flexio_putreg32(mask, IMXRT_FLEXIO_TIMSTAT_OFFSET);
+}
+
+static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted)
+{
+	/* Disable Shifter */
+	flexio_putreg32(0, IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
+
+	/* No start bit, stop bit low */
+	flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) |
+			FLEXIO_SHIFTCFG_PWIDTH(0) |
+			FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_LOW) |
+			FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE),
+			IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4);
+
+	/* Transmit mode, output to FXIO pin, inverted output for bdshot */
+	flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) |
+			FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) |
+			FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT) |
+			FLEXIO_SHIFTCTL_PINSEL(pin) |
+			FLEXIO_SHIFTCTL_PINPOL(inverted) |
+			FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_TRANSMIT),
+			IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
+
+	/* Start transmitting on trigger, disable on compare */
+	flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET) |
+			FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) |
+			FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_NEVER) |
+			FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) |
+			FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH) |
+			FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_DISABLED) |
+			FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_DISABLED),
+			IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4);
+
+	flexio_putreg32(timcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
+
+	/* Baud mode, Trigger on shifter write */
+	flexio_putreg32(FLEXIO_TIMCTL_TRGSEL((4 * channel) + 1) |
+			FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW) |
+			FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) |
+			FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
+			FLEXIO_TIMCTL_PINSEL(0) |
+			FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
+			FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT),
+			IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+
+}
 
 static int flexio_irq_handler(int irq, void *context, void *arg)
 {
+	uint32_t flags = get_shifter_status_flags();
+	uint32_t channel;
 
-	uint32_t flags = flexio_s->ops->get_shifter_status_flags(flexio_s);
+	for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) {
-	uint32_t instance;
+		if (flags & (1 << channel)) {
+			disable_shifter_status_interrupts(1 << channel);
 
-	for (instance = 0; flags && instance < DSHOT_TIMERS; instance++) {
+			if (dshot_inst[channel].irq_data != 0) {
-		if (flags & (1 << instance)) {
+				flexio_putreg32(dshot_inst[channel].irq_data, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
-			flexio_s->ops->disable_shifter_status_interrupts(flexio_s, (1 << instance));
+				dshot_inst[channel].irq_data = 0;
-			flexio_s->ops->disable_timer_status_interrupts(flexio_s, (1 << instance));
 
-			if (dshot_inst[instance].irq_data != 0) {
+			} else if (dshot_inst[channel].irq_data == 0 && dshot_inst[channel].state == BDSHOT_RECEIVE) {
-				uint32_t buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, instance);
+				dshot_inst[channel].state = BDSHOT_RECEIVE_COMPLETE;
-				putreg32(dshot_inst[instance].irq_data, IMXRT_FLEXIO1_BASE + buf_adr);
+				dshot_inst[channel].raw_response = flexio_getreg32(IMXRT_FLEXIO_SHIFTBUFBIS0_OFFSET + channel * 0x4);
-				dshot_inst[instance].irq_data = 0;
+
+				bdshot_recv_mask |= (1 << channel);
+
+				if (bdshot_recv_mask == dshot_mask) {
+					// Received telemetry on all channels
+					// Schedule workqueue?
 				}
 			}
 		}
+	}
+
+	flags = get_timer_status_flags();
+
+	for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) {
+		clear_timer_status_flags(1 << channel);
+
+		if (flags & (1 << channel)) {
+			if (dshot_inst[channel].state == DSHOT_START) {
+				dshot_inst[channel].state = DSHOT_12BIT_TRANSFERRED;
+
+			} else if (!dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) {
+				dshot_inst[channel].state = DSHOT_TRANSMIT_COMPLETE;
+
+			} else if (dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) {
+				disable_shifter_status_interrupts(1 << channel);
+				dshot_inst[channel].state = BDSHOT_RECEIVE;
+
+				/* Transmit done, disable timer and reconfigure to receive*/
+				flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+
+				/* Input data from pin, no start/stop bit*/
+				flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) |
+						FLEXIO_SHIFTCFG_PWIDTH(0) |
+						FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_DISABLE) |
+						FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_SHIFT),
+						IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4);
+
+				/* Shifter receive mdoe, on FXIO pin input */
+				flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) |
+						FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) |
+						FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
+						FLEXIO_SHIFTCTL_PINSEL(timer_io_channels[channel].dshot.flexio_pin) |
+						FLEXIO_SHIFTCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
+						FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_RECEIVE),
+						IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
+
+				/* Make sure there no shifter flags high from transmission */
+				clear_shifter_status_flags(1 << channel);
+
+				/* Enable on pin transition, resychronize through reset on rising edge */
+				flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_AFFECTED_BY_RESET) |
+						FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) |
+						FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_ON_TIMER_PIN_RISING_EDGE) |
+						FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) |
+						FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_BOTH_EDGE) |
+						FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_ENABLE_ON_TIMER_DISABLE) |
+						FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_ENABLED),
+						IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4);
+
+				/* Enable on pin transition, resychronize through reset on rising edge */
+				flexio_putreg32(bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
+
+				/* Trigger on FXIO pin transition, Baud mode */
+				flexio_putreg32(FLEXIO_TIMCTL_TRGSEL(2 * timer_io_channels[channel].dshot.flexio_pin) |
+						FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_HIGH) |
+						FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) |
+						FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) |
+						FLEXIO_TIMCTL_PINSEL(0) |
+						FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) |
+						FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT),
+						IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+
+				/* Enable shifter interrupt for receiving data */
+				enable_shifter_status_interrupts(1 << channel);
+			}
+		}
+
+	}
 
 	return OK;
 }
 
-int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
+
+int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
 {
-	uint32_t timer_compare;
+	/* Calculate dshot timings based on dshot_pwm_freq */
+	dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF);
+	bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF);
 
-	if (dshot_pwm_freq == 150000) {
+	/* Clock FlexIO peripheral */
-		timer_compare = 0x2F8A;
+	imxrt_clockall_flexio1();
 
-	} else if (dshot_pwm_freq == 300000) {
+	/* Reset FlexIO peripheral */
-		timer_compare = 0x2F45;
+	flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
+			   FLEXIO_CTRL_SWRST_MASK);
+	flexio_putreg32(0, IMXRT_FLEXIO_CTRL_OFFSET);
 
-	} else if (dshot_pwm_freq == 600000) {
+	/* Initialize FlexIO peripheral */
-		timer_compare = 0x2F22;
+	flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET,
+			   (FLEXIO_CTRL_DOZEN_MASK |
+			    FLEXIO_CTRL_DBGE_MASK |
+			    FLEXIO_CTRL_FASTACC_MASK |
+			    FLEXIO_CTRL_FLEXEN_MASK),
+			   (FLEXIO_CTRL_DBGE(1) |
+			    FLEXIO_CTRL_FASTACC(1) |
+			    FLEXIO_CTRL_FLEXEN(0)));
 
-	} else if (dshot_pwm_freq == 1200000) {
+	/* FlexIO IRQ handling */
-		timer_compare = 0x2F11;
-
-	} else {
-		// Not supported Dshot frequency
-		return 0;
-	}
-
-	/* Init FlexIO peripheral */
-
-	flexio_s = imxrt_flexio_initialize(1);
 	up_enable_irq(IMXRT_IRQ_FLEXIO1);
-	irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, flexio_s);
+	irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, 0);
+
+	dshot_mask = 0x0;
 
 	for (unsigned channel = 0; (channel_mask != 0) && (channel < DSHOT_TIMERS); channel++) {
 		if (channel_mask & (1 << channel)) {
-			uint8_t timer = timer_io_channels[channel].timer_index;
 
-			if (io_timers[timer].dshot.pinmux == 0) { // board does not configure dshot on this pin
+			if (timer_io_channels[channel].dshot.pinmux == 0) { // board does not configure dshot on this pin
 				continue;
 			}
 
-			imxrt_config_gpio(io_timers[timer].dshot.pinmux);
+			imxrt_config_gpio(timer_io_channels[channel].dshot.pinmux | IOMUX_PULL_UP);
 
-			struct flexio_shifter_config_s shft_cfg;
+			dshot_inst[channel].bdshot = enable_bidirectional_dshot;
-			shft_cfg.timer_select = channel;
-			shft_cfg.timer_polarity = FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE;
-			shft_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT;
-			shft_cfg.pin_select = io_timers[timer].dshot.flexio_pin;
-			shft_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_HIGH;
-			shft_cfg.shifter_mode = FLEXIO_SHIFTER_MODE_TRANSMIT;
-			shft_cfg.parallel_width = 0;
-			shft_cfg.input_source = FLEXIO_SHIFTER_INPUT_FROM_PIN;
-			shft_cfg.shifter_stop = FLEXIO_SHIFTER_STOP_BIT_LOW;
-			shft_cfg.shifter_start = FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE;
 
-			flexio_s->ops->set_shifter_config(flexio_s, channel, &shft_cfg);
+			flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);
-
-			struct flexio_timer_config_s tmr_cfg;
-			tmr_cfg.trigger_select = (4 * channel) + 1;
-			tmr_cfg.trigger_polarity = FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW;
-			tmr_cfg.trigger_source = FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL;
-			tmr_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT_DISABLED;
-			tmr_cfg.pin_select = 0;
-			tmr_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_LOW;
-			tmr_cfg.timer_mode = FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT;
-			tmr_cfg.timer_output = FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET;
-			tmr_cfg.timer_decrement = FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT;
-			tmr_cfg.timer_reset = FLEXIO_TIMER_RESET_NEVER;
-			tmr_cfg.timer_disable = FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE;
-			tmr_cfg.timer_enable = FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH;
-			tmr_cfg.timer_stop = FLEXIO_TIMER_STOP_BIT_DISABLED;
-			tmr_cfg.timer_start = FLEXIO_TIMER_START_BIT_DISABLED;
-			tmr_cfg.timer_compare = timer_compare;
-			flexio_s->ops->set_timer_config(flexio_s, channel, &tmr_cfg);
 
 			dshot_inst[channel].init = true;
+
+			// Mask channel to be active on dshot
+			dshot_mask |= (1 << channel);
 		}
 	}
 
-	flexio_s->ops->enable(flexio_s, true);
+	flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
+			   FLEXIO_CTRL_FLEXEN_MASK);
 
 	return channel_mask;
 }
 
-void up_dshot_trigger(void)
+void up_bdshot_erpm(void)
 {
-	uint32_t buf_adr;
+	uint32_t value;
+	uint32_t erpm;
+	uint32_t csum_data;
 
-	for (uint8_t motor_number = 0; (motor_number < DSHOT_TIMERS); motor_number++) {
+	bdshot_parsed_recv_mask = 0;
-		if (dshot_inst[motor_number].init && dshot_inst[motor_number].data_seg1 != 0) {
+
-			buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, motor_number);
+	// Decode each individual channel
-			putreg32(dshot_inst[motor_number].data_seg1, IMXRT_FLEXIO1_BASE + buf_adr);
+	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
+		if (bdshot_recv_mask & (1 << channel)) {
+			value = ~dshot_inst[channel].raw_response & 0xFFFFF;
+
+			/* if lowest significant isn't 1 we've got a framing error */
+			if (value & 0x1) {
+				/* Decode RLL */
+				value = (value ^ (value >> 1));
+
+				/* Decode GCR */
+				erpm = gcr_decode[value & 0x1fU];
+				erpm |= gcr_decode[(value >> 5U) & 0x1fU] << 4U;
+				erpm |= gcr_decode[(value >> 10U) & 0x1fU] << 8U;
+				erpm |= gcr_decode[(value >> 15U) & 0x1fU] << 12U;
+
+				/* Calculate checksum */
+				csum_data = erpm;
+				csum_data = csum_data ^ (csum_data >> 8U);
+				csum_data = csum_data ^ (csum_data >> NIBBLES_SIZE);
+
+				if ((csum_data & 0xFU) != 0xFU) {
+					dshot_inst[channel].crc_error_cnt++;
+
+				} else {
+					dshot_inst[channel].erpm = ~(erpm >> 4) & 0xFFF;
+					//TODO store this or foward this
+				}
+
+			} else {
+				dshot_inst[channel].frame_error_cnt++;
+			}
 		}
 	}
 
-	flexio_s->ops->clear_timer_status_flags(flexio_s, 0xFF);
+	bdshot_parsed_recv_mask = bdshot_recv_mask;
-	flexio_s->ops->enable_shifter_status_interrupts(flexio_s, 0xFF);
 }
 
+
+
+int up_bdshot_get_erpm(uint8_t channel, int *erpm)
+{
+	if (bdshot_parsed_recv_mask & (1 << channel)) {
+		*erpm = dshot_inst[channel].erpm;
+		return 0;
+	}
+
+	return -1;
+}
+
+
+void up_bdshot_status(void)
+{
+
+	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
+
+		if (dshot_inst[channel].init) {
+			PX4_INFO("Channel %i Last erpm %i value", channel, dshot_inst[channel].erpm);
+			PX4_INFO("CRC errors Frame error No response");
+			PX4_INFO("%10lu %11lu %11lu", dshot_inst[channel].crc_error_cnt, dshot_inst[channel].frame_error_cnt,
+				 dshot_inst[channel].no_response_cnt);
+		}
+	}
+}
+
+void up_dshot_trigger(void)
+{
+	clear_timer_status_flags(0xFF);
+
+	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
+		if (dshot_inst[channel].bdshot && (bdshot_recv_mask & (1 << channel)) == 0) {
+			dshot_inst[channel].no_response_cnt++;
+		}
+
+		if (dshot_inst[channel].init && dshot_inst[channel].data_seg1 != 0) {
+			flexio_putreg32(dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
+		}
+	}
+
+	// Calc data now since we're not event driven
+	if (bdshot_recv_mask != 0x0) {
+		up_bdshot_erpm();
+		bdshot_recv_mask = 0x0;
+	}
+
+	clear_timer_status_flags(0xFF);
+	enable_shifter_status_interrupts(0xFF);
+	enable_timer_status_interrupts(0xFF);
+}
+
+/* Expand packet from 16 bits 48 to get T0H and T1H timing */
 uint64_t dshot_expand_data(uint16_t packet)
 {
 	unsigned int mask;
@@ -197,16 +476,22 @@ uint64_t dshot_expand_data(uint16_t packet)
 * bit 	12		- dshot telemetry enable/disable
 * bits 	13-16	- XOR checksum
 **/
-void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry)
+void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
 {
-	if (motor_number < DSHOT_TIMERS && dshot_inst[motor_number].init) {
+	if (channel < DSHOT_TIMERS && dshot_inst[channel].init) {
+		uint16_t csum_data;
 		uint16_t packet = 0;
 		uint16_t checksum = 0;
 
 		packet |= throttle << DSHOT_THROTTLE_POSITION;
 		packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION;
 
-		uint16_t csum_data = packet;
+		if (dshot_inst[channel].bdshot) {
+			csum_data = ~packet;
+
+		} else {
+			csum_data = packet;
+		}
 
 		/* XOR checksum calculation */
 		csum_data >>= NIBBLES_SIZE;
@@ -219,8 +504,19 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet
 		packet |= (checksum & 0x0F);
 
 		uint64_t dshot_expanded = dshot_expand_data(packet);
-		dshot_inst[motor_number].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF);
+		dshot_inst[channel].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF);
-		dshot_inst[motor_number].irq_data = (uint32_t)(dshot_expanded >> 24);
+		dshot_inst[channel].irq_data = (uint32_t)(dshot_expanded >> 24);
+		dshot_inst[channel].state = DSHOT_START;
+
+		if (dshot_inst[channel].bdshot) {
+
+			flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
+			disable_shifter_status_interrupts(1 << channel);
+
+			flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);
+
+			clear_timer_status_flags(0xFF);
+		}
 	}
 }
 

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

@@ -87,7 +87,6 @@ typedef struct io_timers_t {
 	uint32_t  clock_register;      /* SIM_SCGCn */
 	uint32_t  clock_bit;           /* SIM_SCGCn bit pos */
 	uint32_t  vectorno;            /* IRQ number */
-	dshot_conf_t	dshot;
 } io_timers_t;
 
 typedef struct io_timers_channel_mapping_element_t {
@@ -112,6 +111,7 @@ typedef struct timer_io_channels_t {
 	uint8_t   sub_module;          /* 0 based sub module offset */
 	uint8_t   sub_module_bits;     /* LDOK and CLDOK bits */
 	uint8_t   timer_channel;       /* Unused */
+	dshot_conf_t	dshot;
 } timer_io_channels_t;
 
 #define SM0           0

platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c

@@ -255,6 +255,34 @@ static inline int channels_timer(unsigned channel)
 	return timer_io_channels[channel].timer_index;
 }
 
+static uint32_t get_timer_channels(unsigned timer)
+{
+	uint32_t channels = 0;
+	static uint32_t channels_cache[MAX_IO_TIMERS] = {0};
+
+	if (validate_timer_index(timer) < 0) {
+		return channels;
+
+	} else {
+		if (channels_cache[timer] == 0) {
+			/* Gather the channel bits that belong to the timer */
+
+			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;
+
+			for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) {
+				channels |= 1 << chan_index;
+			}
+
+			/* cache them */
+
+			channels_cache[timer] = channels;
+		}
+	}
+
+	return channels_cache[timer];
+}
+
 static uint32_t get_channel_mask(unsigned channel)
 {
 	return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0;
@@ -391,20 +419,34 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 	return rv;
 }
 
-static int timer_set_rate(unsigned channel, unsigned rate)
+static int timer_set_rate(unsigned timer, unsigned rate)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
+
+	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
+		if ((1 << channel) & channels) {
 			rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
 							   ;
 			rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1;
 			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+
+		}
+	}
+
 	px4_leave_critical_section(flags);
 	return 0;
 }
 
-static inline void io_timer_set_oneshot_mode(unsigned channel)
+static inline void io_timer_set_oneshot_mode(unsigned timer)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
+
+	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
+		if ((1 << channel) & channels) {
 			uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
 			rvalue &= ~SMCTRL_PRSC_MASK;
 			rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD;
@@ -412,13 +454,21 @@ static inline void io_timer_set_oneshot_mode(unsigned channel)
 							   ;
 			rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
 			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+		}
+	}
+
 	px4_leave_critical_section(flags);
 
 }
 
-static inline void io_timer_set_PWM_mode(unsigned channel)
+static inline void io_timer_set_PWM_mode(unsigned timer)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
+
+	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
+		if ((1 << channel) & channels) {
 			uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
 			rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD);
 			rvalue |= SMCTRL_PRSC_DIV16;
@@ -426,6 +476,9 @@ static inline void io_timer_set_PWM_mode(unsigned channel)
 							   ;
 			rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
 			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
+		}
+	}
+
 	px4_leave_critical_section(flags);
 }
 
@@ -530,10 +583,10 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
 			break;
 		}
 
-		uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
+		int channels = get_timer_channels(timer);
-		uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
 
-		for (uint32_t chan = first_channel_index; chan < last_channel_index; chan++) {
+		for (uint32_t chan = 0; chan < DIRECT_PWM_OUTPUT_CHANNELS; ++chan) {
+			if ((1 << chan) & channels) {
 
 				/* Clear all Faults */
 				rFSTS0(timer) = FSTS_FFLAG_MASK;
@@ -553,9 +606,11 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
 				rDTSRCSEL(timer) = 0;
 				rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module);
 				rMCTRL(timer) = timer_io_channels[chan].sub_module_bits;
-			io_timer_set_PWM_mode(chan);
-			timer_set_rate(chan, 50);
 			}
+		}
+
+		io_timer_set_PWM_mode(timer);
+		timer_set_rate(timer, 50);
 
 		px4_leave_critical_section(flags);
 	}
@@ -818,8 +873,7 @@ uint16_t io_channel_get_ccr(unsigned channel)
 	return value;
 }
 
-// The rt has 1:1 group to channel
+uint32_t io_timer_get_group(unsigned timer)
-uint32_t io_timer_get_group(unsigned group)
 {
-	return get_channel_mask(group);
+	return get_timer_channels(timer);
 }

platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt

@@ -36,7 +36,7 @@ add_subdirectory(../imxrt/adc adc)
 add_subdirectory(../imxrt/board_critmon board_critmon)
 add_subdirectory(../imxrt/board_hw_info board_hw_info)
 add_subdirectory(../imxrt/board_reset board_reset)
-#add_subdirectory(../imxrt/dshot dshot)
+add_subdirectory(../imxrt/dshot dshot)
 add_subdirectory(../imxrt/hrt hrt)
 add_subdirectory(../imxrt/led_pwm led_pwm)
 add_subdirectory(../imxrt/io_pins io_pins)

platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/dshot.h

@@ -0,0 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#pragma once
+
+
+#include "../../../imxrt/include/px4_arch/dshot.h"

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

@@ -41,6 +41,7 @@
 #include <nuttx/irq.h>
 
 #include <drivers/drv_hrt.h>
+#include "dshot.h"
 
 #pragma once
 __BEGIN_DECLS
@@ -110,6 +111,7 @@ typedef struct timer_io_channels_t {
 	uint8_t   sub_module;          /* 0 based sub module offset */
 	uint8_t   sub_module_bits;     /* LDOK and CLDOK bits */
 	uint8_t   timer_channel;       /* Unused */
+	dshot_conf_t dshot;
 } timer_io_channels_t;
 
 #define SM0           0

platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h

@@ -601,6 +601,16 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
 	return ret;
 }
 
+static inline constexpr timer_io_channels_t initIOTimerChannelDshot(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
+		PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint32_t flexio_pin)
+{
+	timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, pwm_config, pad);
+
+	ret.dshot.pinmux = dshot_pinmux;
+	ret.dshot.flexio_pin = flexio_pin;
+	return ret;
+}
+
 static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
 {
 	io_timers_t ret{};
@@ -609,3 +619,14 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm
 	ret.submodle = sub;
 	return ret;
 }
+
+
+
+static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
+{
+	io_timers_t ret{};
+
+	ret.base = getFlexPWMBaseRegister(pwm);
+	ret.submodle = sub;
+	return ret;
+}

platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h

@@ -690,6 +690,16 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t
 	return ret;
 }
 
+static inline constexpr timer_io_channels_t initIOTimerChannelDshot(const io_timers_t io_timers_conf[MAX_IO_TIMERS],
+		PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint32_t flexio_pin)
+{
+	timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, pwm_config, pad);
+
+	ret.dshot.pinmux = dshot_pinmux;
+	ret.dshot.flexio_pin = flexio_pin;
+	return ret;
+}
+
 static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
 {
 	io_timers_t ret{};
@@ -699,14 +709,13 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm
 	return ret;
 }
 
-static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub, uint32_t pinmux,
+
-		uint32_t flexio_pin)
+
+static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub)
 {
 	io_timers_t ret{};
 
 	ret.base = getFlexPWMBaseRegister(pwm);
 	ret.submodle = sub;
-	ret.dshot.pinmux = pinmux;
-	ret.dshot.flexio_pin = flexio_pin;
 	return ret;
 }

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

@@ -82,7 +82,7 @@ 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)
+int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
 {
 	unsigned buffer_offset = 0;
 
@@ -152,6 +152,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
+int up_bdshot_get_erpm(uint8_t channel, int *erpm)
+{
+	// Not implemented
+	return -1;
+}
+
+void up_bdshot_status(void)
+{
+}
+
 void up_dshot_trigger(void)
 {
 	for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) {

src/drivers/drv_dshot.h

@@ -91,7 +91,7 @@ typedef enum {
  * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
  * @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_pwm_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 +137,19 @@ __EXPORT extern void up_dshot_trigger(void);
  */
 __EXPORT extern int up_dshot_arm(bool armed);
 
+/**
+ * Print bidrectional dshot status
+ */
+__EXPORT extern void up_bdshot_status(void);
+
+
+/**
+ * Get bidrectional dshot erpm for a channel
+ * @param channel	Dshot channel
+ * @param erpm		pointer to write the erpm value
+ * @return <0 on error, OK on succes
+ */
+__EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm);
+
+
 __END_DECLS

src/drivers/dshot/DShot.cpp

@@ -144,7 +144,9 @@ void DShot::enable_dshot_outputs(const bool enabled)
 			}
 		}
 
-		int ret = up_dshot_init(_output_mask, dshot_frequency);
+		_bdshot = _param_bidirectional_enable.get();
+
+		int ret = up_dshot_init(_output_mask, dshot_frequency, _bdshot);
 
 		if (ret < 0) {
 			PX4_ERR("up_dshot_init failed (%i)", ret);
@@ -167,6 +169,10 @@ void DShot::enable_dshot_outputs(const bool enabled)
 		}
 
 		_outputs_initialized = true;
+
+		if (_bdshot) {
+			init_telemetry(NULL);
+		}
 	}
 
 	if (_outputs_initialized) {
@@ -206,17 +212,20 @@ void DShot::init_telemetry(const char *device)
 
 	_telemetry->esc_status_pub.advertise();
 
+	if (device != NULL) {
 		int ret = _telemetry->handler.init(device);
 
 		if (ret != 0) {
 			PX4_ERR("telemetry init failed (%i)", ret);
 		}
+	}
 
 	update_telemetry_num_motors();
 }
 
-void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
+int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
 {
+	int ret = 0;
 	// fill in new motor data
 	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
 
@@ -243,14 +252,59 @@ void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTele
 		esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
 		esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1;
 
-		_telemetry->esc_status_pub.update();
+		ret = 1; // Indicate we wrapped, so we publish data
-
-		// reset esc data (in case a motor times out, so we won't send stale data)
-		memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc));
-		esc_status.esc_online_flags = 0;
 	}
 
 	_telemetry->last_telemetry_index = telemetry_index;
+
+	return ret;
+}
+
+void DShot::publish_esc_status(void)
+{
+	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+
+	// clear data of the esc that are offline
+	for (uint8_t channel = 0; (channel < _telemetry->last_telemetry_index); channel++) {
+		if ((esc_status.esc_online_flags & (1 << channel)) == 0) {
+			memset(&esc_status.esc[channel], 0, sizeof(struct esc_report_s));
+		}
+	}
+
+	// ESC telem wrap around or bdshot update
+	_telemetry->esc_status_pub.update();
+
+	// reset esc online flags
+	esc_status.esc_online_flags = 0;
+}
+
+int DShot::handle_new_bdshot_erpm(void)
+{
+	int num_erpms = 0;
+	int erpm;
+	uint8_t channel;
+	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
+
+	esc_status.timestamp = hrt_absolute_time();
+	esc_status.counter = _esc_status_counter++;
+	esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
+	esc_status.esc_armed_flags = _outputs_on;
+
+	for (channel = 0; channel < 8; channel++) {
+		if (up_bdshot_get_erpm(channel, &erpm) == 0) {
+			num_erpms++;
+			esc_status.esc_online_flags |= 1 << channel;
+			esc_status.esc[channel].timestamp = hrt_absolute_time();
+			esc_status.esc[channel].esc_rpm = (erpm * 100) /
+							  (_param_mot_pole_count.get() / 2);
+			esc_status.esc[channel].actuator_function = _telemetry->actuator_functions[channel];
+		}
+
+	}
+
+	esc_status.esc_count = num_erpms;
+
+	return num_erpms;
 }
 
 int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)
@@ -463,6 +517,7 @@ void DShot::Run()
 
 	if (_telemetry) {
 		int telem_update = _telemetry->handler.update();
+		int need_to_publish = 0;
 
 		// Are we waiting for ESC info?
 		if (_waiting_for_esc_info) {
@@ -472,10 +527,21 @@ void DShot::Run()
 			}
 
 		} else if (telem_update >= 0) {
-			handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData());
+			need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData());
+		}
+
+		if (_bdshot) {
+			// Add bdshot data to esc status
+			need_to_publish += handle_new_bdshot_erpm();
+		}
+
+		if (need_to_publish > 0) {
+			// ESC telem wrap around or bdshot update
+			publish_esc_status();
 		}
 	}
 
+
 	if (_parameter_update_sub.updated()) {
 		update_params();
 	}
@@ -713,6 +779,9 @@ int DShot::print_status()
 		_telemetry->handler.printStatus();
 	}
 
+	/* Print dshot status */
+	up_bdshot_status();
+
 	return 0;
 }
 

src/drivers/dshot/DShot.h

@@ -131,7 +131,11 @@ private:
 
 	void init_telemetry(const char *device);
 
-	void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
+	int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
+
+	void publish_esc_status(void);
+
+	int handle_new_bdshot_erpm(void);
 
 	int request_esc_info();
 
@@ -158,6 +162,7 @@ private:
 	bool _outputs_initialized{false};
 	bool _outputs_on{false};
 	bool _waiting_for_esc_info{false};
+	bool _bdshot{false};
 
 	static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS};
 	uint32_t _output_mask{0};
@@ -169,12 +174,14 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+	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/DShotTelemetry.cpp

@@ -183,6 +183,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding)
 				  _latest_data.erpm);
 			++_num_successful_responses;
 			successful_decoding = true;
+
+		} else {
+			++_num_checksum_errors;
 		}
 
 		return true;
@@ -195,6 +198,7 @@ void DShotTelemetry::printStatus() const
 {
 	PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses);
 	PX4_INFO("Number of timeouts: %i", _num_timeouts);
+	PX4_INFO("Number of CRC errors: %i", _num_checksum_errors);
 }
 
 uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)

src/drivers/dshot/DShotTelemetry.h

@@ -140,4 +140,5 @@ private:
 	// statistics
 	int _num_timeouts{0};
 	int _num_successful_responses{0};
+	int _num_checksum_errors{0};
 };

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