[WIP] initial bidirectional dshot support

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 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 
 	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 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 	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