diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1c5d266a75..7e31442525 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -3,8 +3,8 @@ # MODULE_COMMAND = tests -MODULE_STACKSIZE = 12000 -MAXOPTIMIZATION = -Os +MODULE_STACKSIZE = 60000 +MAXOPTIMIZATION = -O0 SRCS = test_adc.c \ test_bson.c \ @@ -37,7 +37,7 @@ SRCS = test_adc.c \ ifeq ($(PX4_TARGET_OS), nuttx) SRCS += test_time.c -EXTRACXXFLAGS = -Wframe-larger-than=2500 +EXTRACXXFLAGS = -Wframe-larger-than=6000 else EXTRACXXFLAGS = endif diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 6be961e644..1c7da802c9 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -56,7 +57,7 @@ namespace Eigen typedef Matrix Vector10f; } -static constexpr size_t OPERATOR_ITERATIONS = 60000; +static constexpr size_t OPERATOR_ITERATIONS = 30000; #define TEST_OP(_title, _op) \ { \ @@ -64,7 +65,7 @@ static constexpr size_t OPERATOR_ITERATIONS = 60000; for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ _op; \ } \ - printf(_title ": %.6fus\n", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ + printf("-O0 " _title ": %.6fus\n", static_cast(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ } #define VERIFY_OP(_title, _op, __OP_TEST__) \ @@ -105,13 +106,37 @@ void printEigen(const Eigen::MatrixBase &b) * @brief * Construct new Eigen::Quaternion from euler angles **/ -template -Eigen::Quaternion quatFromEuler(const T roll, const T pitch, const T yaw) +// template +// Eigen::Quaternion quatFromEuler(const T roll, const T pitch, const T yaw) +// { +// Eigen::AngleAxis rollAngle(roll, Eigen::Matrix::UnitZ()); +// Eigen::AngleAxis yawAngle(yaw, Eigen::Matrix::UnitY()); +// Eigen::AngleAxis pitchAngle(pitch, Eigen::Matrix::UnitX()); +// return yawAngle * pitchAngle * rollAngle; +// } + +Eigen::Quaternionf quatFromEuler(const float roll, const float pitch, const float yaw); + +Eigen::Quaternionf quatFromEuler(const float roll, const float pitch, const float yaw) { - Eigen::AngleAxis rollAngle(roll, Eigen::Matrix::UnitZ()); - Eigen::AngleAxis yawAngle(yaw, Eigen::Matrix::UnitY()); - Eigen::AngleAxis pitchAngle(pitch, Eigen::Matrix::UnitX()); - return rollAngle * yawAngle * pitchAngle; + Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitZ()); + Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitX()); + return yawAngle * pitchAngle * rollAngle; +} + +/** +* @brief +* Construct new Eigen::Matrix3f from euler angles +**/ +template +Eigen::Matrix3f matrixFromEuler(const T roll, const T pitch, const T yaw) +{ + Eigen::AngleAxis rollAngle(roll, Eigen::Vector3f::UnitZ()); + Eigen::AngleAxis yawAngle(yaw, Eigen::Vector3f::UnitY()); + Eigen::AngleAxis pitchAngle(pitch, Eigen::Vector3f::UnitX()); + Eigen::Quaternionf q = yawAngle * pitchAngle * rollAngle; + return q.toRotationMatrix(); } @@ -277,28 +302,102 @@ int test_eigen(int argc, char *argv[]) } - /* QUATERNION TESTS CURRENTLY FAILING + warnx("pre-quat"); + + usleep(500000); + + /* QUATERNION TESTS */ { // test conversion rotation matrix to quaternion and back - Eigen::Matrix3f R_orig; + // against existing PX4 mathlib + math::Matrix<3, 3> R_orig; Eigen::Matrix3f R; Eigen::Quaternionf q; float diff = 0.1f; - float tol = 0.00001f; + float tol; warnx("Quaternion transformation methods test."); + warnx("testing known values.."); + + // test against some known values + tol = 0.001f; + Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion 1.0f, 0.0f, 0.0f, 0.0f error: w: %8.4f", q.w()); + rc = 1; + } + + usleep(510000); + warnx("post-first"); + + q_true = quatFromEuler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + usleep(510000); + warnx("post-2"); + + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion 0.9833f, 0.1436f, 0.1060f, 0.0343f error: w: %8.6f, %8.6f", q.w(), q_true.w()); + rc = 1; + } + + usleep(510000); + warnx("post-3"); + + Eigen::Quaternionf q_true2 = quatFromEuler(-1.5f, -0.2f, 0.5f); + Eigen::Quaternionf q2 = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + usleep(510000); + warnx("post-4"); + + if (!q2.isApprox(q_true2, tol)) { + warnx("Quaternion 0.9833f, 0.1436f, 0.1060f, 0.0343f error: w: %8.6f, %8.6f", q2.w(), q_true2.w()); + rc = 1; + } + + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + usleep(510000); + warnx("post-5"); + + q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + + usleep(510000); + warnx("post-6"); + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + warnx("%8.4f, %8.4f, %8.4f, %8.4f, w: %8.4f", + q.vec()(1), q.vec()(2), q.vec()(3), q.vec()(4), q.w()); + rc = 1; + } + } + + usleep(510000); + warnx("post-7"); + + warnx("testing transformation range (this will take a while)"); + tol = 0.00001f; + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { - R_orig.eulerAngles(roll, pitch, yaw); - q = R_orig; //from_dcm + Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX()); + Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ()); + + R_orig.from_euler(roll, pitch, yaw); + + q = yawAngle * pitchAngle * rollAngle; R = q.toRotationMatrix(); for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { if (fabsf(R_orig(i, j) - R(i, j)) > tol) { - warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); + warnx("Quaternion constructor or 'toRotationMatrix' outside tolerance!\n %d, %d: %8.4f vs. %8.4f", i, j, R_orig(i, j), R(i, j)); rc = 1; } } @@ -306,50 +405,6 @@ int test_eigen(int argc, char *argv[]) } } } - - // test against some known values - tol = 0.0001f; - Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; - R_orig.setIdentity(); - q = R_orig; - - for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'from_dcm()' outside tolerance!"); - rc = 1; - } - } - - q_true = quatFromEuler(0.3f, 0.2f, 0.1f); - q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; - - for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'eulerAngles()' outside tolerance!"); - rc = 1; - } - } - - q_true = quatFromEuler(-1.5f, -0.2f, 0.5f); - q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; - - for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'eulerAngles()' outside tolerance!"); - rc = 1; - } - } - - q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); - q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; - - for (size_t i = 0; i < 4; i++) { - if (!q.isApprox(q_true, tol)) { - warnx("Quaternion method 'eulerAngles()' outside tolerance!"); - rc = 1; - } - } - } { @@ -360,29 +415,11 @@ int test_eigen(int argc, char *argv[]) Eigen::Quaternionf q; Eigen::Matrix3f R; float diff = 0.1f; - float tol = 0.00001f; + float tol; warnx("Quaternion vector rotation method test."); - for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { - for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { - for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { - R.eulerAngles(roll, pitch, yaw); - q = quatFromEuler(roll, pitch, yaw); - vector_r = R * vector; - vector_q = q._transformVector(vector); - - for (int i = 0; i < 3; i++) { - if (fabsf(vector_r(i) - vector_q(i)) > tol) { - warnx("Quaternion method 'rotate' outside tolerance"); - rc = 1; - } - } - } - } - } - - // test some values calculated with matlab + // test some values calculated with matlab tol = 0.0001f; q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f); vector_q = q._transformVector(vector); @@ -391,6 +428,9 @@ int test_eigen(int argc, char *argv[]) for (size_t i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); + warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f", + (double)vector_q(1), (double)vector_q(2), (double)vector_q(3), + (double)vector_true(1), (double)vector_true(2), (double)vector_true(3)); rc = 1; } } @@ -402,6 +442,9 @@ int test_eigen(int argc, char *argv[]) for (size_t i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); + warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f", + (double)vector_q(1), (double)vector_q(2), (double)vector_q(3), + (double)vector_true(1), (double)vector_true(2), (double)vector_true(3)); rc = 1; } } @@ -413,6 +456,9 @@ int test_eigen(int argc, char *argv[]) for (size_t i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); + warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f", + (double)vector_q(1), (double)vector_q(2), (double)vector_q(3), + (double)vector_true(1), (double)vector_true(2), (double)vector_true(3)); rc = 1; } } @@ -424,10 +470,36 @@ int test_eigen(int argc, char *argv[]) for (size_t i = 0; i < 3; i++) { if (fabsf(vector_true(i) - vector_q(i)) > tol) { warnx("Quaternion method 'rotate' outside tolerance"); + warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f", + (double)vector_q(1), (double)vector_q(2), (double)vector_q(3), + (double)vector_true(1), (double)vector_true(2), (double)vector_true(3)); rc = 1; } } + + warnx("testing transformation range (this will take a while)"); + tol = 0.00001f; + + Eigen::Vector3f vectorR = {1.0f, 1.0f, 1.0f}; + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R = matrixFromEuler(roll, pitch, yaw); + q = quatFromEuler(roll, pitch, yaw); + vector_r = R * vectorR; + vector_q = q._transformVector(vectorR); + + for (int i = 0; i < 3; i++) { + if (fabsf(vector_r(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + } + } } - */ + return rc; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 71c2ae5b93..dcd03e1be9 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -49,7 +49,7 @@ #include "tests.h" -#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); } +#define TEST_OP(_title, _op) { unsigned int n = 30000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); PX4_INFO(_title ": %.6fus", (double)(t1 - t0) / n); } using namespace math;