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

fix euler calculation

Browse Source 
Conflicts:
	matrix/Euler.hpp
This commit is contained in:
Roman Bapst
2016-01-14 15:51:28 +01:00
committed by Roman
parent 1d9d8e6f9b
commit a22a47fe15
1 changed files with 12 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+12 -15
View File
@@ -48,22 +48,19 @@ public:
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>() Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
{ {
Type psi_val = Type(atan(dcm(1, 0)/ dcm(0, 0))); theta() = (Type)asin(-dcm(2,0));
Type theta_val = Type(asin(-dcm(2,0)));
Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2)));
// protection against NaN if dcm(0,0) or dcm(2,2) == 0 if (fabs(theta() - (Type)M_PI_2) < 1.0e-3) {
psi() = 0; phi() = (Type)0.0;
theta() = 0; psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) + theta();
phi() = 0;
if (psi() >= -(Type)M_PI_2 && psi() <= (Type)M_PI_2) { } else if ((Type)fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) {
psi() = psi_val; phi() = (Type)0.0;
} psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) - theta();
if (theta() >= -(Type)M_PI_2 && theta() <= (Type)M_PI_2) {
theta() = theta_val; } else {
} phi() = (Type)atan2(dcm(2,1), dcm(2,2));
if (phi() >= -(Type)M_PI_2 && phi() <= (Type)M_PI_2) { psi() = (Type)atan2(dcm(1,0), dcm(0,0));
phi() = phi_val;
} }
} }