From bf1d36420108026c29c426f3d46965c30e9d786b Mon Sep 17 00:00:00 2001 From: Richard Allen Date: Sun, 11 Feb 2024 12:15:50 -0600 Subject: [PATCH] HAL_Linux: reduce delay(ms) jitter Fix delay(1) rarely returning immediately. On my RPi4, this once per 5-20k calls that worked. Reduce the last call to microsleep according to the remaining time needed in the last loop iteration. --- libraries/AP_HAL_Linux/Scheduler.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index 7c2cce5d2b..98e3914332 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -173,15 +173,20 @@ void Scheduler::delay(uint16_t ms) return; } - uint64_t start = AP_HAL::millis64(); + if (ms == 0) { + return; + } - while ((AP_HAL::millis64() - start) < ms) { + uint64_t now = AP_HAL::micros64(); + uint64_t end = now + 1000UL * ms + 1U; + do { // this yields the CPU to other apps - microsleep(1000); + microsleep(MIN(1000UL, end-now)); if (in_main_thread() && _min_delay_cb_ms <= ms) { call_delay_cb(); } - } + now = AP_HAL::micros64(); + } while (now < end); } void Scheduler::delay_microseconds(uint16_t us)