From 919aa6191819ac1ccfa47c3a5be6e16e23aad0cb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 3 Nov 2016 09:52:05 +1100
Subject: [PATCH] AP_InertialSensor: support AuxiliaryBus without
 register_periodic_callback()

---
 .../AP_InertialSensor_MPU6000.cpp               | 17 ++++++++++++++++-
 .../AP_InertialSensor_MPU6000.h                 |  1 +
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 6e268788a5..3bf6119b54 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -388,7 +388,10 @@ void AP_InertialSensor_MPU6000::start()
     hal.scheduler->resume_timer_procs();
 
     // start the timer process to read samples
-    _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool));
+    if (_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool)) == nullptr) {
+        // cope with AuxiliaryBus which does not support register_periodic_callback yet
+        hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data_timer, void));
+    }
 }
 
 /*
@@ -445,6 +448,18 @@ bool AP_InertialSensor_MPU6000::_poll_data()
     return true;
 }
 
+/*
+  varient of _poll_data for systems where register_periodic_callback
+  fails. We need to handle the semaphore ourselves.
+ */
+void AP_InertialSensor_MPU6000::_poll_data_timer()
+{
+    if (_dev->get_semaphore()->take_nonblocking()) {
+        _poll_data();
+        _dev->get_semaphore()->give();
+    }
+}
+
 void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
 {
     for (uint8_t i = 0; i < n_samples; i++) {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index e300a4cf36..37825873e3 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -78,6 +78,7 @@ private:
 
     /* Poll for new data (non-blocking) */
     bool _poll_data();
+    void _poll_data_timer();
 
     /* Read and write functions taking the differences between buses into
      * account */