HAL_SITL: allow delay from other than main thread

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) {
+        if (hal.scheduler->in_main_thread()) {
             _fdm_input_step();
+        } else {
+            usleep(1000);
+        }
     }
 }
 

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,10 +60,12 @@ void Scheduler::delay(uint16_t ms)
         delay_microseconds(1000);
         ms--;
         if (_min_delay_cb_ms <= ms) {
+            if (in_main_thread()) {
                 call_delay_cb();
             }
         }
     }
+}
 
 void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
 {

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