✅ (Tests): Update & create new tests

libs/IMUKit/tests/IMUKit_test.cpp

@@ -66,7 +66,7 @@ TEST_F(IMUKitTest, setOrigin)
 
 TEST_F(IMUKitTest, onDataReady)
 {
-	testing::MockFunction<void(interface::EulerAngles angles)> mock_callback {};
+	testing::MockFunction<void(EulerAngles angles)> mock_callback {};
 
 	imukit.onEulerAnglesReady(mock_callback.AsStdFunction());
 

libs/MotionKit/tests/MotionKit_test.cpp

@@ -7,14 +7,13 @@
 #include "IMUKit.hpp"
 #include "gtest/gtest.h"
 #include "mocks/leka/CoreMotor.h"
-#include "mocks/leka/IMUKit.h"
+#include "mocks/leka/IMUKit.hpp"
 #include "mocks/leka/Timeout.h"
 
 using namespace leka;
 
 using ::testing::_;
 using ::testing::AtLeast;
-using ::testing::MockFunction;
 
 // TODO(@leka/dev-embedded): temporary fix, changes are needed when updating fusion algorithm
 
@@ -36,9 +35,7 @@ class MotionKitTest : public ::testing::Test
 
 	mock::Timeout mock_timeout {};
 
-	const interface::EulerAngles angles {
-		0.0F, 0.0F, 0.F
-	};
+	const EulerAngles angles {0.0F, 0.0F, 0.F};
 
 	mock::IMUKit mock_imukit {};
 
@@ -90,15 +87,16 @@ TEST_F(MotionKitTest, rotateCounterClockwise)
 
 TEST_F(MotionKitTest, startStabilisation)
 {
+	const EulerAngles angles_quarter_left {0.0F, 0.0F, 90.F};
+
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
 	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
 
 	motion.startStabilisation();
-
-	mock_imukit.call_angles_ready_callback(angles);
+	mock_imukit.call_angles_ready_callback(angles_quarter_left);
 }
 
 TEST_F(MotionKitTest, rotateAndStop)
@@ -116,13 +114,12 @@ TEST_F(MotionKitTest, rotateAndStop)
 	EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)).Times(AtLeast(1));
 
 	motion.rotate(1, Rotation::clockwise);
+	mock_imukit.call_angles_ready_callback(angles);
 
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
-	mock_imukit.call_angles_ready_callback(angles);
-
 	motion.stop();
 }
 
@@ -143,31 +140,31 @@ TEST_F(MotionKitTest, rotateAndTimeOutOver)
 	EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)).Times(AtLeast(1));
 
 	motion.rotate(1, Rotation::clockwise);
+	mock_imukit.call_angles_ready_callback(angles);
 
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
-	mock_imukit.call_angles_ready_callback(angles);
-
 	on_timeout_callback();
 }
 
 TEST_F(MotionKitTest, startStabilisationAndStop)
 {
+	const EulerAngles angles_quarter_left {0.0F, 0.0F, 90.F};
+
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
 	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
 
 	motion.startStabilisation();
+	mock_imukit.call_angles_ready_callback(angles_quarter_left);
 
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
-	mock_imukit.call_angles_ready_callback(angles);
-
 	motion.stop();
 }

libs/MotionKit/tests/PIDRotation_test.cpp

@@ -0,0 +1,67 @@
+// Leka - LekaOS
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "PIDRotation.hpp"
+
+#include "gtest/gtest.h"
+
+using namespace leka;
+
+class PIDRotationTest : public ::testing::Test
+{
+  protected:
+	PIDRotationTest() = default;
+
+	// void SetUp() override {}
+	//  void TearDown() override {}
+
+	PIDRotation pid {};
+
+	float yaw = 0.F;
+
+	float halfturn_error_speed = 1.85F;			//? mapSpeed(180 * (Kp + Kd)/KDeltaT)
+	float one_turn_error_speed = 3.55000019F;	//? mapSpeed(360 * (Kp + Kd)/KDeltaT)
+	float two_turn_error_speed = 6.95000029F;	//? mapSpeed(720 * (Kp + Kd)/KDeltaT)
+};
+
+TEST_F(PIDRotationTest, initialization)
+{
+	ASSERT_NE(&pid, nullptr);
+}
+
+TEST_F(PIDRotationTest, processRotationAngleDefaultPosition)
+{
+	auto target = 0.F;
+
+	auto speed = pid.processRotationAngle(target, yaw);
+
+	EXPECT_EQ(speed, 0.F);
+}
+
+TEST_F(PIDRotationTest, processRotationAngleHalfTurn)
+{
+	auto target = 180.F;
+
+	auto speed = pid.processRotationAngle(target, yaw);
+
+	EXPECT_EQ(speed, halfturn_error_speed);
+}
+
+TEST_F(PIDRotationTest, processRotationAngleOneTurn)
+{
+	auto target = 360.F;
+
+	auto speed = pid.processRotationAngle(target, yaw);
+
+	EXPECT_EQ(speed, one_turn_error_speed);
+}
+
+TEST_F(PIDRotationTest, processRotationAngle2Turn)
+{
+	auto target = 720.F;
+
+	auto speed = pid.processRotationAngle(target, yaw);
+
+	EXPECT_EQ(speed, two_turn_error_speed);
+}

libs/MotionKit/tests/PIDStabilization_test.cpp

@@ -0,0 +1,81 @@
+// Leka - LekaOS
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "PIDStabilization.hpp"
+
+#include "gtest/gtest.h"
+
+using namespace leka;
+
+class PIDStabilizationTest : public ::testing::Test
+{
+  protected:
+	PIDStabilizationTest() = default;
+
+	// void SetUp() override {}
+	//  void TearDown() override {}
+
+	PIDStabilization pid {};
+
+	EulerAngles target {.pitch = 0.F, .roll = 0.F, .yaw = 0.F};
+	EulerAngles current {};
+
+	float quarterturn_error_speed = 1.F;	 //? mapSpeed(90 * (Kp + Kd)/ KDeltaT)
+	float halfturn_error_speed	  = 1.85F;	 //? mapSpeed(180 * (Kp + Kd)/ KDeltaT)
+};
+
+TEST_F(PIDStabilizationTest, initialization)
+{
+	ASSERT_NE(&pid, nullptr);
+}
+
+TEST_F(PIDStabilizationTest, processStabilizationAngleDefaultPosition)
+{
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = 0.F};
+
+	auto [speed, direction] = pid.processStabilizationAngle(target, current);
+
+	EXPECT_EQ(speed, 0.F);
+	EXPECT_EQ(direction, Rotation::clockwise);
+}
+
+TEST_F(PIDStabilizationTest, processStabilizationAngleRolledOverAHalfRight)
+{
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = 180.F};
+
+	auto [speed, direction] = pid.processStabilizationAngle(target, current);
+
+	EXPECT_EQ(speed, halfturn_error_speed);
+	EXPECT_EQ(direction, Rotation::counterClockwise);
+}
+
+TEST_F(PIDStabilizationTest, processStabilizationAngleRolledOverAQuarterRight)
+{
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = 90.F};
+
+	auto [speed, direction] = pid.processStabilizationAngle(target, current);
+
+	EXPECT_EQ(speed, quarterturn_error_speed);
+	EXPECT_EQ(direction, Rotation::counterClockwise);
+}
+
+TEST_F(PIDStabilizationTest, processStabilizationAngleRolledOverAQuarterLeft)
+{
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = -90.F};
+
+	auto [speed, direction] = pid.processStabilizationAngle(target, current);
+
+	EXPECT_EQ(speed, quarterturn_error_speed);
+	EXPECT_EQ(direction, Rotation::clockwise);
+}
+
+TEST_F(PIDStabilizationTest, processStabilizationAngleRolledOverAHalfLeft)
+{
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = -180.F};
+
+	auto [speed, direction] = pid.processStabilizationAngle(target, current);
+
+	EXPECT_EQ(speed, halfturn_error_speed);
+	EXPECT_EQ(direction, Rotation::clockwise);
+}