diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index 0238cb0f27..9388febcf9 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -122,11 +122,26 @@ static void sig_alrm(int signum) execv(new_argv[0], new_argv); } +void HAL_SITL::exit_signal_handler(int signum) +{ + HALSITL::Scheduler::_should_exit = true; +} + +void HAL_SITL::setup_signal_handlers() const +{ + struct sigaction sa = { }; + + sa.sa_flags = SA_NOCLDSTOP; + sa.sa_handler = HAL_SITL::exit_signal_handler; + sigaction(SIGTERM, &sa, NULL); +} + void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const { assert(callbacks); _sitl_state->init(argc, argv); + scheduler->init(); uartA->begin(115200); @@ -173,10 +188,15 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const signal(SIGALRM, sig_alrm); alarm(2); } + setup_signal_handlers(); uint32_t last_watchdog_save = AP_HAL::millis(); while (!HALSITL::Scheduler::_should_reboot) { + if (HALSITL::Scheduler::_should_exit) { + ::fprintf(stderr, "Exitting\n"); + exit(0); + } callbacks->loop(); HALSITL::Scheduler::_run_io_procs(); diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index c8bbb1d1cd..87d384de51 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -16,6 +16,9 @@ public: private: HALSITL::SITL_State *_sitl_state; + + void setup_signal_handlers() const; + static void exit_signal_handler(int); }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 5d687b911d..2667f4ff48 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -28,6 +28,7 @@ AP_HAL::MemberProc Scheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullpt uint8_t Scheduler::_num_io_procs = 0; bool Scheduler::_in_io_proc = false; bool Scheduler::_should_reboot = false; +bool Scheduler::_should_exit = false; bool Scheduler::_in_semaphore_take_wait = false; diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index a03f759fe3..8d30b3ecf2 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -50,6 +50,7 @@ public: static void _run_io_procs(); static bool _should_reboot; + static bool _should_exit; /* create a new thread