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

Handle some divide by zero edge cases on init.

Browse Source
This commit is contained in:
James Goppert
2016-11-30 01:27:48 -05:00
parent 945edbc58c
commit 74a120f554
3 changed files with 28 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/AxisAngle.hpp
+10 -6
View File
@@ -78,14 +78,14 @@ public:
AxisAngle &v = *this;
Type ang = (Type)2.0f*acosf(q(0));
Type mag = sinf(ang/2.0f);
if (fabsf(ang) < 1e-10f) {
v(0) = 1e-10f;
v(1) = 0;
v(2) = 0;
} else {
if (fabsf(mag) > 0) {
v(0) = ang*q(1)/mag;
v(1) = ang*q(2)/mag;
v(2) = ang*q(3)/mag;
} else {
v(0) = 0;
v(1) = 0;
v(2) = 0;
}
}
@@ -156,7 +156,11 @@ public:
Vector<Type, 3> axis() {
return Vector<Type, 3>::unit();
if (Vector<Type, 3>::norm() > 0) {
return Vector<Type, 3>::unit();
} else {
return Vector3<Type>(1, 0, 0);
}
}
Type angle() {
matrix/Quaternion.hpp
+14 -8
View File
@@ -94,14 +94,20 @@ public:
Vector<Type, 4>()
{
Quaternion &q = *this;
q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(Type(4) * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
(Type(4) * q(0)));
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(Type(4) * q(0)));
q(0) = 1;
q(1) = 0;
q(2) = 0;
q(3) = 0;
if (((Type(1) + dcm(0, 0) + dcm(1, 1) + dcm(2, 2)) > 0) & (fabsf(q(0)) > 0) ) {
q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(Type(4) * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /
(Type(4) * q(0)));
q(3) = Type((dcm(1, 0) - dcm(0, 1)) /
(Type(4) * q(0)));
}
}
/**
test/attitude.cpp
+4
View File
@@ -283,6 +283,10 @@ int main()
AxisAnglef aa_data_init(aa_data);
TEST(isEqual(aa_data_init, AxisAnglef(4.0f, 5.0f, 6.0f)));
AxisAnglef aa_norm_check(Vector3f(0.0f, 0.0f, 0.0f));
TEST(isEqual(aa_norm_check.axis(), Vector3f(1, 0, 0)));
TEST(isEqualF(aa_norm_check.angle(), 0.0f));
q = Quatf(-0.29555112749297824f, 0.25532186f, 0.51064372f, 0.76596558f);
TEST(isEqual(q.imag(), Vector3f(0.25532186f, 0.51064372f, 0.76596558f)));