AP_InertialSensor: implement periodic thread for AuxiliaryBus

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -1120,3 +1120,9 @@ int AP_Invensense_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slav
 
     return 0;
 }
+
+AP_HAL::Device::PeriodicHandle AP_Invensense_AuxiliaryBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
+{
+    auto &backend = AP_InertialSensor_Invensense::from(_ins_backend);
+    return backend._dev->register_periodic_callback(period_usec, cb);
+}

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -173,6 +173,7 @@ class AP_Invensense_AuxiliaryBus : public AuxiliaryBus
 
 public:
     AP_HAL::Semaphore *get_semaphore() override;
+    AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) override;
 
 protected:
     AP_Invensense_AuxiliaryBus(AP_InertialSensor_Invensense &backend, uint32_t devid);

libraries/AP_InertialSensor/AuxiliaryBus.cpp

@@ -100,12 +100,3 @@ int AuxiliaryBus::register_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
 
     return 0;
 }
-
-/*
-  add a periodic callback. This is added to a list which the backend needs to then process
- */
-AP_HAL::Device::PeriodicHandle AuxiliaryBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
-{
-    // not implemented yet
-    return nullptr;
-}

libraries/AP_InertialSensor/AuxiliaryBus.h

@@ -102,9 +102,13 @@ public:
     AuxiliaryBusSlave *request_next_slave(uint8_t addr);
     int register_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg, uint8_t size);
 
-    /* See AP_HAL::Device::register_periodic_callback() */
-    AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb);
-    
+    /* See AP_HAL::Device::register_periodic_callback()
+     *
+     * This method must be implemented by the sensor exposing the
+     * AuxiliaryBus.
+     */
+    virtual AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t, AP_HAL::Device::PeriodicCb) = 0;
+
     /*
      * Get the semaphore needed to call methods on the bus this sensor is on.
      * Internally no locks are taken and it's the caller's duty to lock and
@@ -126,7 +130,7 @@ public:
     uint32_t get_bus_id(void) const {
         return _devid;
     }
-        
+
 protected:
     /* Only AP_InertialSensor_Backend is able to create a bus */
     AuxiliaryBus(AP_InertialSensor_Backend &backend, uint8_t max_slaves, uint32_t devid);