From 34c852255e16bb62d2caf0c3be1f2fea660036ba Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Mon, 31 Oct 2022 08:51:23 -0700 Subject: [PATCH] Changed M_PI to M_PI_F in the matrix library since M_PI is non-standard. (#20458) * Changed M_PI to M_PI_F in the matrix library since M_PI is non-standard. * Added a new M_PI_PRECISE constant definition to px4_platform_common/defines.h to be used in places when M_PI is desired but shouldn't be used because it is not C standard. * Added the px4_platform_common/defines.h include to the matrix library math.hpp header to pull in some non-standard M_PI constants and updated the test files to use those constants. * Fixed PI constants in matrix helper test to prevent test failure --- .../common/include/px4_platform_common/defines.h | 4 ++++ src/lib/matrix/matrix/Euler.hpp | 4 ++-- src/lib/matrix/matrix/helper_functions.hpp | 6 +++--- src/lib/matrix/matrix/math.hpp | 1 + src/lib/matrix/test/MatrixAttitudeTest.cpp | 6 +++--- src/lib/matrix/test/MatrixHelperTest.cpp | 16 ++++++++-------- src/lib/matrix/test/MatrixUnwrapTest.cpp | 4 ++-- 7 files changed, 23 insertions(+), 18 deletions(-) diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index e0a740943b..913481bfee 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -136,5 +136,9 @@ __END_DECLS #define M_LOG2_E_F 0.69314718f #define M_INVLN2_F 1.44269504f // 1 / log(2) +/* The M_PI, as stated above, is not C standard. If you need it and + * it isn't in your math.h file then you can use this instead. */ +#define M_PI_PRECISE 3.141592653589793238462643383279502884 + #define M_DEG_TO_RAD 0.017453292519943295 #define M_RAD_TO_DEG 57.295779513082323 diff --git a/src/lib/matrix/matrix/Euler.hpp b/src/lib/matrix/matrix/Euler.hpp index cb977e690e..d885b85215 100644 --- a/src/lib/matrix/matrix/Euler.hpp +++ b/src/lib/matrix/matrix/Euler.hpp @@ -93,11 +93,11 @@ public: { theta() = std::asin(-dcm(2, 0)); - if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) { + if ((std::fabs(theta() - Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) { phi() = 0; psi() = std::atan2(dcm(1, 2), dcm(0, 2)); - } else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) { + } else if ((std::fabs(theta() + Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) { phi() = 0; psi() = std::atan2(-dcm(1, 2), -dcm(0, 2)); diff --git a/src/lib/matrix/matrix/helper_functions.hpp b/src/lib/matrix/matrix/helper_functions.hpp index 8af22ac53a..6a9e871ef0 100644 --- a/src/lib/matrix/matrix/helper_functions.hpp +++ b/src/lib/matrix/matrix/helper_functions.hpp @@ -95,7 +95,7 @@ Integer wrap(Integer x, Integer low, Integer high) template Type wrap_pi(Type x) { - return wrap(x, Type(-M_PI), Type(M_PI)); + return wrap(x, Type(-M_PI_PRECISE), Type(M_PI_PRECISE)); } /** @@ -104,7 +104,7 @@ Type wrap_pi(Type x) template Type wrap_2pi(Type x) { - return wrap(x, Type(0), Type((2 * M_PI))); + return wrap(x, Type(0), Type((2 * M_PI_PRECISE))); } /** @@ -132,7 +132,7 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high template Type unwrap_pi(const Type last_angle, const Type new_angle) { - return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI)); + return unwrap(last_angle, new_angle, Type(-M_PI_PRECISE), Type(M_PI_PRECISE)); } /** diff --git a/src/lib/matrix/matrix/math.hpp b/src/lib/matrix/matrix/math.hpp index 24a43723bf..6601a5bc1c 100644 --- a/src/lib/matrix/matrix/math.hpp +++ b/src/lib/matrix/matrix/math.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include "helper_functions.hpp" diff --git a/src/lib/matrix/test/MatrixAttitudeTest.cpp b/src/lib/matrix/test/MatrixAttitudeTest.cpp index 40e6dea4d2..3ea81ebf3a 100644 --- a/src/lib/matrix/test/MatrixAttitudeTest.cpp +++ b/src/lib/matrix/test/MatrixAttitudeTest.cpp @@ -200,8 +200,8 @@ TEST(MatrixAttitudeTest, Attitude) } // constants - double deg2rad = M_PI / 180.0; - double rad2deg = 180.0 / M_PI; + double deg2rad = M_PI_PRECISE / 180.0; + double rad2deg = 180.0 / M_PI_PRECISE; // euler dcm round trip check for (double roll = -90; roll <= 90; roll += 90) { @@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude) EXPECT_EQ(q, q_true); // from axis angle, with length of vector the rotation - float n = float(std::sqrt(4 * M_PI * M_PI / 3)); + float n = float(std::sqrt(4 * M_PI_F * M_PI_F / 3)); q = AxisAnglef(n, n, n); EXPECT_EQ(q, Quatf(-1, 0, 0, 0)); q = AxisAnglef(0, 0, 0); diff --git a/src/lib/matrix/test/MatrixHelperTest.cpp b/src/lib/matrix/test/MatrixHelperTest.cpp index 6133f497df..19e74bdd19 100644 --- a/src/lib/matrix/test/MatrixHelperTest.cpp +++ b/src/lib/matrix/test/MatrixHelperTest.cpp @@ -81,20 +81,20 @@ TEST(MatrixHelperTest, Helper) // wrap pi EXPECT_FLOAT_EQ(wrap_pi(0.), 0.); - EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI))); - EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI))); + EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI_PRECISE))); + EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI_PRECISE))); EXPECT_FLOAT_EQ(wrap_pi(3.), 3.); - EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI)); - EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI)); - EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI)); + EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE)); + EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE)); + EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI_PRECISE)); EXPECT_FALSE(std::isfinite(wrap_pi(NAN))); // wrap 2pi EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.); - EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI)); + EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI_PRECISE)); EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.)); - EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI))); - EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI))); + EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI_PRECISE))); + EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE))); EXPECT_FALSE(std::isfinite(wrap_2pi(NAN))); // Equality checks diff --git a/src/lib/matrix/test/MatrixUnwrapTest.cpp b/src/lib/matrix/test/MatrixUnwrapTest.cpp index f774174ccf..138074b072 100644 --- a/src/lib/matrix/test/MatrixUnwrapTest.cpp +++ b/src/lib/matrix/test/MatrixUnwrapTest.cpp @@ -5,7 +5,7 @@ using namespace matrix; TEST(MatrixUnwrapTest, UnwrapFloats) { - const float M_TWO_PI_F = float(M_PI * 2); + const float M_TWO_PI_F = float(M_PI_F * 2); float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25}; @@ -34,7 +34,7 @@ TEST(MatrixUnwrapTest, UnwrapFloats) TEST(MatrixUnwrapTest, UnwrapDoubles) { - const double M_TWO_PI = M_PI * 2; + const double M_TWO_PI = M_PI_PRECISE * 2; double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25}; double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};