From eb57942f02c675c487ec1cc6a4802165318de166 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Thu, 21 Mar 2024 16:06:04 +0100 Subject: [PATCH] dshot: fix clearing out esc status --- .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 2 +- .../src/px4/nxp/imxrt/io_pins/io_timer.c | 14 +++++++--- .../src/px4/stm/stm32_common/dshot/dshot.c | 6 +++++ src/drivers/dshot/DShot.cpp | 26 +++++++++++++++---- src/drivers/dshot/DShot.h | 2 ++ src/drivers/dshot/DShotTelemetry.cpp | 4 +++ src/drivers/dshot/DShotTelemetry.h | 1 + 7 files changed, 45 insertions(+), 10 deletions(-) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index 15fffdfaf2eb..cfd95b936bf6 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -440,7 +440,7 @@ void up_dshot_trigger(void) } // Calc data now since we're not event driven - if(bdshot_recv_mask != 0x0) { + if (bdshot_recv_mask != 0x0) { up_bdshot_erpm(); bdshot_recv_mask = 0x0; } diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c index 77534c35ffaa..4fa7d7fdb6fb 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c @@ -424,15 +424,17 @@ static int timer_set_rate(unsigned timer, unsigned rate) int channels = get_timer_channels(timer); irqstate_t flags = px4_enter_critical_section(); + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { if ((1 << channel) & channels) { rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; + ; rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1; rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; } } + px4_leave_critical_section(flags); return 0; } @@ -442,17 +444,19 @@ static inline void io_timer_set_oneshot_mode(unsigned timer) int channels = get_timer_channels(timer); irqstate_t flags = px4_enter_critical_section(); + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { if ((1 << channel) & channels) { uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); rvalue &= ~SMCTRL_PRSC_MASK; rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD; rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; + ; rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; } } + px4_leave_critical_section(flags); } @@ -462,17 +466,19 @@ static inline void io_timer_set_PWM_mode(unsigned timer) int channels = get_timer_channels(timer); irqstate_t flags = px4_enter_critical_section(); + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { if ((1 << channel) & channels) { uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD); rvalue |= SMCTRL_PRSC_DIV16; rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; + ; rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; } } + px4_leave_critical_section(flags); } @@ -596,7 +602,7 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000; rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000; rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module) - : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); + : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); rDTSRCSEL(timer) = 0; rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module); rMCTRL(timer) = timer_io_channels[chan].sub_module_bits; 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 c977c3a60bdc..1f6e0b923dcc 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -152,6 +152,12 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi return ret_val == OK ? channels_init_mask : ret_val; } +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + // Not implemented + return -1; +} + void up_bdshot_status(void) { } diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 569f6619d69c..d5e294609ce1 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -253,10 +253,6 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; 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; @@ -264,6 +260,24 @@ int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelem return ret; } +void DShot::publish_esc_status(void) +{ + esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + + // clear data of the esc that are offline + for (uint8_t 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)); + } + } + + // ESC telem wrap around or bdshot update + _telemetry->esc_status_pub.update(); + + // reset esc online flags + esc_status.esc_online_flags = 0; +} + int DShot::handle_new_bdshot_erpm(void) { int num_erpms = 0; @@ -279,9 +293,11 @@ int DShot::handle_new_bdshot_erpm(void) for (channel = 0; channel < 8; channel++) { if (up_bdshot_get_erpm(channel, &erpm) == 0) { num_erpms++; + esc_status.esc_online_flags |= 1 << channel; 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[channel].actuator_function = _telemetry->actuator_functions[channel]; } } @@ -521,7 +537,7 @@ void DShot::Run() if (need_to_publish > 0) { // ESC telem wrap around or bdshot update - _telemetry->esc_status_pub.update(); + publish_esc_status(); } } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index a8dfd92239a2..50b8d9eaa012 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -133,6 +133,8 @@ class DShot final : public ModuleBase, public OutputModuleInterface int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); + void publish_esc_status(void); + int handle_new_bdshot_erpm(void); int request_esc_info(); diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 14a21d5654e0..f0444bff1950 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -183,6 +183,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding) _latest_data.erpm); ++_num_successful_responses; successful_decoding = true; + + } else { + ++_num_checksum_errors; } return true; @@ -195,6 +198,7 @@ void DShotTelemetry::printStatus() const { PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses); PX4_INFO("Number of timeouts: %i", _num_timeouts); + PX4_INFO("Number of CRC errors: %i", _num_checksum_errors); } uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed) diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index a5c549c47676..1164d1e2008c 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -140,4 +140,5 @@ class DShotTelemetry // statistics int _num_timeouts{0}; int _num_successful_responses{0}; + int _num_checksum_errors{0}; };