From f00b6dcedb1cea51d053f822ebabf0d91c4a63ca Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Fri, 22 Dec 2023 11:23:07 +0100 Subject: [PATCH] drivers: dshot: prepare to extend for bidrectional dshot --- .../src/px4/stm/stm32_common/dshot/dshot.c | 6 ++++- src/drivers/drv_dshot.h | 7 +++++- src/drivers/dshot/DShot.cpp | 23 ++++++++++++++++++- src/drivers/dshot/DShot.h | 7 +++++- src/drivers/dshot/module.yaml | 10 ++++++++ 5 files changed, 49 insertions(+), 4 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 5b501850668a..c977c3a60bdc 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/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++) { diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 2e8dab2d6db9..8da4cd0fa453 100644 --- a/src/drivers/drv_dshot.h +++ b/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 diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5ae0150efba9..5584ab5b5d6c 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/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; } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 4f314cebb75f..2be6b648f8f1 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -143,6 +143,8 @@ class DShot final : public ModuleBase, 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, public OutputModuleInterface uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationData _esc_status_pub{ORB_ID(esc_status)}; + uint16_t _esc_status_counter{0}; DEFINE_PARAMETERS( (ParamFloat) _param_dshot_min, (ParamBool) _param_dshot_3d_enable, (ParamInt) _param_dshot_3d_dead_h, (ParamInt) _param_dshot_3d_dead_l, - (ParamInt) _param_mot_pole_count + (ParamInt) _param_mot_pole_count, + (ParamBool) _param_bidirectional_enable ) }; diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index edd38edd23f1..e2dcf8bc97e8 100644 --- a/src/drivers/dshot/module.yaml +++ b/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