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

Fix for zero check on quat from dcm ctor.

Browse Source
This commit is contained in:
James Goppert
2016-12-13 10:21:55 -05:00
parent 74a120f554
commit 7eff04742c
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Quaternion.hpp
+4 -3
View File
@@ -98,9 +98,10 @@ public:
q(1) = 0; q(1) = 0;
q(2) = 0; q(2) = 0;
q(3) = 0; q(3) = 0;
if (((Type(1) + dcm(0, 0) + dcm(1, 1) + dcm(2, 2)) > 0) & (fabsf(q(0)) > 0) ) { Type q0_tmp = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
q(0) = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) + dcm(1, 1) + dcm(2, 2)));
dcm(1, 1) + dcm(2, 2))); if (fabsf(q0_tmp) > 0) {
q(0) = q0_tmp;
q(1) = Type((dcm(2, 1) - dcm(1, 2)) / q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(Type(4) * q(0))); (Type(4) * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) / q(2) = Type((dcm(0, 2) - dcm(2, 0)) /