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

Fix bugs in axis angle.

Browse Source
This commit is contained in:
James Goppert
2016-07-06 01:37:50 -04:00
parent 7b7297b674
commit 9353e4cb64
2 changed files with 12 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/AxisAngle.hpp
+11 -5
View File
@@ -78,9 +78,15 @@ public:
AxisAngle &v = *this;
Type angle = (Type)2.0f*acosf(q(0));
Type mag = sinf(angle/2.0f);
v(0) = angle*q(1)/mag;
v(1) = angle*q(2)/mag;
v(2) = angle*q(3)/mag;
if (fabs(angle) < 1e-10f) {
v(0) = 0;
v(1) = 0;
v(2) = 0;
} else {
v(0) = angle*q(1)/mag;
v(1) = angle*q(2)/mag;
v(2) = angle*q(3)/mag;
}
}
/**
@@ -95,7 +101,7 @@ public:
Vector<Type, 3>()
{
AxisAngle &v = *this;
v = Quaternion<float>(dcm);
v = Quaternion<Type>(dcm);
}
/**
@@ -111,7 +117,7 @@ public:
Vector<Type, 3>()
{
AxisAngle &v = *this;
v = Quaternion<float>(euler);
v = Quaternion<Type>(euler);
}
/**
test/attitude.cpp
+1 -1
View File
@@ -259,7 +259,7 @@ int main()
TEST(isEqual(aa_q_init, AxisAnglef(1.0f, 2.0f, 3.0f)));
AxisAnglef aa_euler_init(Eulerf(0.0f, 0.0f, 0.0f));
TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 1.0f)));
TEST(isEqual(aa_euler_init, Vector3f(0.0f, 0.0f, 0.0f)));
Dcmf dcm_aa_check = AxisAnglef(dcm_check);
TEST(isEqual(dcm_aa_check, dcm_check));