From a3de217deec3eb18ed971c68cdba4ace0350417c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 11 Jul 2024 20:51:31 +0100 Subject: [PATCH] AP_HAL_ChibiOS: ensure dshot rate can be set dynamically honour the requested dshot rate as near as possible --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 1e0fbae0a3..33fbe4ac69 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -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; - _dshot_rate = dshot_rate; // BLHeli32 uses a 16 bit counter for input calibration which at 48Mhz will wrap // at 732Hz so never allow rates below 800hz while (drate < 800) { - _dshot_rate++; - drate = _dshot_rate * loop_rate_hz; + dshot_rate++; + 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 - if (drate > 4000) { - _dshot_rate = 4000 / loop_rate_hz; - drate = _dshot_rate * loop_rate_hz; + while (dshot_rate > 1 && drate > MAX(4096, loop_rate_hz)) { + dshot_rate--; + drate = dshot_rate * loop_rate_hz; } + _dshot_rate = dshot_rate; _dshot_period_us = 1000000UL / drate; #if HAL_WITH_IO_MCU if (iomcu_dshot) {