fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 20:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fix float accuracy in ControlMathTest

Browse Source 
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.
This commit is contained in:
Lorenz Meier
2019-12-23 21:33:53 +01:00
parent e6b18fe2da
commit 9ac68abd5c
1 changed files with 28 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
+28 -28
View File
@@ -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)));
}