From 10e9182c21bd748b2010daca4fedf3c01acd28eb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 12 Jul 2018 15:03:27 +1000 Subject: [PATCH] HAL_SITL: fixed delay() for SITL threads with the new thread_create() interface we need to handle delays a bit differently --- libraries/AP_HAL_SITL/Scheduler.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 50eef49c8b..b9b6969474 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -56,15 +56,17 @@ void Scheduler::delay_microseconds(uint16_t usec) void Scheduler::delay(uint16_t ms) { - while (ms > 0) { + uint32_t start = AP_HAL::millis(); + uint32_t now = start; + do { delay_microseconds(1000); - ms--; - if (_min_delay_cb_ms <= ms) { + if (_min_delay_cb_ms <= (ms - (now - start))) { if (in_main_thread()) { call_delay_cb(); } } - } + now = AP_HAL::millis(); + } while (now - start < ms); } void Scheduler::register_timer_process(AP_HAL::MemberProc proc)