diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index cd965724b8..16b28fadd9 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -1,11 +1,11 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "AP_HAL_SITL.h" #include "Scheduler.h" #include "UARTDriver.h" #include #include +#include using namespace HALSITL; @@ -192,4 +192,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; +} + diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index 643d89ff74..45a7aae028 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -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,6 +73,8 @@ 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;