forked from Archive/PX4-Autopilot
ControlMathTest: consistent short float literal notation
This commit is contained in:
parent
ad148fdfad
commit
ee62d0296f
|
@ -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));
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue