diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index e0fad4bb9e..7475d3d584 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/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); 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 5b50185066..c977c3a60b 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 2e8dab2d6d..3d9bed3fda 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,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 diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5ae0150efb..569f6619d6 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/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,7 +252,7 @@ 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)); @@ -251,6 +260,35 @@ void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTele } _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; } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 4f314cebb7..a8dfd92239 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -131,7 +131,9 @@ 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); + + int handle_new_bdshot_erpm(void); int request_esc_info(); @@ -158,6 +160,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 +172,14 @@ private: 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)}; + 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 edd38edd23..e2dcf8bc97 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