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

Some notes:
 - The only place that made sense to use
   suspend_timer_procs()/resume_timer_procs() calls were where we registered
   the timer process. Now there's no need for that anymore. Remove those calls
   from other place in the source too.
 - There's no need to acquire the device lock now that we are running as a
   periodic callback.
---
 .../AP_InertialSensor_BMI160.cpp              | 20 +++++--------------
 .../AP_InertialSensor_BMI160.h                |  4 ++--
 2 files changed, 7 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
index 0613b85a15..087565bd0e 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp
@@ -145,7 +145,6 @@ void AP_InertialSensor_BMI160::start()
 {
     bool r;
 
-    hal.scheduler->suspend_timer_procs();
     if (!_dev->get_semaphore()->take(100)) {
         AP_HAL::panic("BMI160: Unable to get semaphore");
     }
@@ -177,10 +176,9 @@ void AP_InertialSensor_BMI160::start()
     _accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR));
     _gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR));
 
-    hal.scheduler->register_timer_process(
-        FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI160::_poll_data, void));
-
-    hal.scheduler->resume_timer_procs();
+    /* Call _poll_data() at 1kHz */
+    _dev->register_periodic_callback(1000,
+        FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI160::_poll_data, bool));
 }
 
 bool AP_InertialSensor_BMI160::update()
@@ -407,15 +405,10 @@ read_fifo_end:
     }
 }
 
-void AP_InertialSensor_BMI160::_poll_data()
+bool AP_InertialSensor_BMI160::_poll_data()
 {
-    if (!_dev->get_semaphore()->take_nonblocking()) {
-        return;
-    }
-
     _read_fifo();
-
-    _dev->get_semaphore()->give();
+    return true;
 }
 
 bool AP_InertialSensor_BMI160::_hardware_init()
@@ -485,10 +478,7 @@ bool AP_InertialSensor_BMI160::_init()
     bool ret = false;
     _dev->set_read_flag(BMI160_READ_FLAG);
 
-    hal.scheduler->suspend_timer_procs();
     ret = _hardware_init();
-    hal.scheduler->resume_timer_procs();
-
     if (!ret) {
         hal.console->printf("BMI160: failed to init\n");
     }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
index f39d3d0f89..240731b72a 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h
@@ -96,9 +96,9 @@ private:
     bool _configure_fifo();
 
     /**
-     * Timer routine to read data from the sensors.
+     * Device periodic callback to read data from the sensors.
      */
-    void _poll_data();
+    bool _poll_data();
 
     /**
      * Read samples from fifo.