dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1888 from PX4/quat_rot_math_test 

added quaternion rotation method test
This commit is contained in:
Lorenz Meier 2015-03-07 00:56:07 +01:00
parent 600628b55b af56e58540
commit 5b2643fc20
1 changed files with 72 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
src/systemcmds/tests/test_mathlib.cpp
View File

@ -282,5 +282,76 @@ 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_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3,3> R;
float diff = 0.1f;
float tol = 0.00001f;
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.from_euler(roll, pitch, yaw);
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) {
warnx("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.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) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
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) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
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) {
warnx("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.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) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
return rc;
}
}