✨ (imu): Add wake up interrupt

leka · Jul 10, 2024 · a3ae74e · a3ae74e
1 parent 0f38bca
commit a3ae74e
Show file tree

Hide file tree

Showing 5 changed files with 158 additions and 0 deletions.
diff --git 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<void()> 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<uint8_t, kMaxBufferLength> _rx_buffer {};
+
+	std::function<void()> _on_wake_up_callback {};
+	std::function<void()> _on_wake_up_wrapper_callback {};
 };
 
 }	// namespace leka
diff --git 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<void()> 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<void()> 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
@@ -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<void()> 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<void(const leka::interface::IMU::SensorData &data)> {};
+	auto mock_wake_up_callback		  = MockFunction<void()> {};
+	auto on_rise_callback			  = mbed::Callback<void()> {};
+
+	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
@@ -53,6 +53,10 @@ class IMU
 	virtual void enableOnDataAvailable()													= 0;
 	virtual void disableOnDataAvailable()													= 0;
 
+	virtual void registerOnWakeUpCallback(std::function<void()> 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
@@ -19,10 +19,16 @@ class IMU : public interface::IMU
 	MOCK_METHOD(void, enableOnDataAvailable, (), (override));
 	MOCK_METHOD(void, disableOnDataAvailable, (), (override));
 
+	void registerOnWakeUpCallback(std::function<void()> 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<void()> wake_up_callback {};
 };
 
 }	// namespace leka::mock