dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ControlMathTest: consistent short float literal notation

This commit is contained in:
Matthias Grob 2019-12-26 11:34:20 +01:00 committed by Lorenz Meier
parent ad148fdfad
commit ee62d0296f
1 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
View File

@ -43,38 +43,38 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
{
/* expected: zero roll, zero pitch, zero yaw, full thr mag
* reason: thrust pointing full upward */
Vector3f thr{0.0f, 0.0f, -1.0f};
float yaw = 0.0f;
Vector3f thr{0.f, 0.f, -1.f};
float yaw = 0.f;
vehicle_attitude_setpoint_s att{};
thrustToAttitude(thr, yaw, att);
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.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
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_FLOAT_EQ(att.roll_body, 0.0f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.0f);
EXPECT_FLOAT_EQ(att.roll_body, 0.f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
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);
thr = Vector3f(0.f, 0.f, 1.f);
thrustToAttitude(thr, yaw, att);
EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
EXPECT_FLOAT_EQ(att.pitch_body, 0.0f);
EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f);
}
TEST(ControlMathTest, ConstrainXYPriorities)
{
const float max = 5.0f;
const float max = 5.f;
// v0 already at max
Vector2f v0(max, 0);
Vector2f v1(v0(1), -v0(0));
@ -87,17 +87,17 @@ TEST(ControlMathTest, ConstrainXYPriorities)
v0.zero();
v_r = constrainXY(v0, v1, max);
EXPECT_FLOAT_EQ(v_r(1), -max);
EXPECT_FLOAT_EQ(v_r(0), 0.0f);
EXPECT_FLOAT_EQ(v_r(0), 0.f);
v0 = Vector2f(0.5f, 0.5f);
v1 = Vector2f(0.5f, -0.5f);
v0 = Vector2f(.5f, .5f);
v1 = Vector2f(.5f, -.5f);
v_r = constrainXY(v0, v1, max);
const float diff = Vector2f(v_r - (v0 + v1)).length();
EXPECT_FLOAT_EQ(diff, 0.0f);
EXPECT_FLOAT_EQ(diff, 0.f);
// v0 and v1 exceed max and are perpendicular
v0 = Vector2f(4.0f, 0.0f);
v1 = Vector2f(0.0f, -4.0f);
v0 = Vector2f(4.f, 0.f);
v1 = Vector2f(0.f, -4.f);
v_r = constrainXY(v0, v1, max);
EXPECT_FLOAT_EQ(v_r(0), v0(0));
EXPECT_GT(v_r(0), 0);
@ -127,53 +127,53 @@ TEST(ControlMathTest, CrossSphereLine)
*
*
* On trajectory --+----o----1----+--------2/3---+-- */
const Vector3f prev = Vector3f(0.0f, 0.0f, 0.0f);
const Vector3f curr = Vector3f(0.0f, 0.0f, 2.0f);
const Vector3f prev = Vector3f(0.f, 0.f, 0.f);
const Vector3f curr = Vector3f(0.f, 0.f, 2.f);
Vector3f res;
bool retval = false;
// on line, near, before previous waypoint
retval = cross_sphere_line(Vector3f(0.0f, 0.0f, -0.5f), 1.0f, prev, curr, res);
retval = cross_sphere_line(Vector3f(0.f, 0.f, -.5f), 1.f, prev, curr, res);
EXPECT_TRUE(retval);
EXPECT_EQ(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);
retval = cross_sphere_line(Vector3f(0.f, 0.f, 1.f), 1.f, prev, curr, res);
EXPECT_TRUE(retval);
EXPECT_EQ(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);
retval = cross_sphere_line(Vector3f(0.f, 0.f, 2.5f), 1.f, prev, curr, res);
EXPECT_TRUE(retval);
EXPECT_EQ(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);
retval = cross_sphere_line(Vector3f(0.f, .5f, -.5f), 1.f, prev, curr, res);
EXPECT_TRUE(retval);
EXPECT_EQ(res, Vector3f(0.f, 0.f, 0.366025388f));
EXPECT_EQ(res, Vector3f(0.f, 0.f, .366025388f));
// near, before target waypoint
retval = cross_sphere_line(Vector3f(0.0f, 0.5f, 1.0f), 1.0f, prev, curr, res);
retval = cross_sphere_line(Vector3f(0.f, .5f, 1.f), 1.f, prev, curr, res);
EXPECT_TRUE(retval);
EXPECT_EQ(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);
retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, .5f, 2.5f), 1.f, prev, curr, res);
EXPECT_TRUE(retval);
EXPECT_EQ(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);
retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, -.5f), 1.f, prev, curr, res);
EXPECT_FALSE(retval);
EXPECT_EQ(res, Vector3f());
// far, before target waypoint
retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.0f, 2.0f, 1.0f), 1.0f, prev, curr, res);
retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 1.f), 1.f, prev, curr, res);
EXPECT_FALSE(retval);
EXPECT_EQ(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);
retval = ControlMath::cross_sphere_line(matrix::Vector3f(0.f, 2.f, 2.5f), 1.f, prev, curr, res);
EXPECT_FALSE(retval);
EXPECT_EQ(res, Vector3f(0.f, 0.f, 2.f));
}