From 5d2fda61728a2efee1c239dece6c648f19f8b4fd Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Wed, 27 Mar 2024 13:08:54 +0100 Subject: [PATCH] dshot: bdshot fix esc offline/online checks --- .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 15 ++++++++- .../src/px4/stm/stm32_common/dshot/dshot.c | 6 ++++ src/drivers/drv_dshot.h | 9 ++++++ src/drivers/dshot/DShot.cpp | 31 ++++++++++++++----- src/drivers/dshot/DShot.h | 1 + 5 files changed, 54 insertions(+), 8 deletions(-) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index f4b5a1fcc9..d697234c9c 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -86,8 +86,11 @@ typedef struct dshot_handler_t { uint32_t crc_error_cnt; uint32_t frame_error_cnt; uint32_t no_response_cnt; + uint32_t last_no_response_cnt; } dshot_handler_t; +#define BDSHOT_OFFLINE_COUNT 400 // If there are no responses for 400 setpoints ESC is offline + static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {}; static uint32_t dshot_tcmp; @@ -390,6 +393,7 @@ void up_bdshot_erpm(void) } else { dshot_inst[channel].erpm = ~(erpm >> 4) & 0xFFF; bdshot_parsed_recv_mask |= (1 << channel); + dshot_inst[channel].last_no_response_cnt = dshot_inst[channel].no_response_cnt; } } else { @@ -411,6 +415,14 @@ int up_bdshot_get_erpm(uint8_t channel, int *erpm) return -1; } +int up_bdshot_channel_status(uint8_t channel) +{ + if (channel < DSHOT_TIMERS) { + return ((dshot_inst[channel].no_response_cnt - dshot_inst[channel].last_no_response_cnt) < BDSHOT_OFFLINE_COUNT); + } + + return -1; +} void up_bdshot_status(void) { @@ -418,7 +430,8 @@ void up_bdshot_status(void) for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { if (dshot_inst[channel].init) { - PX4_INFO("Channel %i Last erpm %i value", channel, dshot_inst[channel].erpm); + PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline", + dshot_inst[channel].erpm); PX4_INFO("CRC errors Frame error No response"); PX4_INFO("%10lu %11lu %11lu", dshot_inst[channel].crc_error_cnt, dshot_inst[channel].frame_error_cnt, dshot_inst[channel].no_response_cnt); 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 1f6e0b923d..00491e9768 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -158,6 +158,12 @@ int up_bdshot_get_erpm(uint8_t channel, int *erpm) return -1; } +int up_bdshot_channel_status(uint8_t channel) +{ + // Not implemented + return -1; +} + void up_bdshot_status(void) { } diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 3d9bed3fda..81c92b4d50 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -152,4 +152,13 @@ __EXPORT extern void up_bdshot_status(void); __EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm); +/** + * Get bidrectional dshot status for a channel + * @param channel Dshot channel + * @param erpm pointer to write the erpm value + * @return <0 on error / not supported, 0 on offline, 1 on online + */ +__EXPORT extern int up_bdshot_channel_status(uint8_t channel); + + __END_DECLS diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index d5e294609c..26ad1d63b3 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -159,6 +159,9 @@ void DShot::enable_dshot_outputs(const bool enabled) for (unsigned i = 0; i < _num_outputs; ++i) { if (((1 << i) & _output_mask) == 0) { _mixing_output.disableFunction(i); + + } else { + _dshot_esc_count++; } } @@ -248,9 +251,6 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; esc_status.esc_count = _telemetry->handler.numMotors(); ++esc_status.counter; - // FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout - esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; - esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; ret = 1; // Indicate we wrapped, so we publish data } @@ -263,14 +263,31 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem void DShot::publish_esc_status(void) { esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + uint8_t channel; // clear data of the esc that are offline - for (uint8_t channel = 0; (channel < _telemetry->last_telemetry_index); channel++) { + for (channel = 0; (channel < _telemetry->last_telemetry_index); channel++) { if ((esc_status.esc_online_flags & (1 << channel)) == 0) { memset(&esc_status.esc[channel], 0, sizeof(struct esc_report_s)); } } + // FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout + esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; + esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; + + if (_bdshot) { + esc_status.esc_online_flags |= _output_mask; + esc_status.esc_armed_flags |= _output_mask; + esc_status.esc_count = _dshot_esc_count; + + for (channel = 0; (channel < 8); channel++) { + if (up_bdshot_channel_status(channel) == 0) { + esc_status.esc_online_flags &= ~(1 << channel); + } + } + } + // ESC telem wrap around or bdshot update _telemetry->esc_status_pub.update(); @@ -302,8 +319,6 @@ int DShot::handle_new_bdshot_erpm(void) } - esc_status.esc_count = num_erpms; - return num_erpms; } @@ -780,7 +795,9 @@ int DShot::print_status() } /* Print dshot status */ - up_bdshot_status(); + if (_bdshot) { + up_bdshot_status(); + } return 0; } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 50b8d9eaa0..2dcab5696f 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -166,6 +166,7 @@ private: static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; uint32_t _output_mask{0}; + uint32_t _dshot_esc_count{0}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};