Skip to content

Commit

Permalink
ControlMathTest: consistent short float literal notation
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaEtUgR @LorenzMeier
MaEtUgR authored and LorenzMeier committed Dec 26, 2019
1 parent ad148fd commit ee62d02
Showing 1 changed file with 28 additions and 28 deletions.
56 changes: 28 additions & 28 deletions src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand All @@ -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);
Expand Down Expand Up @@ -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));
}

0 comments on commit ee62d02

Please sign in to comment.