From a0fe1a75981f25f7bab5bf902ee8e39325946d9c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Feb 2020 15:47:08 +1100 Subject: [PATCH] HAL_ChibiOS: produce more accurate clocks for DShot and PWM --- libraries/AP_HAL_ChibiOS/RCOutput.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 4adc20a901..73ec58d630 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -540,11 +540,8 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_ uint32_t clock_hz = group.pwm_drv->clock; target_frequency = bitrate * bit_width; uint32_t prescaler = clock_hz / target_frequency; - while ((clock_hz / prescaler) * prescaler != clock_hz && prescaler <= 0x8000) { - prescaler++; - } freq = clock_hz / prescaler; - // hal.console->printf("CLOCK=%u FREQ=%u PRE=%u BR=%u\n", clock_hz, freq/bit_width, prescaler, bitrate); + // hal.console->printf("CLOCK=%u BW=%u FREQ=%u PRE=%u BR=%u\n", clock_hz, bit_width, freq/bit_width, prescaler, bitrate); if (prescaler > 0x8000) { group.dma_handle->unlock(); return false;