From 98733882f508b69d0e1af3c28c52148d3f07d154 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 19 Jun 2024 07:52:05 +1000 Subject: [PATCH] AP_Math: added comments and a test for euler ordering our main euler functions did not have a comment on the ordering convention --- libraries/AP_Math/matrix3.h | 14 ++-- libraries/AP_Math/quaternion.h | 8 +-- libraries/AP_Math/tests/test_rotations.cpp | 78 ++++++++++++++++++++++ 3 files changed, 91 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index ab861e222e..b346198d53 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -235,13 +235,17 @@ public: return a.is_nan() || b.is_nan() || c.is_nan(); } - // create a rotation matrix from Euler angles + /* + create a rotation matrix from Euler angles in 321 euler orderin + */ void from_euler(T roll, T pitch, T yaw); - // create eulers from a rotation matrix. - // roll is from -Pi to Pi - // pitch is from -Pi/2 to Pi/2 - // yaw is from -Pi to Pi + /* create eulers from a rotation matrix. + roll is from -Pi to Pi + pitch is from -Pi/2 to Pi/2 + yaw is from -Pi to Pi + euler order is 321 + */ void to_euler(T *roll, T *pitch, T *yaw) const; // create matrix from rotation enum diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index 1c47540c67..00f29d0898 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -81,11 +81,11 @@ public: // convert a vector from earth to body frame void earth_to_body(Vector3 &v) const; - // create a quaternion from Euler angles + // create a quaternion from Euler angles using 321 euler ordering void from_euler(T roll, T pitch, T yaw); void from_euler(const Vector3 &v); - // create a quaternion from Euler angles applied in yaw, roll, pitch order + // create a quaternion from Euler angles applied in yaw, roll, pitch order (312) // instead of the normal yaw, pitch, roll order void from_vector312(T roll, T pitch, T yaw); @@ -129,7 +129,7 @@ public: // get euler yaw angle in radians T get_euler_yaw() const; - // create eulers (in radians) from a quaternion + // create eulers (in radians) from a quaternion, using 321 ordering void to_euler(float &roll, float &pitch, float &yaw) const; void to_euler(Vector3f &rpy) const { to_euler(rpy.x, rpy.y, rpy.z); @@ -139,7 +139,7 @@ public: to_euler(rpy.x, rpy.y, rpy.z); } - // create eulers from a quaternion + // create eulers from a quaternion with 312 ordering Vector3 to_vector312(void) const; T length_squared(void) const; diff --git a/libraries/AP_Math/tests/test_rotations.cpp b/libraries/AP_Math/tests/test_rotations.cpp index 15fdfa6238..d9de8f82e5 100644 --- a/libraries/AP_Math/tests/test_rotations.cpp +++ b/libraries/AP_Math/tests/test_rotations.cpp @@ -189,5 +189,83 @@ TEST(RotationsTest, TestFailedGetLinux) } }*/ +/* + rotate a matrix using a give order, specified as a string + for example "321" + */ +static void rotate_ordered(Matrix3f &m, const char *order, + const float roll_deg, + const float pitch_deg, + const float yaw_deg) +{ + while (*order) { + Matrix3f m2; + switch (*order) { + case '1': + m2.from_euler(radians(roll_deg), 0, 0); + break; + case '2': + m2.from_euler(0, radians(pitch_deg), 0); + break; + case '3': + m2.from_euler(0, 0, radians(yaw_deg)); + break; + } + m *= m2; + order++; + } +} + +/* + test the two euler orders we use in ArduPilot + */ +TEST(RotationsTest, TestEulerOrder) +{ + const float roll_deg = 20; + const float pitch_deg = 31; + const float yaw_deg = 72; + float r, p, y; + Vector3f v; + + Matrix3f m; + + // apply in 321 ordering + m.identity(); + rotate_ordered(m, "321", roll_deg, pitch_deg, yaw_deg); + + // get using to_euler + m.to_euler(&r, &p, &y); + + EXPECT_FLOAT_EQ(degrees(r), roll_deg); + EXPECT_FLOAT_EQ(degrees(p), pitch_deg); + EXPECT_FLOAT_EQ(degrees(y), yaw_deg); + + // get using to_euler312, should not match + v = m.to_euler312(); + + EXPECT_GE(fabsf(degrees(v.x)-roll_deg), 1); + EXPECT_GE(fabsf(degrees(v.y)-pitch_deg), 1); + EXPECT_GE(fabsf(degrees(v.z)-yaw_deg), 1); + + // apply in 312 ordering + m.identity(); + rotate_ordered(m, "312", roll_deg, pitch_deg, yaw_deg); + + // get using to_euler312 + v = m.to_euler312(); + + EXPECT_FLOAT_EQ(degrees(v.x), roll_deg); + EXPECT_FLOAT_EQ(degrees(v.y), pitch_deg); + EXPECT_FLOAT_EQ(degrees(v.z), yaw_deg); + + // get using to_euler, should not match + m.to_euler(&r, &p, &y); + + EXPECT_GE(fabsf(degrees(r)-roll_deg), 1); + EXPECT_GE(fabsf(degrees(p)-pitch_deg), 1); + EXPECT_GE(fabsf(degrees(y)-yaw_deg), 1); +} + + AP_GTEST_PANIC() AP_GTEST_MAIN()