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

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

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));
 

src/lib/matrix/matrix/helper_functions.hpp

@@ -95,7 +95,7 @@ Integer wrap(Integer x, Integer low, Integer high)
 template<typename Type>
 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<typename Type>
 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<typename Type>
 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));
 }
 
 /**

src/lib/matrix/matrix/math.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <assert.h>
+#include <px4_platform_common/defines.h>
 
 #include "helper_functions.hpp"
 

src/lib/matrix/test/MatrixAttitudeTest.cpp

@@ -200,8 +200,8 @@ TEST(MatrixAttitudeTest, Attitude)
 	}
 
 	// constants
-	double deg2rad = M_PI / 180.0;
+	double deg2rad = M_PI_PRECISE / 180.0;
-	double rad2deg = 180.0 / M_PI;
+	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);

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_PRECISE)));
-	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(3.), 3.);
-	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE));
-	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE));
-	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI));
+	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(200.), (200. - 31. * (2 * M_PI_PRECISE)));
-	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE)));
 	EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));
 
 	// Equality checks

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};