From 89da6a4df505606384f1b1be10b101f459fd0918 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 | 6 ++ drivers/CoreIMU/source/CoreIMU.cpp | 51 +++++++++++++++ drivers/CoreIMU/tests/CoreIMU_test.cpp | 88 ++++++++++++++++++++++++++ include/interface/drivers/IMU.hpp | 4 ++ tests/unit/mocks/mocks/leka/IMU.h | 6 ++ 5 files changed, 155 insertions(+) diff --git a/drivers/CoreIMU/include/CoreIMU.hpp b/drivers/CoreIMU/include/CoreIMU.hpp index 457ad8aa1..0d7c38f4a 100644 --- a/drivers/CoreIMU/include/CoreIMU.hpp +++ b/drivers/CoreIMU/include/CoreIMU.hpp @@ -26,6 +26,10 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled void enableOnDataReadyInterrupt() final; void disableOnDataReadyInterrupt() final; + void registerOnWakeUpCallback(std::function const &callback) final; + void enableOnWakeUpInterrupt() final; + void disableOnWakeUpInterrupt() final; + void setPowerMode(PowerMode mode) final; void enableDeepSleep() final; @@ -58,6 +62,8 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled static constexpr uint8_t kMaxBufferLength = 32; std::array _rx_buffer {}; + + std::function _on_wake_up_callback {}; }; } // namespace leka diff --git a/drivers/CoreIMU/source/CoreIMU.cpp b/drivers/CoreIMU/source/CoreIMU.cpp index c5c2f370d..4966482e7 100644 --- a/drivers/CoreIMU/source/CoreIMU.cpp +++ b/drivers/CoreIMU/source/CoreIMU.cpp @@ -137,6 +137,55 @@ void CoreIMU::disableDeepSleep() setPowerMode(interface::IMU::PowerMode::Normal); } +void CoreIMU::registerOnWakeUpCallback(std::function const &callback) +{ + _on_wake_up_callback = 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); + + auto on_wake_up_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(); + } + }); + }; + + setDataReadyInterruptCallback(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); + + setDataReadyInterruptCallback({}); +} + 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 @@ -177,6 +226,8 @@ void CoreIMU::setDataReadyInterruptCallback(std::function const &callbac { if (callback) { _irq.onRise(callback); + } else { + _irq.onRise([] {}); } } diff --git a/drivers/CoreIMU/tests/CoreIMU_test.cpp b/drivers/CoreIMU/tests/CoreIMU_test.cpp index fd4150538..cb2ba2dbc 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; @@ -120,3 +121,90 @@ TEST_F(CoreIMUTest, disableDeepSleep) coreimu.disableDeepSleep(); } + +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.registerOnDataReadyCallback(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.enableOnDataReadyInterrupt(); + 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.disableOnDataReadyInterrupt(); + 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(); + } + + { + // Disable Wake Up + EXPECT_CALL(mock_data_available_callback, Call).Times(0); + EXPECT_CALL(mock_wake_up_callback, Call).Times(0); + coreimu.disableOnWakeUpInterrupt(); + on_rise_callback = spy_InterruptIn_getRiseCallback(); + on_rise_callback(); + } +} diff --git a/include/interface/drivers/IMU.hpp b/include/interface/drivers/IMU.hpp index 74ac00f5e..b255cb2ba 100644 --- a/include/interface/drivers/IMU.hpp +++ b/include/interface/drivers/IMU.hpp @@ -54,6 +54,10 @@ class IMU virtual void enableOnDataReadyInterrupt() = 0; virtual void disableOnDataReadyInterrupt() = 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 0eefe4b80..e29f4442c 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, enableOnDataReadyInterrupt, (), (override)); MOCK_METHOD(void, disableOnDataReadyInterrupt, (), (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_ready_callback(const SensorData &data) { data_ready_callback(data); } + void call_wake_up_callback() { wake_up_callback(); } private: data_ready_callback_t data_ready_callback {}; + std::function wake_up_callback {}; }; } // namespace leka::mock