From 0f38bcaf03a2c034fe04673b3e3330c6a85b4368 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Mon, 19 Feb 2024 17:19:23 +0100 Subject: [PATCH] :recycle: (imu): Break setDataAvailableInterrupt in multiple methods --- drivers/CoreIMU/include/CoreIMU.hpp | 6 +++- drivers/CoreIMU/source/CoreIMU.cpp | 48 ++++++++++++++++++-------- drivers/CoreIMU/tests/CoreIMU_test.cpp | 16 +++++++++ include/interface/drivers/IMU.hpp | 8 +++-- tests/unit/mocks/mocks/leka/IMU.h | 2 ++ 5 files changed, 63 insertions(+), 17 deletions(-) diff --git a/drivers/CoreIMU/include/CoreIMU.hpp b/drivers/CoreIMU/include/CoreIMU.hpp index 2cbf6ad70b..c2f25b7f62 100644 --- a/drivers/CoreIMU/include/CoreIMU.hpp +++ b/drivers/CoreIMU/include/CoreIMU.hpp @@ -22,6 +22,8 @@ class CoreIMU : public interface::IMU void init() final; void registerOnDataAvailableCallback(data_available_callback_t const &callback) final; + void enableOnDataAvailable() final; + void disableOnDataAvailable() final; void setPowerMode(PowerMode mode) final; @@ -35,7 +37,8 @@ class CoreIMU : public interface::IMU -> int32_t; void onDataAvailableHandler(auto timestamp); - void setDataAvailableInterrupt(); + + void setInterruptCallback(std::function const &callback); interface::I2C &_i2c; CoreEventQueue _event_queue {}; @@ -48,6 +51,7 @@ class CoreIMU : public interface::IMU std::array data_raw_xl {}; std::array data_raw_gy {}; data_available_callback_t _on_data_available_callback {}; + std::function _on_data_available_wrapper_callback {}; static constexpr uint8_t kMaxBufferLength = 32; std::array _rx_buffer {}; diff --git a/drivers/CoreIMU/source/CoreIMU.cpp b/drivers/CoreIMU/source/CoreIMU.cpp index d86f50db4c..f0fd2b24e7 100644 --- a/drivers/CoreIMU/source/CoreIMU.cpp +++ b/drivers/CoreIMU/source/CoreIMU.cpp @@ -33,7 +33,7 @@ void CoreIMU::init() lsm6dsox_dataready_pulsed_t data_ready_pulsed {LSM6DSOX_DRDY_PULSED}; lsm6dsox_data_ready_mode_set(&_register_io_function, data_ready_pulsed); - setDataAvailableInterrupt(); + enableOnDataAvailable(); } void CoreIMU::setPowerMode(PowerMode mode) @@ -77,6 +77,13 @@ void CoreIMU::setPowerMode(PowerMode mode) void CoreIMU::registerOnDataAvailableCallback(data_available_callback_t const &callback) { _on_data_available_callback = callback; + + _on_data_available_wrapper_callback = [this] { + auto timestamp = rtos::Kernel::Clock::now(); + _event_queue.call([this, timestamp] { onDataAvailableHandler(timestamp); }); + }; + + setInterruptCallback(_on_data_available_wrapper_callback); } void CoreIMU::onDataAvailableHandler(auto timestamp) @@ -100,6 +107,28 @@ void CoreIMU::onDataAvailableHandler(auto timestamp) } } +void CoreIMU::enableOnDataAvailable() +{ + lsm6dsox_pin_int1_route_t lsm6dsox_int1 { + .drdy_xl = PROPERTY_ENABLE, + .den_flag = PROPERTY_ENABLE, + }; + lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1); + + setInterruptCallback(_on_data_available_wrapper_callback); +} + +void CoreIMU::disableOnDataAvailable() +{ + lsm6dsox_pin_int1_route_t lsm6dsox_int1 { + .drdy_xl = PROPERTY_DISABLE, + .den_flag = PROPERTY_DISABLE, + }; + lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1); + + setInterruptCallback({}); +} + auto CoreIMU::read(uint8_t register_address, uint16_t number_bytes_to_read, uint8_t *p_buffer) -> int32_t { // Send component address, without STOP condition @@ -136,20 +165,11 @@ auto CoreIMU::ptr_io_read(CoreIMU *handle, uint8_t read_address, uint8_t *p_buff return handle->read(read_address, number_bytes_to_read, p_buffer); } -void CoreIMU::setDataAvailableInterrupt() +void CoreIMU::setInterruptCallback(std::function const &callback) { - lsm6dsox_pin_int1_route_t lsm6dsox_int1 { - .drdy_xl = PROPERTY_ENABLE, - .den_flag = PROPERTY_ENABLE, - }; - lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1); - - auto data_available_callback = [this] { - auto timestamp = rtos::Kernel::Clock::now(); - _event_queue.call([this, timestamp] { onDataAvailableHandler(timestamp); }); - }; - - _irq.onRise(data_available_callback); + if (callback != nullptr) { + _irq.onRise(callback); + } } } // namespace leka diff --git a/drivers/CoreIMU/tests/CoreIMU_test.cpp b/drivers/CoreIMU/tests/CoreIMU_test.cpp index e47d3c471a..fdf60c0899 100644 --- a/drivers/CoreIMU/tests/CoreIMU_test.cpp +++ b/drivers/CoreIMU/tests/CoreIMU_test.cpp @@ -86,3 +86,19 @@ TEST_F(CoreIMUTest, emptyOnDataAvailableCallback) auto on_rise_callback = spy_InterruptIn_getRiseCallback(); on_rise_callback(); } + +TEST_F(CoreIMUTest, enableOnDataAvailable) +{ + EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); + EXPECT_CALL(mocki2c, read).Times(AtLeast(1)); + + coreimu.enableOnDataAvailable(); +} + +TEST_F(CoreIMUTest, disableOnDataAvailable) +{ + EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); + EXPECT_CALL(mocki2c, read).Times(AtLeast(1)); + + coreimu.disableOnDataAvailable(); +} diff --git a/include/interface/drivers/IMU.hpp b/include/interface/drivers/IMU.hpp index 45a819154e..62e4ee44dc 100644 --- a/include/interface/drivers/IMU.hpp +++ b/include/interface/drivers/IMU.hpp @@ -47,8 +47,12 @@ class IMU using data_available_callback_t = std::function; - virtual void init() = 0; + virtual void init() = 0; + virtual void registerOnDataAvailableCallback(data_available_callback_t const &callback) = 0; - virtual void setPowerMode(PowerMode) = 0; + virtual void enableOnDataAvailable() = 0; + virtual void disableOnDataAvailable() = 0; + + virtual void setPowerMode(PowerMode) = 0; }; } // namespace leka::interface diff --git a/tests/unit/mocks/mocks/leka/IMU.h b/tests/unit/mocks/mocks/leka/IMU.h index 4020cc6ec9..2178868144 100644 --- a/tests/unit/mocks/mocks/leka/IMU.h +++ b/tests/unit/mocks/mocks/leka/IMU.h @@ -16,6 +16,8 @@ class IMU : public interface::IMU MOCK_METHOD(void, setPowerMode, (PowerMode), (override)); void registerOnDataAvailableCallback(data_available_callback_t const &cb) override { data_available_callback = cb; } + MOCK_METHOD(void, enableOnDataAvailable, (), (override)); + MOCK_METHOD(void, disableOnDataAvailable, (), (override)); void call_data_available_callback(const SensorData &data) { data_available_callback(data); }