diff --git a/drivers/CoreIMU/include/CoreIMU.hpp b/drivers/CoreIMU/include/CoreIMU.hpp index c2f25b7f62..88ddd894be 100644 --- a/drivers/CoreIMU/include/CoreIMU.hpp +++ b/drivers/CoreIMU/include/CoreIMU.hpp @@ -25,6 +25,10 @@ class CoreIMU : public interface::IMU void enableOnDataAvailable() final; void disableOnDataAvailable() final; + void registerOnWakeUpCallback(std::function const &callback) final; + void enableOnWakeUpInterrupt() final; + void disableOnWakeUpInterrupt() final; + void setPowerMode(PowerMode mode) final; private: @@ -55,6 +59,9 @@ class CoreIMU : public interface::IMU static constexpr uint8_t kMaxBufferLength = 32; std::array _rx_buffer {}; + + std::function _on_wake_up_callback {}; + std::function _on_wake_up_wrapper_callback {}; }; } // namespace leka diff --git a/drivers/CoreIMU/source/CoreIMU.cpp b/drivers/CoreIMU/source/CoreIMU.cpp index f0fd2b24e7..2ffc359703 100644 --- a/drivers/CoreIMU/source/CoreIMU.cpp +++ b/drivers/CoreIMU/source/CoreIMU.cpp @@ -129,6 +129,57 @@ void CoreIMU::disableOnDataAvailable() setInterruptCallback({}); } +void CoreIMU::registerOnWakeUpCallback(std::function const &callback) +{ + _on_wake_up_callback = callback; + + _on_wake_up_wrapper_callback = [this] { + _event_queue.call([this] { + lsm6dsox_all_sources_t all_source; + lsm6dsox_all_sources_get(&_register_io_function, &all_source); + + if (all_source.sleep_change && all_source.sleep_state == 0 && _on_wake_up_callback != nullptr) { + _on_wake_up_callback(); + } + }); + }; + + setInterruptCallback(_on_wake_up_wrapper_callback); +} + +void CoreIMU::enableOnWakeUpInterrupt() +{ + // ? Set filter and disable user offset + lsm6dsox_xl_hp_path_internal_set(&_register_io_function, LSM6DSOX_USE_SLOPE); + lsm6dsox_xl_usr_offset_on_wkup_set(&_register_io_function, 0); + + // ? Set Wakeup config + lsm6dsox_wkup_threshold_set(&_register_io_function, 2); + lsm6dsox_wkup_ths_weight_set(&_register_io_function, LSM6DSOX_LSb_FS_DIV_64); + lsm6dsox_wkup_dur_set(&_register_io_function, 0x02); + + // ? Set Activity config + lsm6dsox_act_sleep_dur_set(&_register_io_function, 0x02); + lsm6dsox_act_mode_set(&_register_io_function, LSM6DSOX_XL_AND_GY_NOT_AFFECTED); + + lsm6dsox_pin_int1_route_t lsm6dsox_int1 { + .sleep_change = PROPERTY_ENABLE, + }; + lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1); + + setInterruptCallback(_on_wake_up_wrapper_callback); +} + +void CoreIMU::disableOnWakeUpInterrupt() +{ + lsm6dsox_pin_int1_route_t lsm6dsox_int1 { + .sleep_change = 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 @@ -169,6 +220,8 @@ void CoreIMU::setInterruptCallback(std::function const &callback) { if (callback != nullptr) { _irq.onRise(callback); + } else { + _irq.onRise([] {}); } } diff --git a/drivers/CoreIMU/tests/CoreIMU_test.cpp b/drivers/CoreIMU/tests/CoreIMU_test.cpp index fdf60c0899..1e0a7d7333 100644 --- a/drivers/CoreIMU/tests/CoreIMU_test.cpp +++ b/drivers/CoreIMU/tests/CoreIMU_test.cpp @@ -13,6 +13,7 @@ using namespace leka; using testing::_; +using testing::AnyNumber; using testing::AtLeast; using testing::MockFunction; @@ -102,3 +103,90 @@ TEST_F(CoreIMUTest, disableOnDataAvailable) coreimu.disableOnDataAvailable(); } + +TEST_F(CoreIMUTest, onWakeUpCallback) +{ + MockFunction mock_callback; + + EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); + EXPECT_CALL(mocki2c, read).Times(AtLeast(1)); + EXPECT_CALL(mock_callback, Call).Times(AnyNumber()); + + coreimu.registerOnWakeUpCallback(mock_callback.AsStdFunction()); + + auto on_rise_callback = spy_InterruptIn_getRiseCallback(); + on_rise_callback(); +} + +TEST_F(CoreIMUTest, emptyOnWakeUpCallback) +{ + coreimu.registerOnWakeUpCallback({}); + + auto on_rise_callback = spy_InterruptIn_getRiseCallback(); + on_rise_callback(); +} + +TEST_F(CoreIMUTest, enableOnWakeUpInterrupt) +{ + EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); + EXPECT_CALL(mocki2c, read).Times(AtLeast(1)); + + coreimu.enableOnWakeUpInterrupt(); +} + +TEST_F(CoreIMUTest, disableOnWakeUpInterrupt) +{ + EXPECT_CALL(mocki2c, write).Times(AtLeast(1)); + EXPECT_CALL(mocki2c, read).Times(AtLeast(1)); + + coreimu.disableOnWakeUpInterrupt(); +} + +TEST_F(CoreIMUTest, switchCallbacks) +{ + auto mock_data_available_callback = MockFunction {}; + auto mock_wake_up_callback = MockFunction {}; + auto on_rise_callback = mbed::Callback {}; + + EXPECT_CALL(mocki2c, write).Times(AnyNumber()); + EXPECT_CALL(mocki2c, read).Times(AnyNumber()); + + coreimu.registerOnDataAvailableCallback(mock_data_available_callback.AsStdFunction()); + coreimu.registerOnWakeUpCallback(mock_wake_up_callback.AsStdFunction()); + + { + // Enable Data Available + EXPECT_CALL(mock_data_available_callback, Call).Times(1); + EXPECT_CALL(mock_wake_up_callback, Call).Times(0); + coreimu.enableOnDataAvailable(); + on_rise_callback = spy_InterruptIn_getRiseCallback(); + on_rise_callback(); + } + + { + // Enable Wake Up + EXPECT_CALL(mock_data_available_callback, Call).Times(0); + EXPECT_CALL(mock_wake_up_callback, Call).Times(AnyNumber()); + coreimu.enableOnWakeUpInterrupt(); + on_rise_callback = spy_InterruptIn_getRiseCallback(); + on_rise_callback(); + } + + { + // Enable Data Available + EXPECT_CALL(mock_data_available_callback, Call).Times(1); + EXPECT_CALL(mock_wake_up_callback, Call).Times(0); + coreimu.enableOnDataAvailable(); + on_rise_callback = spy_InterruptIn_getRiseCallback(); + on_rise_callback(); + } + + { + // Disable Data Available + EXPECT_CALL(mock_data_available_callback, Call).Times(0); + EXPECT_CALL(mock_wake_up_callback, Call).Times(0); + coreimu.disableOnDataAvailable(); + on_rise_callback = spy_InterruptIn_getRiseCallback(); + on_rise_callback(); + } +} diff --git a/include/interface/drivers/IMU.hpp b/include/interface/drivers/IMU.hpp index 62e4ee44dc..e319d23944 100644 --- a/include/interface/drivers/IMU.hpp +++ b/include/interface/drivers/IMU.hpp @@ -53,6 +53,10 @@ class IMU virtual void enableOnDataAvailable() = 0; virtual void disableOnDataAvailable() = 0; + virtual void registerOnWakeUpCallback(std::function const &callback) = 0; + virtual void enableOnWakeUpInterrupt() = 0; + virtual void disableOnWakeUpInterrupt() = 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 2178868144..6c1e285b3c 100644 --- a/tests/unit/mocks/mocks/leka/IMU.h +++ b/tests/unit/mocks/mocks/leka/IMU.h @@ -19,10 +19,16 @@ class IMU : public interface::IMU MOCK_METHOD(void, enableOnDataAvailable, (), (override)); MOCK_METHOD(void, disableOnDataAvailable, (), (override)); + void registerOnWakeUpCallback(std::function const &cb) override { wake_up_callback = cb; } + MOCK_METHOD(void, enableOnWakeUpInterrupt, (), (override)); + MOCK_METHOD(void, disableOnWakeUpInterrupt, (), (override)); + void call_data_available_callback(const SensorData &data) { data_available_callback(data); } + void call_wake_up_callback() { wake_up_callback(); } private: data_available_callback_t data_available_callback {}; + std::function wake_up_callback {}; }; } // namespace leka::mock