drivers: dshot: prepare to extend for bidrectional dshot

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,9 @@ __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);
+
 __END_DECLS

src/drivers/dshot/DShot.cpp

@@ -144,7 +144,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 +607,24 @@ void DShot::update_params()
 	}
 }
 
+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];
@@ -713,6 +731,9 @@ int DShot::print_status()
 		_telemetry->handler.printStatus();
 	}
 
+	/* TODO cleanup Print bidirectiona dshot status */
+	up_bdshot_status();
+
 	return 0;
 }
 

src/drivers/dshot/DShot.h

@@ -143,6 +143,8 @@ class DShot final : public ModuleBase<DShot>, public OutputModuleInterface
 
 	void handle_vehicle_commands();
 
+	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{};
 
@@ -169,12 +171,15 @@ 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)};
+	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