fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 22:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

NuttX carry minimal c++ cmath (replacing Matrix stdlib_imports.hpp)

Browse Source
This commit is contained in:
Daniel Agar
2022-07-12 08:05:06 -04:00
parent fe22167512
commit a73efd9c4f
32 changed files with 297 additions and 323 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/test/MatrixAssignmentTest.cpp
+3 -1
View File
@@ -31,6 +31,8 @@
*
****************************************************************************/
#include <cmath>
#include <gtest/gtest.h>
#include <matrix/math.hpp>
@@ -65,7 +67,7 @@ TEST(MatrixAssignmentTest, Assignment)
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
EXPECT_TRUE(isnan(m_nan(i, j)));
EXPECT_TRUE(std::isnan(m_nan(i, j)));
}
}
src/lib/matrix/test/MatrixAttitudeTest.cpp
+4 -4
View File
@@ -379,14 +379,14 @@ TEST(MatrixAttitudeTest, Attitude)
Vector3f rot(1.f, 0.f, 0.f);
Quatf q_test;
q_test.rotate(rot);
Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f);
Quatf q_true(std::cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f);
EXPECT_EQ(q_test, q_true);
// rotate quaternion (zero rotation)
rot(0) = rot(1) = rot(2) = 0.0f;
q_test = Quatf();
q_test.rotate(rot);
q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f);
q_true = Quatf(std::cos(0.0f), sin(0.0f), 0.0f, 0.0f);
EXPECT_EQ(q_test, q_true);
// rotate quaternion (random non-commutating rotation)
@@ -397,7 +397,7 @@ TEST(MatrixAttitudeTest, Attitude)
EXPECT_EQ(q, q_true);
// get rotation axis from quaternion (nonzero rotation)
q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
q = Quatf(std::cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
rot = AxisAnglef(q);
EXPECT_FLOAT_EQ(rot(0), 0.0f);
EXPECT_FLOAT_EQ(rot(1), 1.0f);
@@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude)
EXPECT_EQ(q, q_true);
// from axis angle, with length of vector the rotation
float n = float(sqrt(4 * M_PI * M_PI / 3));
float n = float(std::sqrt(4 * M_PI * M_PI / 3));
q = AxisAnglef(n, n, n);
EXPECT_EQ(q, Quatf(-1, 0, 0, 0));
q = AxisAnglef(0, 0, 0);
src/lib/matrix/test/MatrixDualTest.cpp
+13 -11
View File
@@ -31,6 +31,8 @@
*
****************************************************************************/
#include <cmath>
#include <gtest/gtest.h>
#include <matrix/math.hpp>
#include <iostream>
@@ -162,7 +164,7 @@ TEST(MatrixDualTest, Dual)
{
// sqrt
EXPECT_FLOAT_EQ(sqrt(a).value, sqrt(a.value));
EXPECT_FLOAT_EQ(sqrt(a).value, std::sqrt(a.value));
EXPECT_FLOAT_EQ(sqrt(a).derivative(0), 1.f / sqrt(12.f));
}
@@ -202,7 +204,7 @@ TEST(MatrixDualTest, Dual)
{
// isnan
EXPECT_FALSE(IsNan(a));
Dual<float, 1> c(sqrt(-1.f), 0);
Dual<float, 1> c(std::sqrt(-1.f), 0);
EXPECT_TRUE(IsNan(c));
}
@@ -210,7 +212,7 @@ TEST(MatrixDualTest, Dual)
// isfinite/isinf
EXPECT_TRUE(IsFinite(a));
EXPECT_FALSE(IsInf(a));
Dual<float, 1> c(sqrt(-1.f), 0);
Dual<float, 1> c(std::sqrt(-1.f), 0);
EXPECT_FALSE(IsFinite(c));
EXPECT_FALSE(IsInf(c));
Dual<float, 1> d(INFINITY, 0);
@@ -221,25 +223,25 @@ TEST(MatrixDualTest, Dual)
{
// sin/cos/tan
EXPECT_FLOAT_EQ(sin(a).value, sin(a.value));
EXPECT_FLOAT_EQ(sin(a).derivative(0), cos(a.value)); // sin'(x) = cos(x)
EXPECT_FLOAT_EQ(sin(a).derivative(0), std::cos(a.value)); // sin'(x) = cos(x)
EXPECT_FLOAT_EQ(cos(a).value, cos(a.value));
EXPECT_FLOAT_EQ(cos(a).derivative(0), -sin(a.value)); // cos'(x) = -sin(x)
EXPECT_FLOAT_EQ(cos(a).derivative(0), -std::sin(a.value)); // cos'(x) = -sin(x)
EXPECT_FLOAT_EQ(tan(a).value, tan(a.value));
EXPECT_FLOAT_EQ(tan(a).derivative(0), 1.f + tan(a.value)*tan(a.value)); // tan'(x) = 1 + tan^2(x)
EXPECT_FLOAT_EQ(tan(a).derivative(0), 1.f + std::tan(a.value)*std::tan(a.value)); // tan'(x) = 1 + tan^2(x)
}
{
// asin/acos/atan
Dual<float, 1> c(0.3f, 0);
EXPECT_FLOAT_EQ(asin(c).value, asin(c.value));
EXPECT_FLOAT_EQ(asin(c).derivative(0), 1.f / sqrt(1.f - 0.3f * 0.3f)); // asin'(x) = 1/sqrt(1-x^2)
EXPECT_FLOAT_EQ(asin(c).value, std::asin(c.value));
EXPECT_FLOAT_EQ(asin(c).derivative(0), 1.f / std::sqrt(1.f - 0.3f * 0.3f)); // asin'(x) = 1/sqrt(1-x^2)
EXPECT_FLOAT_EQ(acos(c).value, acos(c.value));
EXPECT_FLOAT_EQ(acos(c).derivative(0), -1.f / sqrt(1.f - 0.3f * 0.3f)); // acos'(x) = -1/sqrt(1-x^2)
EXPECT_FLOAT_EQ(acos(c).value, std::acos(c.value));
EXPECT_FLOAT_EQ(acos(c).derivative(0), -1.f / std::sqrt(1.f - 0.3f * 0.3f)); // acos'(x) = -1/sqrt(1-x^2)
EXPECT_FLOAT_EQ(atan(c).value, atan(c.value));
EXPECT_FLOAT_EQ(atan(c).value, std::atan(c.value));
EXPECT_FLOAT_EQ(atan(c).derivative(0), 1.f / (1.f + 0.3f * 0.3f)); // tan'(x) = 1 + x^2
}
src/lib/matrix/test/MatrixHelperTest.cpp
+6 -6
View File
@@ -81,21 +81,21 @@ TEST(MatrixHelperTest, Helper)
// wrap pi
EXPECT_FLOAT_EQ(wrap_pi(0.), 0.);
EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - M_TWOPI));
EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + M_TWOPI));
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(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_FALSE(is_finite(wrap_pi(NAN)));
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(3.), (3.));
EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * M_TWOPI));
EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * M_TWOPI));
EXPECT_FALSE(is_finite(wrap_2pi(NAN)));
EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI)));
EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI)));
EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));
// Equality checks
EXPECT_TRUE(isEqualF(1., 1.));
src/lib/matrix/test/MatrixIntegralTest.cpp
+1 -1
View File
@@ -50,6 +50,6 @@ TEST(MatrixIntegralTest, Integral)
float tf = 2;
float h = 0.001f;
integrate_rk4(f, y, u, t0, tf, h, y);
float v = 1 + cos(tf) - cos(t0);
float v = 1 + std::cos(tf) - std::cos(t0);
EXPECT_EQ(y, (ones<float, 6, 1>()*v));
}
src/lib/matrix/test/test_data.py
+1 -1
View File
@@ -47,7 +47,7 @@ def quat_prod(q, r):
def dcm_to_euler(dcm):
return array([
arctan(dcm[2,1]/ dcm[2,2]),
arctan(-dcm[2,0]/ sqrt(1 - dcm[2,0]**2)),
arctan(-dcm[2,0]/ std::sqrt(1 - dcm[2,0]**2)),
arctan(dcm[1,0]/ dcm[0,0]),
])