HAL_SITL: update to match plane3.9.0beta6

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);
+        }
     }
 }
 

libraries/AP_HAL_SITL/Scheduler.cpp

@@ -1,11 +1,11 @@
 #include <AP_HAL/AP_HAL.h>
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
 
 #include "AP_HAL_SITL.h"
 #include "Scheduler.h"
 #include "UARTDriver.h"
 #include <sys/time.h>
 #include <fenv.h>
+#include <pthread.h>
 
 using namespace HALSITL;
 
@@ -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)
@@ -47,13 +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) {
-            call_delay_cb();
+        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)
@@ -192,4 +205,34 @@ void Scheduler::stop_clock(uint64_t time_usec)
     }
 }
 
-#endif
+/*
+  trampoline for thread create
+*/
+void *Scheduler::thread_create_trampoline(void *ctx)
+{
+    AP_HAL::MemberProc *t = (AP_HAL::MemberProc *)ctx;
+    (*t)();
+    free(t);
+    return nullptr;
+}
+
+
+/*
+  create a new thread
+*/
+bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
+{
+    // take a copy of the MemberProc, it is freed after thread exits
+    AP_HAL::MemberProc *tproc = (AP_HAL::MemberProc *)malloc(sizeof(proc));
+    if (!tproc) {
+        return false;
+    }
+    *tproc = proc;
+    pthread_t thread {};
+    if (pthread_create(&thread, NULL, thread_create_trampoline, tproc) != 0) {
+        free(tproc);
+        return false;
+    }
+    return true;
+}
+

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);
@@ -49,6 +49,12 @@ public:
 
     static void _run_io_procs();
     static bool _should_reboot;
+
+    /*
+      create a new thread
+     */
+    bool thread_create(AP_HAL::MemberProc, const char *name,
+                       uint32_t stack_size, priority_base base, int8_t priority) override;
     
 private:
     SITL_State *_sitlState;
@@ -67,8 +73,11 @@ private:
 
     void stop_clock(uint64_t time_usec);
 
+    static void *thread_create_trampoline(void *ctx);
+    
     bool _initialized;
     uint64_t _stopped_clock_usec;
     uint64_t _last_io_run;
+    pthread_t _main_ctx;
 };
 #endif  // CONFIG_HAL_BOARD