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

Tests: Fix code style on system tests

Browse Source
This commit is contained in:
Lorenz Meier
2015-03-28 19:51:15 +01:00
parent b24af0f402
commit 32e790110e
17 changed files with 424 additions and 237 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/tests/test_mathlib.cpp
+26 -21
View File
@@ -284,11 +284,11 @@ int test_mathlib(int argc, char *argv[])
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f,1.0f,1.0f};
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3,3> R;
Matrix<3, 3> R;
float diff = 0.1f;
float tol = 0.00001f;
@@ -298,11 +298,12 @@ int test_mathlib(int argc, char *argv[])
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;
q.from_euler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.rotate(vector);
for (int i = 0; i < 3; i++) {
if(fabsf(vector_r(i) - vector_q(i)) > tol) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
@@ -313,41 +314,45 @@ int test_mathlib(int argc, char *argv[])
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F,0.0f,0.0f);
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.rotate(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) {
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) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(0.3f,0.2f,0.1f);
q.from_euler(0.3f, 0.2f, 0.1f);
vector_q = q.rotate(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) {
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(-1.5f,-0.2f,0.5f);
q.from_euler(-1.5f, -0.2f, 0.5f);
vector_q = q.rotate(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) {
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f);
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.rotate(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) {
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}