diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index e4c0ac40d7..97e87b908c 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -84,10 +84,12 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const callbacks->setup(); scheduler->system_initialized(); - for (;;) { + while (!HALSITL::Scheduler::_should_reboot) { callbacks->loop(); HALSITL::Scheduler::_run_io_procs(false); } + execv(argv[0], argv); + AP_HAL::panic("PANIC: REBOOT FAILED"); } const AP_HAL::HAL& AP_HAL::get_HAL() { diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index abdd570e21..8aa339e6a0 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -23,6 +23,7 @@ bool Scheduler::_in_timer_proc = false; AP_HAL::MemberProc Scheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullptr}; uint8_t Scheduler::_num_io_procs = 0; bool Scheduler::_in_io_proc = false; +bool Scheduler::_should_reboot = false; Scheduler::Scheduler(SITL_State *sitlState) : _sitlState(sitlState), @@ -130,7 +131,7 @@ void Scheduler::sitl_end_atomic() { void Scheduler::reboot(bool hold_in_bootloader) { - hal.uartA->printf("REBOOT NOT IMPLEMENTED\r\n\n"); + _should_reboot = true; } void Scheduler::_run_timer_procs(bool called_from_isr) diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index 6d154d7255..e7d3562510 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -50,6 +50,7 @@ public: uint64_t stopped_clock_usec() const { return _stopped_clock_usec; } static void _run_io_procs(bool called_from_isr); + static bool _should_reboot; private: SITL_State *_sitlState;