From b2593567039612cac4aa0092a8516f25be8ebe7e Mon Sep 17 00:00:00 2001
From: Gustavo Jose de Sousa <gustavo.sousa@intel.com>
Date: Mon, 29 Aug 2016 11:35:07 -0300
Subject: [PATCH] AP_Compass: BMM150: convert it to use Device periodic
 callback

---
 libraries/AP_Compass/AP_Compass_BMM150.cpp | 19 +++++++------------
 libraries/AP_Compass/AP_Compass_BMM150.h   |  5 ++++-
 2 files changed, 11 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_BMM150.cpp b/libraries/AP_Compass/AP_Compass_BMM150.cpp
index 2de7da8e81..f1c5159804 100644
--- a/libraries/AP_Compass/AP_Compass_BMM150.cpp
+++ b/libraries/AP_Compass/AP_Compass_BMM150.cpp
@@ -193,9 +193,8 @@ bool AP_Compass_BMM150::init()
     _compass_instance = register_compass();
     set_dev_id(_compass_instance, AP_COMPASS_TYPE_BMM150);
 
-    hal.scheduler->suspend_timer_procs();
-    hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
-    hal.scheduler->resume_timer_procs();
+    _dev->register_periodic_callback(MEASURE_TIME_USEC,
+            FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, bool));
 
     return true;
 
@@ -242,25 +241,20 @@ int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall)
     return constrain_int32(dividend / divisor, -0x8000, 0x8000);
 }
 
-void AP_Compass_BMM150::_update()
+bool AP_Compass_BMM150::_update()
 {
     const uint32_t time_usec = AP_HAL::micros();
 
     if (time_usec - _last_update_timestamp < MEASURE_TIME_USEC) {
-        return;
-    }
-
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return;
+        return true;
     }
 
     le16_t data[4];
     bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));
-    _dev->get_semaphore()->give();
 
     /* Checking data ready status */
     if (!ret || !(data[3] & 0x1)) {
-        return;
+        return true;
     }
 
     const uint16_t rhall = le16toh(data[3] >> 2);
@@ -287,7 +281,7 @@ void AP_Compass_BMM150::_update()
     correct_field(raw_field, _compass_instance);
 
     if (!_accum_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        return;
+        return true;
     }
     _mag_accum += raw_field;
     _accum_count++;
@@ -297,6 +291,7 @@ void AP_Compass_BMM150::_update()
     }
     _last_update_timestamp = time_usec;
     _accum_sem->give();
+    return true;
 }
 
 void AP_Compass_BMM150::read()
diff --git a/libraries/AP_Compass/AP_Compass_BMM150.h b/libraries/AP_Compass/AP_Compass_BMM150.h
index a3b821002c..6f5a4d6210 100644
--- a/libraries/AP_Compass/AP_Compass_BMM150.h
+++ b/libraries/AP_Compass/AP_Compass_BMM150.h
@@ -39,7 +39,10 @@ public:
 private:
     AP_Compass_BMM150(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev);
 
-    void _update();
+    /**
+     * Device periodic callback to read data from the sensor.
+     */
+    bool _update();
     bool _load_trim_values();
     int16_t _compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2);
     int16_t _compensate_z(int16_t z, uint32_t rhall);