Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_ChibiOS: ensure dshot rate can be set dynamically 

honour the requested dshot rate as near as possible
This commit is contained in:
Andy Piper 2024-07-11 20:51:31 +01:00 committed by Andrew Tridgell
parent 3cc4b1991c
commit a3de217dee
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -553,19 +553,19 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
} }
uint16_t drate = dshot_rate * loop_rate_hz; uint16_t drate = dshot_rate * loop_rate_hz;
_dshot_rate = dshot_rate;
// BLHeli32 uses a 16 bit counter for input calibration which at 48Mhz will wrap // BLHeli32 uses a 16 bit counter for input calibration which at 48Mhz will wrap
// at 732Hz so never allow rates below 800hz // at 732Hz so never allow rates below 800hz
while (drate < 800) { while (drate < 800) {
_dshot_rate++; dshot_rate++;
drate = _dshot_rate * loop_rate_hz; drate = dshot_rate * loop_rate_hz;
} }
// prevent stupidly high rates, ideally should also prevent high rates // prevent stupidly high rate multiples, ideally should also prevent high rates
// with slower dshot variants // with slower dshot variants
if (drate > 4000) { while (dshot_rate > 1 && drate > MAX(4096, loop_rate_hz)) {
_dshot_rate = 4000 / loop_rate_hz; dshot_rate--;
drate = _dshot_rate * loop_rate_hz; drate = dshot_rate * loop_rate_hz;
} }
_dshot_rate = dshot_rate;
_dshot_period_us = 1000000UL / drate; _dshot_period_us = 1000000UL / drate;
#if HAL_WITH_IO_MCU #if HAL_WITH_IO_MCU
if (iomcu_dshot) { if (iomcu_dshot) {