fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:48:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

imxrt: dshot fix erpm calculation by implementing 3-bit exponent and 9-bit period

Browse Source
This commit is contained in:
Peter van der Perk
2024-05-30 04:56:42 -04:00
committed by David Sidrane
parent 3874b4c55d
commit ff6966da57
2 changed files with 24 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
+23 -8
View File
@@ -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;
}
src/drivers/dshot/DShot.cpp
+1 -2
View File
@@ -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];
}