From 91dabbe418930bc6fa780666b1c0637ee85dbcf2 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Wed, 1 Mar 2017 00:18:40 -0800
Subject: [PATCH] AP_HAL_PX4: implement method to ajust periodic callback

Just setting up the periodic callback sampling time on initialization
may not work well for sensors that need to request for a sample with a
bus transaction, sleep and then read the new data. That's because the
function will be kept calling at a periodic rate, while the time in
which we can read the value is not really that sampling time, but rather
the time in which sensor was last read + the time spent in the function
before sending a new sample request.

Instead of creating a new type of thread to handle this case, just
implement the minimal and easy case of updating the period for this
callback, that can only be called from inside the callback function.
---
 libraries/AP_HAL_PX4/Device.cpp    | 19 +++++++++++++++++++
 libraries/AP_HAL_PX4/Device.h      |  1 +
 libraries/AP_HAL_PX4/I2CDevice.cpp | 10 ++++++++--
 libraries/AP_HAL_PX4/SPIDevice.cpp |  2 +-
 4 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_HAL_PX4/Device.cpp b/libraries/AP_HAL_PX4/Device.cpp
index e15fef037a..e972e5c02f 100644
--- a/libraries/AP_HAL_PX4/Device.cpp
+++ b/libraries/AP_HAL_PX4/Device.cpp
@@ -133,4 +133,23 @@ AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t pe
     return callback;
 }
 
+/*
+ * Adjust the timer for the next call: it needs to be called from the bus
+ * thread, otherwise it will race with it
+ */
+bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
+{
+    if (!pthread_equal(pthread_self(), thread_ctx)) {
+        fprintf(stderr, "can't adjust timer from unknown thread context\n");
+        return false;
+    }
+
+    DeviceBus::callback_info *callback = static_cast<DeviceBus::callback_info *>(h);
+
+    callback->period_usec = period_usec;
+    callback->next_usec = AP_HAL::micros64() + period_usec;
+
+    return true;
+}
+
 }
diff --git a/libraries/AP_HAL_PX4/Device.h b/libraries/AP_HAL_PX4/Device.h
index 8eeda33462..79be5a84ce 100644
--- a/libraries/AP_HAL_PX4/Device.h
+++ b/libraries/AP_HAL_PX4/Device.h
@@ -30,6 +30,7 @@ public:
     Semaphore semaphore;
 
     AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device);
+    bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec);
     static void *bus_thread(void *arg);
     
 private:
diff --git a/libraries/AP_HAL_PX4/I2CDevice.cpp b/libraries/AP_HAL_PX4/I2CDevice.cpp
index 606af46311..5dd8375aa4 100644
--- a/libraries/AP_HAL_PX4/I2CDevice.cpp
+++ b/libraries/AP_HAL_PX4/I2CDevice.cpp
@@ -150,9 +150,15 @@ AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t pe
 */
 bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
 {
-    return false;
+    if (_busnum >= num_buses) {
+        return false;
+    }
+
+    struct DeviceBus &binfo = businfo[_busnum];
+
+    return binfo.adjust_timer(h, period_usec);
 }
-    
+
 AP_HAL::OwnPtr<AP_HAL::I2CDevice>
 I2CDeviceManager::get_device(uint8_t bus, uint8_t address)
 {
diff --git a/libraries/AP_HAL_PX4/SPIDevice.cpp b/libraries/AP_HAL_PX4/SPIDevice.cpp
index aba35f97d9..2329458cfa 100644
--- a/libraries/AP_HAL_PX4/SPIDevice.cpp
+++ b/libraries/AP_HAL_PX4/SPIDevice.cpp
@@ -239,7 +239,7 @@ AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t pe
 
 bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec)
 {
-    return false;
+    return bus.adjust_timer(h, period_usec);
 }
 
 /*