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

Testing cleanup from Daniel Agar

Browse Source
This commit is contained in:
Lorenz Meier
2016-07-29 13:27:58 +02:00
parent 99aa5f49fc
commit 6ab9dc0acf
56 changed files with 1930 additions and 3416 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/tests/test_mathlib.cpp
+240 -164
View File
@@ -31,11 +31,18 @@
*
****************************************************************************/
/**
* @file test_mathlib.cpp
*
* Mathlib test
*/
#include <unit_test/unit_test.h>
#include <errno.h>
#include <fcntl.h>
#include <float.h>
#include <math.h>
#include <px4_config.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/types.h>
#include <unistd.h>
#include <px4_log.h>
#include <stdio.h>
@@ -47,17 +54,33 @@
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
class MathlibTest : public UnitTest
{
public:
virtual bool run_tests(void);
private:
bool testVector2();
bool testVector3();
bool testVector4();
bool testVector10();
bool testMatrix3x3();
bool testMatrix10x10();
bool testMatrixNonsymmetric();
bool testRotationMatrixQuaternion();
bool testQuaternionfrom_dcm();
bool testQuaternionfrom_euler();
bool testQuaternionRotate();
};
#include "tests.h"
#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;
int test_mathlib(int argc, char *argv[])
bool MathlibTest::testVector2(void)
{
int rc = 0;
PX4_INFO("testing mathlib");
{
Vector<2> v;
Vector<2> v1(1.0f, 2.0f);
@@ -75,6 +98,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<2> * Vector<2>", v * v1);
TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
}
return true;
}
bool MathlibTest::testVector3(void)
{
{
Vector<3> v;
@@ -108,7 +136,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
}
return true;
}
bool MathlibTest::testVector4(void)
{
{
Vector<4> v;
Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
@@ -125,7 +157,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> * Vector<4>", v * v1);
}
return true;
}
bool MathlibTest::testVector10(void)
{
{
Vector<10> v1;
v1.zero();
@@ -134,7 +170,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
}
return true;
}
bool MathlibTest::testMatrix3x3(void)
{
{
Matrix<3, 3> m1;
m1.identity();
@@ -145,7 +185,11 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
}
return true;
}
bool MathlibTest::testMatrix10x10(void)
{
{
Matrix<10, 10> m1;
m1.identity();
@@ -157,9 +201,14 @@ int test_mathlib(int argc, char *argv[])
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
}
return true;
}
bool MathlibTest::testMatrixNonsymmetric(void)
{
int rc = true;
{
PX4_INFO("Nonsymmetric matrix operations test");
//PX4_INFO("Nonsymmetric matrix operations test");
// test nonsymmetric +, -, +=, -=
float data1[2][3] = {{1, 2, 3}, {4, 5, 6}};
@@ -175,17 +224,21 @@ int test_mathlib(int argc, char *argv[])
(m1 + m2).print();
printf("!=\n");
m3.print();
rc = 1;
rc = false;
}
ut_assert("m1 + m2 == m3", m1 + m2 == m3);
if (m3 - m2 != m1) {
PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
(m3 - m2).print();
printf("!=\n");
m1.print();
rc = 1;
rc = false;
}
ut_assert("m3 - m2 == m1", m3 - m2 == m1);
m1 += m2;
if (m1 != m3) {
@@ -193,9 +246,11 @@ int test_mathlib(int argc, char *argv[])
m1.print();
printf("!=\n");
m3.print();
rc = 1;
rc = false;
}
ut_assert("m1 == m3", m1 == m3);
m1 -= m2;
Matrix<2, 3> m1_orig(data1);
@@ -204,160 +259,181 @@ int test_mathlib(int argc, char *argv[])
m1.print();
printf("!=\n");
m1_orig.print();
rc = 1;
rc = false;
}
}
ut_assert("m1 == m1_orig", m1 == m1_orig);
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
float diff = 0.1f;
float tol = 0.00001f;
PX4_INFO("Quaternion transformation methods 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_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) {
PX4_WARN("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!");
rc = 1;
}
}
}
}
}
}
// test against some known values
tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
R_orig.identity();
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_dcm()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
if (fabsf(q.data[i] - q_true.data[i]) > tol) {
PX4_WARN("Quaternion method 'from_euler()' outside tolerance!");
rc = 1;
}
}
}
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
float diff = 0.1f;
float tol = 0.00001f;
PX4_INFO("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.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
PX4_WARN("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
return rc;
}
bool MathlibTest::testRotationMatrixQuaternion(void)
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("Quaternion transformation methods 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_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol);
}
}
}
}
}
return true;
}
bool MathlibTest::testQuaternionfrom_dcm(void)
{
// test against some known values
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
R_orig.identity();
math::Quaternion q;
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
return true;
}
bool MathlibTest::testQuaternionfrom_euler(void)
{
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
R_orig.identity();
math::Quaternion q;
q.from_dcm(R_orig);
q_true.from_euler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
}
return true;
}
bool MathlibTest::testQuaternionRotate(void)
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("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.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
return true;
}
bool MathlibTest::run_tests(void)
{
ut_run_test(testVector2);
ut_run_test(testVector3);
ut_run_test(testVector4);
ut_run_test(testVector10);
ut_run_test(testMatrix3x3);
ut_run_test(testMatrix10x10);
ut_run_test(testMatrixNonsymmetric);
ut_run_test(testRotationMatrixQuaternion);
ut_run_test(testQuaternionfrom_dcm);
ut_run_test(testQuaternionfrom_euler);
ut_run_test(testQuaternionRotate);
return (_tests_failed == 0);
}
ut_declare_test_c(test_mathlib, MathlibTest)