diff --git a/drivers/CoreIMU/include/CoreIMU.hpp b/drivers/CoreIMU/include/CoreIMU.hpp index a6d860199..457ad8aa1 100644 --- a/drivers/CoreIMU/include/CoreIMU.hpp +++ b/drivers/CoreIMU/include/CoreIMU.hpp @@ -23,6 +23,8 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled void init() final; void registerOnDataReadyCallback(data_ready_callback_t const &callback) final; + void enableOnDataReadyInterrupt() final; + void disableOnDataReadyInterrupt() final; void setPowerMode(PowerMode mode) final; @@ -39,7 +41,8 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled uint16_t number_bytes_to_read) -> int32_t; void onDataReadyHandler(auto timestamp); - void setDataReadyInterrupt(); + + void setDataReadyInterruptCallback(std::function const &callback); interface::I2C &_i2c; CoreEventQueue _event_queue {}; diff --git a/drivers/CoreIMU/source/CoreIMU.cpp b/drivers/CoreIMU/source/CoreIMU.cpp index d351696ef..46e03c687 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); - setDataReadyInterrupt(); + enableOnDataReadyInterrupt(); } void CoreIMU::setPowerMode(PowerMode mode) @@ -100,6 +100,33 @@ void CoreIMU::onDataReadyHandler(auto timestamp) } } +void CoreIMU::enableOnDataReadyInterrupt() +{ + 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 on_data_ready_callback = [this] { + auto timestamp = rtos::Kernel::Clock::now(); + _event_queue.call([this, timestamp] { onDataReadyHandler(timestamp); }); + }; + + setDataReadyInterruptCallback(on_data_ready_callback); +} + +void CoreIMU::disableOnDataReadyInterrupt() +{ + 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); + + setDataReadyInterruptCallback({}); +} + void CoreIMU::enableDeepSleep() { setPowerMode(interface::IMU::PowerMode::Off); @@ -146,20 +173,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::setDataReadyInterrupt() +void CoreIMU::setDataReadyInterruptCallback(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_ready_callback = [this] { - auto timestamp = rtos::Kernel::Clock::now(); - _event_queue.call([this, timestamp] { onDataReadyHandler(timestamp); }); - }; - - _irq.onRise(data_ready_callback); + if (callback) { + _irq.onRise(callback); + } } } // namespace leka diff --git a/drivers/CoreIMU/tests/CoreIMU_test.cpp b/drivers/CoreIMU/tests/CoreIMU_test.cpp index 3b1fe2da1..fd4150538 100644 --- a/drivers/CoreIMU/tests/CoreIMU_test.cpp +++ b/drivers/CoreIMU/tests/CoreIMU_test.cpp @@ -87,6 +87,22 @@ TEST_F(CoreIMUTest, emptyOnDataReadyCallback) on_rise_callback(); } +TEST_F(CoreIMUTest, enableOnDataReadyInterrupt) +{ + EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); + EXPECT_CALL(mocki2c, read).Times(AtLeast(1)); + + coreimu.enableOnDataReadyInterrupt(); +} + +TEST_F(CoreIMUTest, disableOnDataReadyInterrupt) +{ + EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); + EXPECT_CALL(mocki2c, read).Times(AtLeast(1)); + + coreimu.disableOnDataReadyInterrupt(); +} + TEST_F(CoreIMUTest, enableDeepSleep) { EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); diff --git a/include/interface/drivers/IMU.hpp b/include/interface/drivers/IMU.hpp index 009c6069b..74ac00f5e 100644 --- a/include/interface/drivers/IMU.hpp +++ b/include/interface/drivers/IMU.hpp @@ -47,8 +47,13 @@ class IMU using data_ready_callback_t = std::function; - virtual void init() = 0; + virtual void init() = 0; + virtual void registerOnDataReadyCallback(data_ready_callback_t const &callback) = 0; - virtual void setPowerMode(PowerMode) = 0; + + virtual void enableOnDataReadyInterrupt() = 0; + virtual void disableOnDataReadyInterrupt() = 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 6f8218228..0eefe4b80 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 registerOnDataReadyCallback(data_ready_callback_t const &cb) override { data_ready_callback = cb; } + MOCK_METHOD(void, enableOnDataReadyInterrupt, (), (override)); + MOCK_METHOD(void, disableOnDataReadyInterrupt, (), (override)); void call_data_ready_callback(const SensorData &data) { data_ready_callback(data); }