diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index 97e87b908c..9e6cd2ddd1 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -86,7 +86,7 @@ void HAL_SITL::run(int argc, char * const argv[], Callbacks* callbacks) const while (!HALSITL::Scheduler::_should_reboot) { callbacks->loop(); - HALSITL::Scheduler::_run_io_procs(false); + HALSITL::Scheduler::_run_io_procs(); } execv(argv[0], argv); AP_HAL::panic("PANIC: REBOOT FAILED"); diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 8aa339e6a0..367bf09bac 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -13,8 +13,6 @@ extern const AP_HAL::HAL& hal; AP_HAL::Proc Scheduler::_failsafe = nullptr; -volatile bool Scheduler::_timer_suspended = false; -volatile bool Scheduler::_timer_event_missed = false; AP_HAL::MemberProc Scheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {nullptr}; uint8_t Scheduler::_num_timer_procs = 0; @@ -91,18 +89,6 @@ void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_u _failsafe = failsafe; } -void Scheduler::suspend_timer_procs() { - _timer_suspended = true; -} - -void Scheduler::resume_timer_procs() { - _timer_suspended = false; - if (_timer_event_missed) { - _timer_event_missed = false; - _run_timer_procs(false); - } -} - void Scheduler::system_initialized() { if (_initialized) { AP_HAL::panic( @@ -134,7 +120,7 @@ void Scheduler::reboot(bool hold_in_bootloader) _should_reboot = true; } -void Scheduler::_run_timer_procs(bool called_from_isr) +void Scheduler::_run_timer_procs() { if (_in_timer_proc) { // the timer calls took longer than the period of the @@ -154,15 +140,11 @@ void Scheduler::_run_timer_procs(bool called_from_isr) } _in_timer_proc = true; - if (!_timer_suspended) { - // now call the timer based drivers - for (int i = 0; i < _num_timer_procs; i++) { - if (_timer_proc[i]) { - _timer_proc[i](); - } + // now call the timer based drivers + for (int i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i]) { + _timer_proc[i](); } - } else if (called_from_isr) { - _timer_event_missed = true; } // and the failsafe, if one is setup @@ -173,22 +155,18 @@ void Scheduler::_run_timer_procs(bool called_from_isr) _in_timer_proc = false; } -void Scheduler::_run_io_procs(bool called_from_isr) +void Scheduler::_run_io_procs() { if (_in_io_proc) { return; } _in_io_proc = true; - if (!_timer_suspended) { - // now call the IO based drivers - for (int i = 0; i < _num_io_procs; i++) { - if (_io_proc[i]) { - _io_proc[i](); - } + // now call the IO based drivers + for (int i = 0; i < _num_io_procs; i++) { + if (_io_proc[i]) { + _io_proc[i](); } - } else if (called_from_isr) { - _timer_event_missed = true; } _in_io_proc = false; @@ -209,7 +187,7 @@ void Scheduler::stop_clock(uint64_t time_usec) _stopped_clock_usec = time_usec; if (time_usec - _last_io_run > 10000) { _last_io_run = time_usec; - _run_io_procs(false); + _run_io_procs(); } } diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index e7d3562510..643d89ff74 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -23,8 +23,6 @@ public: void register_timer_process(AP_HAL::MemberProc); void register_io_process(AP_HAL::MemberProc); - void suspend_timer_procs(); - void resume_timer_procs(); void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us); @@ -43,13 +41,13 @@ public: void sitl_end_atomic(); static void timer_event() { - _run_timer_procs(true); - _run_io_procs(true); + _run_timer_procs(); + _run_io_procs(); } uint64_t stopped_clock_usec() const { return _stopped_clock_usec; } - static void _run_io_procs(bool called_from_isr); + static void _run_io_procs(); static bool _should_reboot; private: @@ -57,9 +55,8 @@ private: uint8_t _nested_atomic_ctr; static AP_HAL::Proc _failsafe; - static void _run_timer_procs(bool called_from_isr); + static void _run_timer_procs(); - static volatile bool _timer_suspended; static volatile bool _timer_event_missed; static AP_HAL::MemberProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS]; static AP_HAL::MemberProc _io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];