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

@@ -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;
+}
+
 }

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:

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)
 {

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);
 }
 
 /*