From ff6966da5761f2c0697e962551e7918a55ff9fed Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Wed, 1 May 2024 16:59:21 +0200 Subject: [PATCH] imxrt: dshot fix erpm calculation by implementing 3-bit exponent and 9-bit period --- .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 31 ++++++++++++++----- src/drivers/dshot/DShot.cpp | 3 +- 2 files changed, 24 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 d697234c9c..cb92dd1209 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -361,8 +361,11 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi void up_bdshot_erpm(void) { uint32_t value; - uint32_t erpm; + uint32_t data; uint32_t csum_data; + uint8_t exponent; + uint16_t period; + uint16_t erpm; bdshot_parsed_recv_mask = 0; @@ -377,13 +380,13 @@ void up_bdshot_erpm(void) value = (value ^ (value >> 1)); /* Decode GCR */ - erpm = gcr_decode[value & 0x1fU]; - erpm |= gcr_decode[(value >> 5U) & 0x1fU] << 4U; - erpm |= gcr_decode[(value >> 10U) & 0x1fU] << 8U; - erpm |= gcr_decode[(value >> 15U) & 0x1fU] << 12U; + data = gcr_decode[value & 0x1fU]; + data |= gcr_decode[(value >> 5U) & 0x1fU] << 4U; + data |= gcr_decode[(value >> 10U) & 0x1fU] << 8U; + data |= gcr_decode[(value >> 15U) & 0x1fU] << 12U; /* Calculate checksum */ - csum_data = erpm; + csum_data = data; csum_data = csum_data ^ (csum_data >> 8U); csum_data = csum_data ^ (csum_data >> NIBBLES_SIZE); @@ -391,7 +394,19 @@ void up_bdshot_erpm(void) dshot_inst[channel].crc_error_cnt++; } else { - dshot_inst[channel].erpm = ~(erpm >> 4) & 0xFFF; + data = (data >> 4) & 0xFFF; + + if (data == 0xFFF) { + erpm = 0; + + } else { + exponent = ((data >> 9U) & 0x7U); /* 3 bit: exponent */ + period = (data & 0x1ffU); /* 9 bit: period base */ + period = period << exponent; /* Period in usec */ + erpm = ((1000000U * 60U / 100U + period / 2U) / period); + } + + dshot_inst[channel].erpm = erpm; bdshot_parsed_recv_mask |= (1 << channel); dshot_inst[channel].last_no_response_cnt = dshot_inst[channel].no_response_cnt; } @@ -408,7 +423,7 @@ void up_bdshot_erpm(void) int up_bdshot_get_erpm(uint8_t channel, int *erpm) { if (bdshot_parsed_recv_mask & (1 << channel)) { - *erpm = dshot_inst[channel].erpm; + *erpm = (int)dshot_inst[channel].erpm; return 0; } diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 26ad1d63b3..3582dab43c 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -312,8 +312,7 @@ int DShot::handle_new_bdshot_erpm(void) 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].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2); esc_status.esc[channel].actuator_function = _telemetry->actuator_functions[channel]; }