✨ (imu): Add wake up interrupt

leka · Aug 26, 2024 · 89da6a4 · 89da6a4
1 parent f8d89cc
commit 89da6a4
Show file tree

Hide file tree

Showing 5 changed files with 155 additions and 0 deletions.
diff --git 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<void()> 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<uint8_t, kMaxBufferLength> _rx_buffer {};
+
+	std::function<void()> _on_wake_up_callback {};
 };
 
 }	// namespace leka
diff --git 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<void()> 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<void()> 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
@@ -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<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.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
@@ -54,6 +54,10 @@ class IMU
 	virtual void enableOnDataReadyInterrupt()  = 0;
 	virtual void disableOnDataReadyInterrupt() = 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, enableOnDataReadyInterrupt, (), (override));
 	MOCK_METHOD(void, disableOnDataReadyInterrupt, (), (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_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<void()> wake_up_callback {};
 };
 
 }	// namespace leka::mock