Fix float accuracy in ControlMathTest

The test was testing the result of 3D float vector operations with binary equality of the floating point numbers, which is not a valid assumption to make for floating point math. This change switches to proper comparisons with float accuracy and compares vectors using the norm of their difference.

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -47,29 +47,29 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	float yaw = 0.0f;
 	vehicle_attitude_setpoint_s att{};
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_EQ(att.roll_body, 0);
-	EXPECT_EQ(att.pitch_body, 0);
-	EXPECT_EQ(att.yaw_body, 0);
-	EXPECT_EQ(att.thrust_body[2], -1.f);
+	EXPECT_FLOAT_EQ(att.roll_body, 0.0f);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.0f);
+	EXPECT_FLOAT_EQ(att.yaw_body, 0.0f);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
 	yaw = M_PI_2_F;
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_EQ(att.roll_body, 0);
-	EXPECT_EQ(att.pitch_body, 0);
-	EXPECT_EQ(att.yaw_body, M_PI_2_F);
-	EXPECT_EQ(att.thrust_body[2], -1.f);
+	EXPECT_FLOAT_EQ(att.roll_body, 0.0f);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.0f);
+	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 
 	/* expected: same as before but roll 180
 	 * reason: thrust points straight down and order Euler
 	 * order is: 1. roll, 2. pitch, 3. yaw */
 	thr = Vector3f(0.0f, 0.0f, 1.0f);
 	thrustToAttitude(thr, yaw, att);
-	EXPECT_NEAR(att.roll_body, -M_PI_F, 1e-4);
-	EXPECT_EQ(att.pitch_body, 0);
-	EXPECT_EQ(att.yaw_body, M_PI_2_F);
-	EXPECT_EQ(att.thrust_body[2], -1.f);
+	EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
+	EXPECT_FLOAT_EQ(att.pitch_body, 0.0f);
+	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
+	EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
 }
 
 TEST(ControlMathTest, ConstrainXYPriorities)
@@ -80,29 +80,29 @@ TEST(ControlMathTest, ConstrainXYPriorities)
 	Vector2f v1(v0(1), -v0(0));
 
 	Vector2f v_r = constrainXY(v0, v1, max);
-	EXPECT_EQ(v_r(0), max);
-	EXPECT_EQ(v_r(1), 0);
+	EXPECT_FLOAT_EQ(v_r(0), max);
+	EXPECT_FLOAT_EQ(v_r(1), 0);
 
 	// norm of v1 exceeds max but v0 is zero
 	v0.zero();
 	v_r = constrainXY(v0, v1, max);
-	EXPECT_EQ(v_r(1), -max);
-	EXPECT_EQ(v_r(0), 0);
+	EXPECT_FLOAT_EQ(v_r(1), -max);
+	EXPECT_FLOAT_EQ(v_r(0), 0.0f);
 
 	v0 = Vector2f(0.5f, 0.5f);
 	v1 = Vector2f(0.5f, -0.5f);
 	v_r = constrainXY(v0, v1, max);
 	const float diff = Vector2f(v_r - (v0 + v1)).length();
-	EXPECT_EQ(diff, 0);
+	EXPECT_FLOAT_EQ(diff, 0.0f);
 
 	// v0 and v1 exceed max and are perpendicular
 	v0 = Vector2f(4.0f, 0.0f);
 	v1 = Vector2f(0.0f, -4.0f);
 	v_r = constrainXY(v0, v1, max);
-	EXPECT_EQ(v_r(0), v0(0));
+	EXPECT_FLOAT_EQ(v_r(0), v0(0));
 	EXPECT_GT(v_r(0), 0);
 	const float remaining = sqrtf(max * max - (v0(0) * v0(0)));
-	EXPECT_EQ(v_r(1), -remaining);
+	EXPECT_FLOAT_EQ(v_r(1), -remaining);
 }
 
 TEST(ControlMathTest, CrossSphereLine)
@@ -135,45 +135,45 @@ TEST(ControlMathTest, CrossSphereLine)
 	// on line, near, before previous waypoint
 	retval = cross_sphere_line(Vector3f(0.0f, 0.0f, -0.5f), 1.0f, prev, curr, res);
 	EXPECT_TRUE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.5f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 0.5f)));
 
 	// on line, near, before target waypoint
 	retval = cross_sphere_line(Vector3f(0.0f, 0.0f, 1.0f), 1.0f, prev, curr, res);
 	EXPECT_TRUE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 2.f)));
 
 	// on line, near, after target waypoint
 	retval = cross_sphere_line(Vector3f(0.0f, 0.0f, 2.5f), 1.0f, prev, curr, res);
 	EXPECT_TRUE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 2.f)));
 
 	// near, before previous waypoint
 	retval = cross_sphere_line(Vector3f(0.0f, 0.5f, -0.5f), 1.0f, prev, curr, res);
 	EXPECT_TRUE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.366025388f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 0.366025388f)));
 
 	// near, before target waypoint
 	retval = cross_sphere_line(Vector3f(0.0f, 0.5f, 1.0f), 1.0f, prev, curr, res);
 	EXPECT_TRUE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.866025448f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 1.866025448f)));
 
 	// near, after target waypoint
 	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.0f, 0.5f, 2.5f), 1.0f, prev, curr, res);
 	EXPECT_TRUE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 2.f)));
 
 	// far, before previous waypoint
 	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, -0.5f), 1.0f, prev, curr, res);
 	EXPECT_FALSE(retval);
-	EXPECT_EQ(res, Vector3f());
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f()));
 
 	// far, before target waypoint
 	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, 1.0f), 1.0f, prev, curr, res);
 	EXPECT_FALSE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 1.f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 1.f)));
 
 	// far, after target waypoint
 	retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, 2.5f), 1.0f, prev, curr, res);
 	EXPECT_FALSE(retval);
-	EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
+	EXPECT_TRUE(matrix::isEqual(res, Vector3f(0.f, 0.f, 2.f)));
 }