diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index 8cfbac847b..e0bf4d55fb 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -222,19 +222,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 == true) { - _run_timers(false); - _timer_event_missed = false; - } -} extern void Reset_Handler(); void Scheduler::reboot(bool hold_in_bootloader) { @@ -252,22 +239,18 @@ void Scheduler::reboot(bool hold_in_bootloader) NVIC_SystemReset(); } -void Scheduler::_run_timers(bool called_from_timer_thread) +void Scheduler::_run_timers() { if (_in_timer_proc) { return; } _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_timer_thread) { - _timer_event_missed = true; } // and the failsafe, if one is setup @@ -295,7 +278,7 @@ void Scheduler::_timer_thread(void *arg) sched->delay_microseconds(1000); // run registered timers - sched->_run_timers(true); + sched->_run_timers(); // process any pending RC output requests hal.rcout->timer_tick(); @@ -356,12 +339,10 @@ void Scheduler::_run_io(void) } _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](); } } diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h index 8a5d6dab0e..c13c956d3b 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.h +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -73,8 +73,6 @@ public: void register_timer_process(AP_HAL::MemberProc) override; void register_io_process(AP_HAL::MemberProc) override; void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override; - void suspend_timer_procs() override; - void resume_timer_procs() override; void reboot(bool hold_in_bootloader) override; bool in_main_thread() const override; @@ -102,7 +100,6 @@ private: bool _called_boost; bool _priority_boosted; - volatile bool _timer_suspended; AP_HAL::MemberProc _timer_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS]; uint8_t _num_timer_procs; @@ -112,8 +109,6 @@ private: uint8_t _num_io_procs; volatile bool _in_io_proc; - volatile bool _timer_event_missed; - thread_t* _timer_thread_ctx; thread_t* _rcin_thread_ctx; thread_t* _io_thread_ctx; @@ -131,7 +126,7 @@ private: #if HAL_WITH_UAVCAN static void _uavcan_thread(void *arg); #endif - void _run_timers(bool called_from_timer_thread); + void _run_timers(); void _run_io(void); }; #endif