♻️ (imu): Break setDataAvailableInterrupt in multiple methods

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

Hide file tree

Showing 5 changed files with 63 additions and 17 deletions.
diff --git a/drivers/CoreIMU/include/CoreIMU.hpp b/drivers/CoreIMU/include/CoreIMU.hpp
@@ -22,6 +22,8 @@ class CoreIMU : public interface::IMU
 	void init() final;
 
 	void registerOnDataAvailableCallback(data_available_callback_t const &callback) final;
+	void enableOnDataAvailable() final;
+	void disableOnDataAvailable() final;
 
 	void setPowerMode(PowerMode mode) final;
 
@@ -35,7 +37,8 @@ class CoreIMU : public interface::IMU
 		-> int32_t;
 
 	void onDataAvailableHandler(auto timestamp);
-	void setDataAvailableInterrupt();
+
+	void setInterruptCallback(std::function<void()> const &callback);
 
 	interface::I2C &_i2c;
 	CoreEventQueue _event_queue {};
@@ -48,6 +51,7 @@ class CoreIMU : public interface::IMU
 	std::array<int16_t, 3> data_raw_xl {};
 	std::array<int16_t, 3> data_raw_gy {};
 	data_available_callback_t _on_data_available_callback {};
+	std::function<void()> _on_data_available_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);
 
-	setDataAvailableInterrupt();
+	enableOnDataAvailable();
 }
 
 void CoreIMU::setPowerMode(PowerMode mode)
@@ -77,6 +77,13 @@ void CoreIMU::setPowerMode(PowerMode mode)
 void CoreIMU::registerOnDataAvailableCallback(data_available_callback_t const &callback)
 {
 	_on_data_available_callback = callback;
+
+	_on_data_available_wrapper_callback = [this] {
+		auto timestamp = rtos::Kernel::Clock::now();
+		_event_queue.call([this, timestamp] { onDataAvailableHandler(timestamp); });
+	};
+
+	setInterruptCallback(_on_data_available_wrapper_callback);
 }
 
 void CoreIMU::onDataAvailableHandler(auto timestamp)
@@ -100,6 +107,28 @@ void CoreIMU::onDataAvailableHandler(auto timestamp)
 	}
 }
 
+void CoreIMU::enableOnDataAvailable()
+{
+	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);
+
+	setInterruptCallback(_on_data_available_wrapper_callback);
+}
+
+void CoreIMU::disableOnDataAvailable()
+{
+	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);
+
+	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
@@ -136,20 +165,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::setDataAvailableInterrupt()
+void CoreIMU::setInterruptCallback(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_available_callback = [this] {
-		auto timestamp = rtos::Kernel::Clock::now();
-		_event_queue.call([this, timestamp] { onDataAvailableHandler(timestamp); });
-	};
-
-	_irq.onRise(data_available_callback);
+	if (callback != nullptr) {
+		_irq.onRise(callback);
+	}
 }
 
 }	// namespace leka
diff --git a/drivers/CoreIMU/tests/CoreIMU_test.cpp b/drivers/CoreIMU/tests/CoreIMU_test.cpp
@@ -86,3 +86,19 @@ TEST_F(CoreIMUTest, emptyOnDataAvailableCallback)
 	auto on_rise_callback = spy_InterruptIn_getRiseCallback();
 	on_rise_callback();
 }
+
+TEST_F(CoreIMUTest, enableOnDataAvailable)
+{
+	EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
+	EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
+
+	coreimu.enableOnDataAvailable();
+}
+
+TEST_F(CoreIMUTest, disableOnDataAvailable)
+{
+	EXPECT_CALL(mocki2c, write).Times(AtLeast(1));
+	EXPECT_CALL(mocki2c, read).Times(AtLeast(1));
+
+	coreimu.disableOnDataAvailable();
+}
diff --git a/include/interface/drivers/IMU.hpp b/include/interface/drivers/IMU.hpp
@@ -47,8 +47,12 @@ class IMU
 
 	using data_available_callback_t = std::function<void(const SensorData)>;
 
-	virtual void init()																		= 0;
+	virtual void init() = 0;
+
 	virtual void registerOnDataAvailableCallback(data_available_callback_t const &callback) = 0;
-	virtual void setPowerMode(PowerMode)													= 0;
+	virtual void enableOnDataAvailable()													= 0;
+	virtual void disableOnDataAvailable()													= 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 registerOnDataAvailableCallback(data_available_callback_t const &cb) override { data_available_callback = cb; }
+	MOCK_METHOD(void, enableOnDataAvailable, (), (override));
+	MOCK_METHOD(void, disableOnDataAvailable, (), (override));
 
 	void call_data_available_callback(const SensorData &data) { data_available_callback(data); }