45 #include <px4_platform_common/px4_config.h> 49 #include <sys/types.h> 52 #include <px4_platform_common/log.h> 58 #include <mathlib/mathlib.h> 84 #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); } 94 float data[2] = {1.0f, 2.0f};
99 TEST_OP(
"matrix::Vector2f = matrix::Vector2f", v = v1);
100 TEST_OP(
"matrix::Vector2f + matrix::Vector2f", v + v1);
101 TEST_OP(
"matrix::Vector2f - matrix::Vector2f", v - v1);
102 TEST_OP(
"matrix::Vector2f += matrix::Vector2f", v += v1);
103 TEST_OP(
"matrix::Vector2f -= matrix::Vector2f", v -= v1);
104 TEST_OP(
"matrix::Vector2f * matrix::Vector2f", v * v1);
105 TEST_OP(
"matrix::Vector2f %% matrix::Vector2f", v1 % v2);
117 float data[3] = {1.0f, 2.0f, 3.0f};
122 TEST_OP(
"matrix::Vector3f = matrix::Vector3f", v = v1);
123 TEST_OP(
"matrix::Vector3f + matrix::Vector3f", v + v1);
124 TEST_OP(
"matrix::Vector3f - matrix::Vector3f", v - v1);
125 TEST_OP(
"matrix::Vector3f += matrix::Vector3f", v += v1);
126 TEST_OP(
"matrix::Vector3f -= matrix::Vector3f", v -= v1);
127 TEST_OP(
"matrix::Vector3f * float", v1 * 2.0
f);
128 TEST_OP(
"matrix::Vector3f / float", v1 / 2.0f);
129 TEST_OP(
"matrix::Vector3f *= float", v1 *= 2.0f);
130 TEST_OP(
"matrix::Vector3f /= float", v1 /= 2.0f);
131 TEST_OP(
"matrix::Vector3f * matrix::Vector3f", v * v1);
132 TEST_OP(
"matrix::Vector3f %% matrix::Vector3f", v1 % v2);
133 TEST_OP(
"matrix::Vector3f length", v1.length());
134 #pragma GCC diagnostic push 135 #pragma GCC diagnostic ignored "-Wunused-variable" 138 TEST_OP(
"matrix::Vector3f element read",
volatile float a = v1(0));
139 #pragma GCC diagnostic pop 140 TEST_OP(
"matrix::Vector3f element write", v1(0) = 1.0f);
153 TEST_OP(
"matrix::Matrix3f * matrix::Vector3f", m1 * v1);
154 TEST_OP(
"matrix::Matrix3f + matrix::Matrix3f", m1 + m2);
155 TEST_OP(
"matrix::Matrix3f * matrix::Matrix3f", m1 * m2);
167 float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
168 float data2[2][3] = {{2, 4, 6}, {8, 10, 12}};
169 float data3[2][3] = {{3, 6, 9}, {12, 15, 18}};
176 PX4_ERR(
"matrix::Matrix<float, 2, 3> + matrix::Matrix<float, 2, 3> failed!");
183 ut_assert(
"m1 + m2 == m3", m1 + m2 == m3);
186 PX4_ERR(
"matrix::Matrix<float, 2, 3> - matrix::Matrix<float, 2, 3> failed!");
193 ut_assert(
"m3 - m2 == m1", m3 - m2 == m1);
198 PX4_ERR(
"matrix::Matrix<float, 2, 3> += matrix::Matrix<float, 2, 3> failed!");
211 PX4_ERR(
"matrix::Matrix<float, 2, 3> -= matrix::Matrix<float, 2, 3> failed!");
218 ut_assert(
"m1 == m1_orig", m1 == m1_orig);
231 float tol = 0.00001f;
235 for (
float roll = -
M_PI_F; roll <=
M_PI_F; roll += diff) {
236 for (
float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
242 for (
int i = 0; i < 3; i++) {
243 for (
int j = 0; j < 3; j++) {
244 ut_assert(
"matrix::Quatf method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig(i, j) - R(i, j)) < tol);
267 for (
unsigned i = 0; i < 4; i++) {
268 ut_assert(
"matrix::Quatf method 'from_dcm()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
286 q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
288 for (
unsigned i = 0; i < 4; i++) {
289 ut_assert(
"matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
293 q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
295 for (
unsigned i = 0; i < 4; i++) {
296 ut_assert(
"matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
300 q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
302 for (
unsigned i = 0; i < 4; i++) {
303 ut_assert(
"matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
318 float tol = 0.00001f;
322 for (
float roll = -
M_PI_F; roll <=
M_PI_F; roll += diff) {
323 for (
float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
327 vector_r = R * vector;
330 for (
int i = 0; i < 3; i++) {
331 ut_assert(
"matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
344 for (
unsigned i = 0; i < 3; i++) {
345 ut_assert(
"matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
350 vector_true = {1.1566, 0.7792, 1.0273};
352 for (
unsigned i = 0; i < 3; i++) {
353 ut_assert(
"matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
358 vector_true = {0.5095, 1.4956, -0.7096};
360 for (
unsigned i = 0; i < 3; i++) {
361 ut_assert(
"matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
366 vector_true = { -1.3660, 0.3660, 1.0000};
368 for (
unsigned i = 0; i < 3; i++) {
369 ut_assert(
"matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
377 ut_assert(
"PX4_ISFINITE(0.0f)", PX4_ISFINITE(0.0
f) ==
true);
378 ut_assert(
"PX4_ISFINITE(-0.0f)", PX4_ISFINITE(-0.0
f) ==
true);
379 ut_assert(
"PX4_ISFINITE(1.0f)", PX4_ISFINITE(1.0
f) ==
true);
380 ut_assert(
"PX4_ISFINITE(-1.0f)", PX4_ISFINITE(-1.0
f) ==
true);
382 ut_assert(
"PX4_ISFINITE(NAN)", PX4_ISFINITE(NAN) ==
false);
383 ut_assert(
"PX4_ISFINITE(1/0)", PX4_ISFINITE(1.0
f / 0.0
f) ==
false);
384 ut_assert(
"PX4_ISFINITE(0/0)", PX4_ISFINITE(0.0
f / 0.0
f) ==
false);
385 ut_assert(
"PX4_ISFINITE(INFINITY)", PX4_ISFINITE(INFINITY) ==
false);
386 ut_assert(
"PX4_ISFINITE(NAN * INFINITY)", PX4_ISFINITE(NAN * INFINITY) ==
false);
387 ut_assert(
"PX4_ISFINITE(NAN * 1.0f)", PX4_ISFINITE(NAN * 1.0
f) ==
false);
388 ut_assert(
"PX4_ISFINITE(INFINITY * 2.0f)", PX4_ISFINITE(INFINITY * 2.0f) ==
false);
#define ut_declare_test_c(test_function, test_class)
bool testRotationMatrixQuaternion()
Quaternion from_dcm(const Matrix< Type, 3, 3 > &dcm)
XXX DEPRECATED, can use assignment or ctor.
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Base class to be used for unit tests.
High-resolution timer with callouts and timekeeping.
virtual bool run_tests()
Override to run your unit tests.
int _tests_failed
The number of unit tests which failed.
int test_mathlib(int argc, char *argv[])
void print(FILE *stream=stdout) const
#define ut_assert(message, test)
Used to assert a value within a unit test.
bool testMatrixNonsymmetric()
bool testQuaternionfrom_dcm()
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
#define ut_assert_true(test)
To assert specifically to true.
#define TEST_OP(_title, _op)
bool testQuaternionRotate()
#define ut_run_test(test)
Runs a single unit test.
bool testQuaternionfrom_euler()