drivers: dshot: prepare to extend for bidrectional dshot

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

@@ -89,6 +89,7 @@ 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)
 {
@@ -350,13 +351,14 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
 	return channel_mask;
 }
 
-
 void up_bdshot_erpm(void)
 {
 	uint32_t value;
 	uint32_t erpm;
 	uint32_t csum_data;
 
+	bdshot_parsed_recv_mask = 0;
+
 	// Decode each individual channel
 	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
 		if (bdshot_recv_mask & (1 << channel)) {
@@ -391,13 +393,25 @@ void up_bdshot_erpm(void)
 			}
 		}
 	}
+
+	bdshot_parsed_recv_mask = bdshot_recv_mask;
+}
+
+
+
+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)
 {
-	/* Call this function to calculate last ERPM ideally a workqueue does this.
-	   For now this to debug using the dshot status cli command */
-	up_bdshot_erpm();
 
 	for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
 
@@ -424,6 +438,9 @@ void up_dshot_trigger(void)
 		}
 	}
 
+	// Calc data now since we're not event driven
+	up_bdshot_erpm();
+
 	bdshot_recv_mask = 0x0;
 
 	clear_timer_status_flags(0xFF);

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,10 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq)
 	return ret_val == OK ? channels_init_mask : ret_val;
 }
 
+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();
 
-	int ret = _telemetry->handler.init(device);
+	if (device != NULL) {
+		int ret = _telemetry->handler.init(device);
 
-	if (ret != 0) {
-		PX4_ERR("telemetry init failed (%i)", ret);
+		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,43 @@ 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;
+}
+
+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[channel].timestamp = hrt_absolute_time();
+			esc_status.esc[channel].esc_rpm = (erpm * 100) /
+							  (_param_mot_pole_count.get() / 2);
+		}
+
+	}
+
+	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 +501,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 +511,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
+			_telemetry->esc_status_pub.update();
 		}
 	}
 
+
 	if (_parameter_update_sub.updated()) {
 		update_params();
 	}
@@ -713,6 +763,9 @@ int DShot::print_status()
 		_telemetry->handler.printStatus();
 	}
 
+	/* Print dshot status */
+	up_bdshot_status();
+
 	return 0;
 }
 

src/drivers/dshot/DShot.h

@@ -131,7 +131,9 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 
 	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);
+
+	int handle_new_bdshot_erpm(void);
 
 	int request_esc_info();
 
@@ -158,6 +160,7 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 	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 +172,14 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 	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/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