From 3fc69687002c7a4d0b18c197f95fce7746231797 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Jul 2018 12:42:00 +1000 Subject: [PATCH] HAL_SITL: allow delay from other than main thread --- libraries/AP_HAL_SITL/SITL_State.cpp | 6 +++++- libraries/AP_HAL_SITL/Scheduler.cpp | 13 ++++++++++++- libraries/AP_HAL_SITL/Scheduler.h | 3 ++- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index e3df075b4d..eb8d0938fd 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -185,7 +185,11 @@ void SITL_State::_fdm_input_step(void) void SITL_State::wait_clock(uint64_t wait_time_usec) { while (AP_HAL::micros64() < wait_time_usec) { - _fdm_input_step(); + if (hal.scheduler->in_main_thread()) { + _fdm_input_step(); + } else { + usleep(1000); + } } } diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 16b28fadd9..50eef49c8b 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -31,6 +31,15 @@ Scheduler::Scheduler(SITL_State *sitlState) : void Scheduler::init() { + _main_ctx = pthread_self(); +} + +bool Scheduler::in_main_thread() const +{ + if (!_in_timer_proc && !_in_io_proc && pthread_self() == _main_ctx) { + return true; + } + return false; } void Scheduler::delay_microseconds(uint16_t usec) @@ -51,7 +60,9 @@ void Scheduler::delay(uint16_t ms) delay_microseconds(1000); ms--; if (_min_delay_cb_ms <= ms) { - call_delay_cb(); + if (in_main_thread()) { + call_delay_cb(); + } } } } diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index 45a7aae028..829a1dca5e 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -26,7 +26,7 @@ public: void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us); - bool in_main_thread() const override { return !_in_timer_proc && !_in_io_proc; }; + bool in_main_thread() const override; void system_initialized(); void reboot(bool hold_in_bootloader); @@ -78,5 +78,6 @@ private: bool _initialized; uint64_t _stopped_clock_usec; uint64_t _last_io_run; + pthread_t _main_ctx; }; #endif // CONFIG_HAL_BOARD