diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp index 07396498c9..214fcb1993 100644 --- a/libraries/AP_HAL_QURT/Scheduler.cpp +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -80,20 +80,11 @@ void Scheduler::delay(uint16_t ms) while (((now=AP_HAL::micros64()) - start)/1000 < ms) { delay_microseconds(1000); 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_QURT/Scheduler.h b/libraries/AP_HAL_QURT/Scheduler.h index 7613646937..ce1bdb24d2 100644 --- a/libraries/AP_HAL_QURT/Scheduler.h +++ b/libraries/AP_HAL_QURT/Scheduler.h @@ -23,7 +23,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); void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);