diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 1ff8ba0f90..97b6fd70b2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/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); +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 16e23b85e0..ad415c110d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/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); diff --git a/libraries/AP_InertialSensor/AuxiliaryBus.cpp b/libraries/AP_InertialSensor/AuxiliaryBus.cpp index 39f92be2c8..4697cb3337 100644 --- a/libraries/AP_InertialSensor/AuxiliaryBus.cpp +++ b/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; -} diff --git a/libraries/AP_InertialSensor/AuxiliaryBus.h b/libraries/AP_InertialSensor/AuxiliaryBus.h index 318ff15723..26873865a5 100644 --- a/libraries/AP_InertialSensor/AuxiliaryBus.h +++ b/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);