From 6bab28397ac2933a5711ed660b212a9ad8da7935 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 31 Jul 2017 12:29:43 -0700 Subject: [PATCH] AP_Compass: remove user of timesliced logic --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 7 +++---- libraries/AP_Compass/AP_Compass_AK8963.h | 1 - 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index e5cdaa3e49..9c61933dcc 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -170,7 +170,7 @@ bool AP_Compass_AK8963::init() /* timer needs to be called every 10ms so set the freq_div to 10 */ if (!_bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void))) { // fallback to timer - _timesliced = hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update_timer, void), 10); + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update_timer, void)); } return true; @@ -272,7 +272,7 @@ void AP_Compass_AK8963::_update_timer() { uint32_t now = AP_HAL::micros(); - if (!_timesliced && now - _last_update_timestamp < 10000) { + if (now - _last_update_timestamp < 10000) { return; } @@ -281,9 +281,8 @@ void AP_Compass_AK8963::_update_timer() } _update(); - _last_update_timestamp = now; - + _bus->get_semaphore()->give(); } diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 518351ee42..49e798a0a6 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -64,7 +64,6 @@ private: uint8_t _compass_instance; bool _initialized; - bool _timesliced; enum Rotation _rotation; };