From a489c9300103bf78ed5fdb5d1e29313e900aa4c0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Jul 2018 11:51:18 +1000 Subject: [PATCH] HAL_ChibiOS: removed restriction on delay in threads threads other than the main thread should be able to sleep, but not call the delay callback --- libraries/AP_HAL_ChibiOS/Scheduler.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 8c58c048ec..47d3d0f04a 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -176,16 +176,14 @@ bool Scheduler::check_called_boost(void) void Scheduler::delay(uint16_t ms) { - if (!in_main_thread()) { - //chprintf("ERROR: delay() from timer process\n"); - return; - } uint64_t start = AP_HAL::micros64(); while ((AP_HAL::micros64() - start)/1000 < ms) { delay_microseconds(1000); if (_min_delay_cb_ms <= ms) { - call_delay_cb(); + if (in_main_thread()) { + call_delay_cb(); + } } } }