From 921e91863a8ddbd6d94569030ac2c5a2185c0aae Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Sun, 23 Nov 2025 18:51:07 +0100 Subject: [PATCH] dshot: IMXRT BDSHOT baud training AM32 bdshot doesn't do clock compensation like BLHeli32 did. Instead we traing BDSHOT timing on known zero value to lock on best bdshot baudrate to receive. Reduces CRC and frame errors on AM32 a lot --- .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 101 ++++++++++++++++-- 1 file changed, 91 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 d0558592a5..7bd2508ddf 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -74,26 +74,40 @@ typedef enum { BDSHOT_RECEIVE_COMPLETE, } dshot_state; -typedef struct dshot_handler_t { +typedef struct dshot_channel_t { bool init; uint32_t data_seg1; uint32_t irq_data; dshot_state state; - bool bdshot; uint32_t raw_response; uint16_t erpm; uint32_t crc_error_cnt; uint32_t frame_error_cnt; uint32_t no_response_cnt; uint32_t last_no_response_cnt; -} dshot_handler_t; + bool bdshot; + uint32_t bdshot_tcmp; + uint32_t bdshot_training_mask; + uint8_t bdshot_training_count; + uint8_t bdshot_training_success; + bool bdshot_training_done; + int8_t bdshot_tcmp_offset; +} dshot_channel_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] = {}; +#define BDSHOT_TCMP_MIN_OFFSET -16 +#define BDSHOT_TCMP_MAX_OFFSET 15 +#define BDSHOT_TCMP_TO_MASK(x) ((x) - BDSHOT_TCMP_MIN_OFFSET) +#define BDSHOT_TRAINING_TRIES 200 +#define BDSHOT_TRAINING_SUCCESS 198 + +#define BDSHOT_ZERO_RESPONSE 0x52951 + +static dshot_channel_t dshot_inst[DSHOT_TIMERS] = {}; static uint32_t dshot_tcmp; -static uint32_t bdshot_tcmp; +static unsigned dshot_speed; static uint32_t dshot_mask; static uint32_t bdshot_recv_mask; static uint32_t bdshot_parsed_recv_mask; @@ -155,6 +169,12 @@ static inline void clear_timer_status_flags(uint32_t mask) flexio_putreg32(mask, IMXRT_FLEXIO_TIMSTAT_OFFSET); } +static inline void flexio_dshot_set_tcmp(uint32_t channel) +{ + dshot_inst[channel].bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_speed * 5 / 4) / 2) + + dshot_inst[channel].bdshot_tcmp_offset) & 0xFF); +} + static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted) { /* Disable Shifter */ @@ -278,7 +298,7 @@ static int flexio_irq_handler(int irq, void *context, void *arg) IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4); /* Enable on pin transition, resychronize through reset on rising edge */ - flexio_putreg32(bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4); + flexio_putreg32(dshot_inst[channel].bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4); /* Trigger on FXIO pin transition, Baud mode */ flexio_putreg32(FLEXIO_TIMCTL_TRGSEL(2 * timer_io_channels[channel].dshot.flexio_pin) | @@ -305,7 +325,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi { /* Calculate dshot timings based on dshot_pwm_freq */ dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF); - bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 3) & 0xFF); + dshot_speed = dshot_pwm_freq; /* Clock FlexIO peripheral */ imxrt_clockall_flexio1(); @@ -340,7 +360,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi imxrt_config_gpio(timer_io_channels[channel].dshot.pinmux | IOMUX_PULL_UP); - dshot_inst[channel].bdshot = enable_bidirectional_dshot; + if (enable_bidirectional_dshot) { + dshot_inst[channel].bdshot = true; + dshot_inst[channel].bdshot_training_mask = 0; + dshot_inst[channel].bdshot_tcmp_offset = BDSHOT_TCMP_MIN_OFFSET; + dshot_inst[channel].bdshot_training_done = false; + flexio_dshot_set_tcmp(channel); + + } else { + dshot_inst[channel].bdshot = false; + } flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot); @@ -357,6 +386,52 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi return channel_mask; } +void up_bdshot_training(uint32_t channel, uint32_t value) +{ + dshot_channel_t *ch = &dshot_inst[channel]; + + if (value == BDSHOT_ZERO_RESPONSE) { + // Count successful responses + ch->bdshot_training_success++; + + } else if ((value & 0x1) == 0) { + // Invalidate frame error immediately + ch->bdshot_training_count = BDSHOT_TRAINING_TRIES - 1; + } + + // Keep count and check if a training round finished + ch->bdshot_training_count++; + + if (ch->bdshot_training_count == BDSHOT_TRAINING_TRIES) { + if (ch->bdshot_training_success >= BDSHOT_TRAINING_SUCCESS) { + ch->bdshot_training_mask |= + (1 << BDSHOT_TCMP_TO_MASK(ch->bdshot_tcmp_offset)); + } + + ch->bdshot_training_count = 0; + ch->bdshot_training_success = 0; + ch->bdshot_tcmp_offset++; + + if (ch->bdshot_tcmp_offset == BDSHOT_TCMP_MAX_OFFSET) { + + if (ch->bdshot_training_mask == 0) { + // No candidates retry + ch->bdshot_tcmp_offset = BDSHOT_TCMP_MIN_OFFSET; + + } else { + // Training done, use mask to find best offset + int low = __builtin_ctz(ch->bdshot_training_mask); + int high = 31 - __builtin_clz(ch->bdshot_training_mask); + ch->bdshot_tcmp_offset = ((low + high) / 2) + BDSHOT_TCMP_MIN_OFFSET; + ch->bdshot_training_done = true; + } + } + + // Update TCMP + flexio_dshot_set_tcmp(channel); + } +} + void up_bdshot_erpm(void) { uint32_t value; @@ -373,8 +448,12 @@ void up_bdshot_erpm(void) if (bdshot_recv_mask & (1 << channel)) { value = ~dshot_inst[channel].raw_response & 0xFFFFF; - /* if lowest significant isn't 1 we've got a framing error */ - if (value & 0x1) { + // BDSHOT ESC hardware varies and timings differ between units. + // Run training to estimate the correct baudrate to lock onto. + if (!dshot_inst[channel].bdshot_training_done) { + up_bdshot_training(channel, value); + + } else if (value & 0x1) { /* if lowest significant isn't 1 we've got a framing error */ /* Decode RLL */ value = (value ^ (value >> 1)); @@ -461,6 +540,8 @@ void up_bdshot_status(void) if (dshot_inst[channel].init) { PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline", dshot_inst[channel].erpm); + PX4_INFO("BDSHOT Training done: %s TCMP offset: %d", dshot_inst[channel].bdshot_training_done ? "YES" : "NO", + dshot_inst[channel].bdshot_tcmp_offset); 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);