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 euler calculation

Browse Source 
Conflicts:
	matrix/Euler.hpp
This commit is contained in:
Roman Bapst 2015-12-10 16:40:30 +01:00 committed by Roman
parent 1d9d8e6f9b
commit a22a47fe15
1 changed files with 12 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
matrix/Euler.hpp
View File

@ -48,22 +48,19 @@ public:
Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
{
Type psi_val = Type(atan(dcm(1, 0)/ dcm(0, 0)));
Type theta_val = Type(asin(-dcm(2,0)));
Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2)));
theta() = (Type)asin(-dcm(2,0));
// protection against NaN if dcm(0,0) or dcm(2,2) == 0
psi() = 0;
theta() = 0;
phi() = 0;
if (psi() >= -(Type)M_PI_2 && psi() <= (Type)M_PI_2) {
psi() = psi_val;
}
if (theta() >= -(Type)M_PI_2 && theta() <= (Type)M_PI_2) {
theta() = theta_val;
}
if (phi() >= -(Type)M_PI_2 && phi() <= (Type)M_PI_2) {
phi() = phi_val;
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();
} 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();
} else {
phi() = (Type)atan2(dcm(2,1), dcm(2,2));
psi() = (Type)atan2(dcm(1,0), dcm(0,0));
}
}