fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Removed all uses of C library from tests

Browse Source
This commit is contained in:
Pavel Kirienko 2017-03-17 13:38:48 +03:00 committed by James Goppert
parent 9ebf5f89db
commit e09cf12e2e
17 changed files with 87 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
test/attitude.cpp
View File

@ -1,20 +1,7 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
using matrix::AxisAnglef;
using matrix::Dcm;
using matrix::Dcmf;
using matrix::Euler;
using matrix::Eulerf;
using matrix::eye;
using matrix::isEqualF;
using matrix::Matrix;
using matrix::Quaternion;
using matrix::Quatf;
using matrix::SquareMatrix;
using matrix::Vector3f;
using matrix::Vector;
using matrix::zeros;
using namespace matrix;
int main()
{
@ -50,10 +37,10 @@ int main()
// quaternion ctor
Quatf q0(1, 2, 3, 4);
Quatf q(q0);
TEST(fabsf(q(0) - 1) < eps);
TEST(fabsf(q(1) - 2) < eps);
TEST(fabsf(q(2) - 3) < eps);
TEST(fabsf(q(3) - 4) < eps);
TEST(fabs(q(0) - 1) < eps);
TEST(fabs(q(1) - 2) < eps);
TEST(fabs(q(2) - 3) < eps);
TEST(fabs(q(3) - 4) < eps);
// quat normalization
q.normalize();
@ -107,7 +94,7 @@ int main()
for (auto & row : A._data) {
Vector3f rvec(row);
err += fabsf(1.0f - rvec.length());
err += fabs(1.0f - rvec.length());
}
TEST(err < eps);
@ -215,17 +202,17 @@ int main()
// quaternion inverse
q = q_check.inversed();
TEST(fabsf(q_check(0) - q(0)) < eps);
TEST(fabsf(q_check(1) + q(1)) < eps);
TEST(fabsf(q_check(2) + q(2)) < eps);
TEST(fabsf(q_check(3) + q(3)) < eps);
TEST(fabs(q_check(0) - q(0)) < eps);
TEST(fabs(q_check(1) + q(1)) < eps);
TEST(fabs(q_check(2) + q(2)) < eps);
TEST(fabs(q_check(3) + q(3)) < eps);
q = q_check;
q.invert();
TEST(fabsf(q_check(0) - q(0)) < eps);
TEST(fabsf(q_check(1) + q(1)) < eps);
TEST(fabsf(q_check(2) + q(2)) < eps);
TEST(fabsf(q_check(3) + q(3)) < eps);
TEST(fabs(q_check(0) - q(0)) < eps);
TEST(fabs(q_check(1) + q(1)) < eps);
TEST(fabs(q_check(2) + q(2)) < eps);
TEST(fabs(q_check(3) + q(3)) < eps);
// non-unit quaternion invese
Quatf qI(1.0f, 0.0f, 0.0f, 0.0f);
@ -237,52 +224,52 @@ int main()
rot(0) = 1.0f;
rot(1) = rot(2) = 0.0f;
qI.rotate(rot);
Quatf q_true(cosf(1.0f / 2), sinf(1.0f / 2), 0.0f, 0.0f);
TEST(fabsf(qI(0) - q_true(0)) < eps);
TEST(fabsf(qI(1) - q_true(1)) < eps);
TEST(fabsf(qI(2) - q_true(2)) < eps);
TEST(fabsf(qI(3) - q_true(3)) < eps);
Quatf q_true(cos(1.0f / 2), sin(1.0f / 2), 0.0f, 0.0f);
TEST(fabs(qI(0) - q_true(0)) < eps);
TEST(fabs(qI(1) - q_true(1)) < eps);
TEST(fabs(qI(2) - q_true(2)) < eps);
TEST(fabs(qI(3) - q_true(3)) < eps);
// rotate quaternion (zero rotation)
qI = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
rot(0) = 0.0f;
rot(1) = rot(2) = 0.0f;
qI.rotate(rot);
q_true = Quatf(cosf(0.0f), sinf(0.0f), 0.0f, 0.0f);
TEST(fabsf(qI(0) - q_true(0)) < eps);
TEST(fabsf(qI(1) - q_true(1)) < eps);
TEST(fabsf(qI(2) - q_true(2)) < eps);
TEST(fabsf(qI(3) - q_true(3)) < eps);
q_true = Quatf(cos(0.0f), sin(0.0f), 0.0f, 0.0f);
TEST(fabs(qI(0) - q_true(0)) < eps);
TEST(fabs(qI(1) - q_true(1)) < eps);
TEST(fabs(qI(2) - q_true(2)) < eps);
TEST(fabs(qI(3) - q_true(3)) < eps);
// get rotation axis from quaternion (nonzero rotation)
q = Quatf(cosf(1.0f / 2), 0.0f, sinf(1.0f / 2), 0.0f);
q = Quatf(cos(1.0f / 2), 0.0f, sin(1.0f / 2), 0.0f);
rot = q.to_axis_angle();
TEST(fabsf(rot(0)) < eps);
TEST(fabsf(rot(1) - 1.0f) < eps);
TEST(fabsf(rot(2)) < eps);
TEST(fabs(rot(0)) < eps);
TEST(fabs(rot(1) - 1.0f) < eps);
TEST(fabs(rot(2)) < eps);
// get rotation axis from quaternion (zero rotation)
q = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
rot = q.to_axis_angle();
TEST(fabsf(rot(0)) < eps);
TEST(fabsf(rot(1)) < eps);
TEST(fabsf(rot(2)) < eps);
TEST(fabs(rot(0)) < eps);
TEST(fabs(rot(1)) < eps);
TEST(fabs(rot(2)) < eps);
// from axis angle (zero rotation)
rot(0) = rot(1) = rot(2) = 0.0f;
q.from_axis_angle(rot, 0.0f);
q_true = Quatf(1.0f, 0.0f, 0.0f, 0.0f);
TEST(fabsf(q(0) - q_true(0)) < eps);
TEST(fabsf(q(1) - q_true(1)) < eps);
TEST(fabsf(q(2) - q_true(2)) < eps);
TEST(fabsf(q(3) - q_true(3)) < eps);
TEST(fabs(q(0) - q_true(0)) < eps);
TEST(fabs(q(1) - q_true(1)) < eps);
TEST(fabs(q(2) - q_true(2)) < eps);
TEST(fabs(q(3) - q_true(3)) < eps);
// Quaternion initialisation per array
float q_array[] = {0.9833f, -0.0343f, -0.1060f, -0.1436f};
Quaternion<float>q_from_array(q_array);
for (size_t i = 0; i < 4; i++) {
TEST(fabsf(q_from_array(i) - q_array[i]) < eps);
TEST(fabs(q_from_array(i) - q_array[i]) < eps);
}
// axis angle
@ -350,10 +337,10 @@ int main()
q = Quatf(1, 2, 3, 4);
float dst[4] = {};
q.copyTo(dst);
TEST(fabsf(q(0) - dst[0]) < eps);
TEST(fabsf(q(1) - dst[1]) < eps);
TEST(fabsf(q(2) - dst[2]) < eps);
TEST(fabsf(q(3) - dst[3]) < eps);
TEST(fabs(q(0) - dst[0]) < eps);
TEST(fabs(q(1) - dst[1]) < eps);
TEST(fabs(q(2) - dst[2]) < eps);
TEST(fabs(q(3) - dst[3]) < eps);
}

6
test/filter.cpp
View File

@ -1,11 +1,7 @@
#include "test_macros.hpp"
#include <matrix/filter.hpp>
using matrix::Matrix;
using matrix::kalman_correct;
using matrix::eye;
using matrix::SquareMatrix;
using matrix::Vector;
using namespace matrix;
int main()
{

5
test/hatvee.cpp
View File

@ -1,10 +1,7 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
using matrix::Dcm;
using matrix::Euler;
using matrix::isEqual;
using matrix::Vector3;
using namespace matrix;
int main()
{

5
test/helper.cpp
View File

@ -1,10 +1,7 @@
#include "test_macros.hpp"
#include <matrix/helper_functions.hpp>
using matrix::isEqual;
using matrix::isEqualF;
using matrix::Vector3f;
using matrix::wrap_pi;
using namespace matrix;
int main()
{

8
test/integration.cpp
View File

@ -1,14 +1,12 @@
#include "test_macros.hpp"
#include <matrix/integration.hpp>
using matrix::Matrix;
using matrix::ones;
using matrix::Vector;
using namespace matrix;
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/);
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & /*y*/, const Matrix<float, 3, 1> & /*u*/) {
float v = -sinf(t);
float v = -sin(t);
return v*ones<float, 6, 1>();
}
@ -20,7 +18,7 @@ int main()
float tf = 2;
float h = 0.001f;
integrate_rk4(f, y, u, t0, tf, h, y);
float v = 1 + cosf(tf) - cosf(t0);
float v = 1 + cos(tf) - cos(t0);
TEST(isEqual(y, (ones<float, 6, 1>()*v)));
return 0;
}

3
test/inverse.cpp
View File

@ -1,8 +1,7 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
using matrix::SquareMatrix;
using matrix::zeros;
using namespace matrix;
static const size_t n_large = 50;

20
test/matrixAssignment.cpp
View File

@ -1,11 +1,7 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
using matrix::Matrix;
using matrix::Matrix3f;
using matrix::Scalar;
using matrix::Vector;
using matrix::Vector2f;
using namespace matrix;
int main()
{
@ -26,7 +22,7 @@ int main()
Matrix3f m2(data);
for(int i=0; i<9; i++) {
TEST(fabsf(data[i] - m2.data()[i]) < 1e-6f);
TEST(fabs(data[i] - m2.data()[i]) < 1e-6f);
}
float data2d[3][3] = {
@ -36,7 +32,7 @@ int main()
};
m2 = Matrix3f(data2d);
for(int i=0; i<9; i++) {
TEST(fabsf(data[i] - m2.data()[i]) < 1e-6f);
TEST(fabs(data[i] - m2.data()[i]) < 1e-6f);
}
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
@ -98,17 +94,17 @@ int main()
m4.swapCols(2, 2);
TEST(isEqual(m4, Matrix3f(data)));
TEST(fabsf(m4.min() - 1) < 1e-5);
TEST(fabsf((-m4).min() + 9) < 1e-5);
TEST(fabs(m4.min() - 1) < 1e-5);
TEST(fabs((-m4).min() + 9) < 1e-5);
Scalar<float> s;
s = 1;
const Vector<float, 1> & s_vect = s;
TEST(fabsf(s - 1) < 1e-5);
TEST(fabsf(s_vect(0) - 1.0f) < 1e-5);
TEST(fabs(s - 1) < 1e-5);
TEST(fabs(s_vect(0) - 1.0f) < 1e-5);
Matrix<float, 1, 1> m5 = s;
TEST(fabsf(m5(0,0) - s) < 1e-5);
TEST(fabs(m5(0,0) - s) < 1e-5);
Matrix<float, 2, 2> m6;
m6.setRow(0, Vector2f(1, 2));

3
test/matrixMult.cpp
View File

@ -1,8 +1,7 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
using matrix::Matrix3f;
using matrix::eye;
using namespace matrix;
int main()
{

2
test/matrixScalarMult.cpp
View File

@ -2,7 +2,7 @@
#include <matrix/math.hpp>
using matrix::Matrix3f;
using namespace matrix;
int main()
{

10
test/setIdentity.cpp
View File

@ -1,7 +1,7 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
using matrix::Matrix3f;
using namespace matrix;
int main()
{
@ -11,10 +11,10 @@ int main()
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (i == j) {
TEST(fabsf(A(i, j) - 1) < 1e-7);
TEST(fabs(A(i, j) - 1) < 1e-7);
} else {
TEST(fabsf(A(i, j) - 0) < 1e-7);
TEST(fabs(A(i, j) - 0) < 1e-7);
}
}
}
@ -25,10 +25,10 @@ int main()
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (i == j) {
TEST(fabsf(B(i, j) - 1) < 1e-7);
TEST(fabs(B(i, j) - 1) < 1e-7);
} else {
TEST(fabsf(B(i, j) - 0) < 1e-7);
TEST(fabs(B(i, j) - 0) < 1e-7);
}
}
}

3
test/slice.cpp
View File

@ -1,8 +1,7 @@
#include "test_macros.hpp"
#include <matrix/math.hpp>
using matrix::Matrix;
using matrix::SquareMatrix;
using namespace matrix;
int main()
{

3
test/squareMatrix.cpp
View File

@ -2,8 +2,7 @@
#include <matrix/math.hpp>
using matrix::SquareMatrix;
using matrix::Vector3;
using namespace matrix;
int main()
{

2
test/transpose.cpp
View File

@ -2,7 +2,7 @@
#include <matrix/math.hpp>
using matrix::Matrix;
using namespace matrix;
int main()
{

3
test/vector.cpp
View File

@ -2,8 +2,7 @@
#include <matrix/math.hpp>
using matrix::isEqualF;
using matrix::Vector;
using namespace matrix;
int main()
{

21
test/vector2.cpp
View File

@ -4,31 +4,30 @@
#include "test_macros.hpp"
using matrix::Matrix;
using matrix::Vector2f;
using namespace matrix;
int main()
{
Vector2f a(1, 0);
Vector2f b(0, 1);
TEST(fabsf(a % b - 1.0f) < 1e-5);
TEST(fabs(a % b - 1.0f) < 1e-5);
Vector2f c;
TEST(fabsf(c(0) - 0) < 1e-5);
TEST(fabsf(c(1) - 0) < 1e-5);
TEST(fabs(c(0) - 0) < 1e-5);
TEST(fabs(c(1) - 0) < 1e-5);
Matrix<float, 2, 1> d(a);
TEST(fabsf(d(0,0) - 1) < 1e-5);
TEST(fabsf(d(1,0) - 0) < 1e-5);
TEST(fabs(d(0,0) - 1) < 1e-5);
TEST(fabs(d(1,0) - 0) < 1e-5);
Vector2f e(d);
TEST(fabsf(e(0) - 1) < 1e-5);
TEST(fabsf(e(1) - 0) < 1e-5);
TEST(fabs(e(0) - 1) < 1e-5);
TEST(fabs(e(1) - 0) < 1e-5);
float data[] = {4,5};
Vector2f f(data);
TEST(fabsf(f(0) - 4) < 1e-5);
TEST(fabsf(f(1) - 5) < 1e-5);
TEST(fabs(f(0) - 4) < 1e-5);
TEST(fabs(f(1) - 5) < 1e-5);
return 0;
}

5
test/vector3.cpp
View File

@ -2,10 +2,7 @@
#include <matrix/math.hpp>
using matrix::isEqual;
using matrix::isEqualF;
using matrix::Matrix;
using matrix::Vector3f;
using namespace matrix;
int main()
{

21
test/vectorAssignment.cpp
View File

@ -2,8 +2,7 @@
#include "test_macros.hpp"
using matrix::SquareMatrix;
using matrix::Vector3f;
using namespace matrix;
int main()
{
@ -14,20 +13,20 @@ int main()
static const float eps = 1e-7f;
TEST(fabsf(v(0) - 1) < eps);
TEST(fabsf(v(1) - 2) < eps);
TEST(fabsf(v(2) - 3) < eps);
TEST(fabs(v(0) - 1) < eps);
TEST(fabs(v(1) - 2) < eps);
TEST(fabs(v(2) - 3) < eps);
Vector3f v2(4, 5, 6);
TEST(fabsf(v2(0) - 4) < eps);
TEST(fabsf(v2(1) - 5) < eps);
TEST(fabsf(v2(2) - 6) < eps);
TEST(fabs(v2(0) - 4) < eps);
TEST(fabs(v2(1) - 5) < eps);
TEST(fabs(v2(2) - 6) < eps);
SquareMatrix<float, 3> m = diag(Vector3f(1,2,3));
TEST(fabsf(m(0, 0) - 1) < eps);
TEST(fabsf(m(1, 1) - 2) < eps);
TEST(fabsf(m(2, 2) - 3) < eps);
TEST(fabs(m(0, 0) - 1) < eps);
TEST(fabs(m(1, 1) - 2) < eps);
TEST(fabs(m(2, 2) - 3) < eps);
return 0;
}