♻️ (imu): Break setDataReadyInterrupt in multiple methods

leka · Jul 18, 2024 · 9d2d183 · 9d2d183
1 parent 7e8481d
commit 9d2d183
Show file tree

Hide file tree

Showing 5 changed files with 64 additions and 17 deletions.
diff --git a/drivers/CoreIMU/include/CoreIMU.hpp b/drivers/CoreIMU/include/CoreIMU.hpp
@@ -23,6 +23,8 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled
 	void init() final;
 
 	void registerOnDataReadyCallback(data_ready_callback_t const &callback) final;
+	void enableOnDataReady() final;
+	void disableOnDataReady() final;
 
 	void setPowerMode(PowerMode mode) final;
 
@@ -39,7 +41,8 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled
 							uint16_t number_bytes_to_read) -> int32_t;
 
 	void onDataReadyHandler(auto timestamp);
-	void setDataReadyInterrupt();
+
+	void setDataReadyInterruptCallback(std::function<void()> const &callback);
 
 	interface::I2C &_i2c;
 	CoreEventQueue _event_queue {};
@@ -52,6 +55,7 @@ class CoreIMU : public interface::IMU, public interface::DeepSleepEnabled
 	std::array<int16_t, 3> data_raw_xl {};
 	std::array<int16_t, 3> data_raw_gy {};
 	data_ready_callback_t _on_data_ready_callback {};
+	std::function<void()> _on_data_ready_wrapper_callback {};
 
 	static constexpr uint8_t kMaxBufferLength = 32;
 	std::array<uint8_t, kMaxBufferLength> _rx_buffer {};

diff --git a/drivers/CoreIMU/source/CoreIMU.cpp b/drivers/CoreIMU/source/CoreIMU.cpp
@@ -33,7 +33,7 @@ void CoreIMU::init()
 	lsm6dsox_dataready_pulsed_t data_ready_pulsed {LSM6DSOX_DRDY_PULSED};
 	lsm6dsox_data_ready_mode_set(&_register_io_function, data_ready_pulsed);
 
-	setDataReadyInterrupt();
+	enableOnDataReady();
 }
 
 void CoreIMU::setPowerMode(PowerMode mode)
@@ -77,6 +77,13 @@ void CoreIMU::setPowerMode(PowerMode mode)
 void CoreIMU::registerOnDataReadyCallback(data_ready_callback_t const &callback)
 {
 	_on_data_ready_callback = callback;
+
+	_on_data_ready_wrapper_callback = [this] {
+		auto timestamp = rtos::Kernel::Clock::now();
+		_event_queue.call([this, timestamp] { onDataReadyHandler(timestamp); });
+	};
+
+	setDataReadyInterruptCallback(_on_data_ready_wrapper_callback);
 }
 
 void CoreIMU::onDataReadyHandler(auto timestamp)
@@ -100,6 +107,28 @@ void CoreIMU::onDataReadyHandler(auto timestamp)
 	}
 }
 
+void CoreIMU::enableOnDataReady()
+{
+	lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
+		.drdy_xl  = PROPERTY_ENABLE,
+		.den_flag = PROPERTY_ENABLE,
+	};
+	lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);
+
+	setDataReadyInterruptCallback(_on_data_ready_wrapper_callback);
+}
+
+void CoreIMU::disableOnDataReady()
+{
+	lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
+		.drdy_xl  = PROPERTY_DISABLE,
+		.den_flag = PROPERTY_DISABLE,
+	};
+	lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);
+
+	setDataReadyInterruptCallback({});
+}
+
 void CoreIMU::enableDeepSleep()
 {
 	setPowerMode(interface::IMU::PowerMode::Off);
@@ -146,20 +175,11 @@ auto CoreIMU::ptr_io_read(CoreIMU *handle, uint8_t read_address, uint8_t *p_buff
 	return handle->read(read_address, number_bytes_to_read, p_buffer);
 }
 
-void CoreIMU::setDataReadyInterrupt()
+void CoreIMU::setDataReadyInterruptCallback(std::function<void()> const &callback)
 {
-	lsm6dsox_pin_int1_route_t lsm6dsox_int1 {
-		.drdy_xl  = PROPERTY_ENABLE,
-		.den_flag = PROPERTY_ENABLE,
-	};
-	lsm6dsox_pin_int1_route_set(&_register_io_function, lsm6dsox_int1);
-
-	auto data_ready_callback = [this] {
-		auto timestamp = rtos::Kernel::Clock::now();
-		_event_queue.call([this, timestamp] { onDataReadyHandler(timestamp); });
-	};
-
-	_irq.onRise(data_ready_callback);
+	if (callback) {
+		_irq.onRise(callback);
+	}
 }
 
 }	// namespace leka
diff --git a/drivers/CoreIMU/tests/CoreIMU_test.cpp b/drivers/CoreIMU/tests/CoreIMU_test.cpp
@@ -87,6 +87,22 @@ TEST_F(CoreIMUTest, emptyOnDataReadyCallback)
 	on_rise_callback();
 }
 
+TEST_F(CoreIMUTest, enableOnDataReady)
+{
+	EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
+	EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
+
+	coreimu.enableOnDataReady();
+}
+
+TEST_F(CoreIMUTest, disableOnDataReady)
+{
+	EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
+	EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
+
+	coreimu.disableOnDataReady();
+}
+
 TEST_F(CoreIMUTest, enableDeepSleep)
 {
 	EXPECT_CALL(mocki2c, write).Times(AtLeast(1));

diff --git a/include/interface/drivers/IMU.hpp b/include/interface/drivers/IMU.hpp
@@ -47,8 +47,13 @@ class IMU
 
 	using data_ready_callback_t = std::function<void(const SensorData)>;
 
-	virtual void init()																= 0;
+	virtual void init() = 0;
+
 	virtual void registerOnDataReadyCallback(data_ready_callback_t const &callback) = 0;
-	virtual void setPowerMode(PowerMode)											= 0;
+
+	virtual void enableOnDataReady()  = 0;
+	virtual void disableOnDataReady() = 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
@@ -16,6 +16,8 @@ class IMU : public interface::IMU
 	MOCK_METHOD(void, setPowerMode, (PowerMode), (override));
 
 	void registerOnDataReadyCallback(data_ready_callback_t const &cb) override { data_ready_callback = cb; }
+	MOCK_METHOD(void, enableOnDataReady, (), (override));
+	MOCK_METHOD(void, disableOnDataReady, (), (override));
 
 	void call_data_ready_callback(const SensorData &data) { data_ready_callback(data); }