fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +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:17:13 -05:00
parent 74a120f554
commit 7eff04742c
1 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
matrix/Quaternion.hpp
View File

@ -98,9 +98,10 @@ public:
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)));
Type q0_tmp = Type(0.5) * Type(sqrt(Type(1) + dcm(0, 0) +
dcm(1, 1) + dcm(2, 2)));
if (fabsf(q0_tmp) > 0) {
q(0) = q0_tmp;
q(1) = Type((dcm(2, 1) - dcm(1, 2)) /
(Type(4) * q(0)));
q(2) = Type((dcm(0, 2) - dcm(2, 0)) /