From 4796bd3a39229eb414220e1543b4f69e6c610370 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Mon, 19 Feb 2024 17:50:38 +0100 Subject: [PATCH] :sparkles: (imu): Add wake up interrupt --- drivers/CoreIMU/include/CoreIMU.hpp | 7 ++++ drivers/CoreIMU/source/CoreIMU.cpp | 49 ++++++++++++++++++++++++++ drivers/CoreIMU/tests/CoreIMU_test.cpp | 38 ++++++++++++++++++++ include/interface/drivers/IMU.hpp | 4 +++ tests/unit/mocks/mocks/leka/IMU.h | 6 ++++ 5 files changed, 104 insertions(+) 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..4fad622bba 100644 --- a/drivers/CoreIMU/source/CoreIMU.cpp +++ b/drivers/CoreIMU/source/CoreIMU.cpp @@ -129,6 +129,55 @@ void CoreIMU::disableOnDataAvailable() setInterruptCallback({}); } +void CoreIMU::registerOnWakeUpCallback(std::function const &callback) +{ + _on_wake_up_callback = callback; + + _on_wake_up_wrapper_callback = [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(); + } + }; + + 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_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 diff --git a/drivers/CoreIMU/tests/CoreIMU_test.cpp b/drivers/CoreIMU/tests/CoreIMU_test.cpp index fdf60c0899..9eb6e70161 100644 --- a/drivers/CoreIMU/tests/CoreIMU_test.cpp +++ b/drivers/CoreIMU/tests/CoreIMU_test.cpp @@ -102,3 +102,41 @@ 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(1); // TODO: Should setup struct + + 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(); +} 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