✨ (IMUKit): Add onEulerAnglesReady function

leka · Mar 6, 2023 · 0226be2 · 0226be2
1 parent d5df351
commit 0226be2
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 0 deletions.
diff --git a/libs/IMUKit/include/IMUKit.hpp b/libs/IMUKit/include/IMUKit.hpp
@@ -19,13 +19,15 @@ class IMUKit : public interface::IMUKit
 	void stop() final;
 
 	void setOrigin() final;
+	void onEulerAnglesReady(angles_ready_callback_t const &callback) final;
 	[[nodiscard]] auto getEulerAngles() const -> EulerAngles final;
 
   private:
 	void drdy_callback(interface::LSM6DSOX::SensorData data);
 
 	interface::LSM6DSOX &_lsm6dsox;
 	EulerAngles _euler_angles {};
+	angles_ready_callback_t _on_euler_angles_rdy_callback {};
 };
 
 }	// namespace leka
diff --git a/libs/IMUKit/source/IMUKit.cpp b/libs/IMUKit/source/IMUKit.cpp
@@ -83,6 +83,11 @@ auto IMUKit::getEulerAngles() const -> EulerAngles
 	return _euler_angles;
 }
 
+void IMUKit::onEulerAnglesReady(angles_ready_callback_t const &callback)
+{
+	_on_euler_angles_rdy_callback = callback;
+}
+
 void IMUKit::drdy_callback(const interface::LSM6DSOX::SensorData data)
 {
 	// ? Note: For a detailed explanation on the code below, checkout
@@ -113,4 +118,8 @@ void IMUKit::drdy_callback(const interface::LSM6DSOX::SensorData data)
 		   .roll  = euler.angle.roll,
 		   .yaw	  = euler.angle.yaw,
 	   };
+
+	if (_on_euler_angles_rdy_callback) {
+		_on_euler_angles_rdy_callback(_euler_angles);
+	}
 };
diff --git a/libs/IMUKit/tests/IMUKit_test.cpp b/libs/IMUKit/tests/IMUKit_test.cpp
@@ -66,6 +66,41 @@ TEST_F(IMUKitTest, setOrigin)
 
 TEST_F(IMUKitTest, onDataReady)
 {
+	testing::MockFunction<void(EulerAngles angles)> mock_callback {};
+
+	imukit.onEulerAnglesReady(mock_callback.AsStdFunction());
+
+	const auto data_initial = interface::LSM6DSOX::SensorData {
+		.xl = {.x = 0.F, .y = 0.F, .z = 0.F}, .gy = {.x = 0.F, .y = 0.F, .z = 0.F }
+	};
+
+	EXPECT_CALL(mock_callback, Call);
+
+	mock_lsm6dox.call_drdy_callback(data_initial);
+
+	const auto angles_initial = imukit.getEulerAngles();
+
+	spy_kernel_addElapsedTimeToTickCount(80ms);
+
+	const auto data_updated = interface::LSM6DSOX::SensorData {
+		.xl = {.x = 1.F, .y = 2.F, .z = 3.F}, .gy = {.x = 1.F, .y = 2.F, .z = 3.F }
+	};
+
+	EXPECT_CALL(mock_callback, Call);
+
+	mock_lsm6dox.call_drdy_callback(data_updated);
+
+	auto angles_updated = imukit.getEulerAngles();
+
+	EXPECT_NE(angles_initial.pitch, angles_updated.pitch);
+	EXPECT_NE(angles_initial.roll, angles_updated.roll);
+	EXPECT_NE(angles_initial.yaw, angles_updated.yaw);
+}
+
+TEST_F(IMUKitTest, onDataReadyEmptyEulerAngleCallback)
+{
+	imukit.onEulerAnglesReady({});
+
 	const auto data_initial = interface::LSM6DSOX::SensorData {
 		.xl = {.x = 0.F, .y = 0.F, .z = 0.F}, .gy = {.x = 0.F, .y = 0.F, .z = 0.F }
 	};