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

Fix for euler.

Browse Source
This commit is contained in:
James Goppert
2016-01-14 15:24:37 -05:00
parent a22a47fe15
commit fa31c61f2c
8 changed files with 281 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+3 -3
View File
@@ -52,11 +52,11 @@ public:
if (fabs(theta() - (Type)M_PI_2) < 1.0e-3) {
phi() = (Type)0.0;
psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) + theta();
psi() = (Type)atan2(dcm(0,1), dcm(1,1));
psi() = (Type)atan2(dcm(1,2), dcm(0,2));
} else if ((Type)fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) {
phi() = (Type)0.0;
psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) - theta();
psi() = (Type)atan2(-dcm(1,2), -dcm(0,2));
} else {
phi() = (Type)atan2(dcm(2,1), dcm(2,2));