diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 0ab59ef829..d73d9e9148 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -160,7 +160,8 @@ bool AP_Compass_AK8963::init() /* register the compass instance in the frontend */ _compass_instance = register_compass(); set_dev_id(_compass_instance, _bus->get_dev_id()); - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void)); + /* timer needs to be called every 10ms so set the freq_div to 10 */ + _timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void), 10); _bus_sem->give(); hal.scheduler->resume_timer_procs(); @@ -230,8 +231,8 @@ void AP_Compass_AK8963::_update() Vector3f raw_field; uint32_t time_us = AP_HAL::micros(); - - if (AP_HAL::micros() - _last_update_timestamp < 10000) { + if (!_timesliced && + AP_HAL::micros() - _last_update_timestamp < 10000) { goto end; } diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 6a62c820bc..f345ac90f5 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -83,6 +83,7 @@ private: bool _initialized; uint32_t _last_update_timestamp; uint32_t _last_accum_time; + bool _timesliced; AP_AK8963_SerialBus *_bus = nullptr; AP_HAL::Semaphore *_bus_sem;