diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 6382c5c240..abdd570e21 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -52,20 +52,11 @@ void Scheduler::delay(uint16_t ms) delay_microseconds(1000); ms--; if (_min_delay_cb_ms <= ms) { - if (_delay_cb) { - _delay_cb(); - } + call_delay_cb(); } } } -void Scheduler::register_delay_callback(AP_HAL::Proc proc, - uint16_t min_time_ms) -{ - _delay_cb = proc; - _min_delay_cb_ms = min_time_ms; -} - void Scheduler::register_timer_process(AP_HAL::MemberProc proc) { for (uint8_t i = 0; i < _num_timer_procs; i++) { diff --git a/libraries/AP_HAL_SITL/Scheduler.h b/libraries/AP_HAL_SITL/Scheduler.h index effea60193..6d154d7255 100644 --- a/libraries/AP_HAL_SITL/Scheduler.h +++ b/libraries/AP_HAL_SITL/Scheduler.h @@ -20,7 +20,6 @@ public: void init(); void delay(uint16_t ms); void delay_microseconds(uint16_t us); - void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); void register_timer_process(AP_HAL::MemberProc); void register_io_process(AP_HAL::MemberProc); @@ -55,8 +54,6 @@ public: private: SITL_State *_sitlState; uint8_t _nested_atomic_ctr; - AP_HAL::Proc _delay_cb; - uint16_t _min_delay_cb_ms; static AP_HAL::Proc _failsafe; static void _run_timer_procs(bool called_from_isr);