mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
AP_HAL_SITL: catch SIGTERM and exit with zero exit status
This commit is contained in:
parent
85332d49a2
commit
045960fe57
@ -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();
|
||||
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
|
||||
|
@ -50,6 +50,7 @@ public:
|
||||
|
||||
static void _run_io_procs();
|
||||
static bool _should_reboot;
|
||||
static bool _should_exit;
|
||||
|
||||
/*
|
||||
create a new thread
|
||||
|
Loading…
Reference in New Issue
Block a user