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

Euler: simplify DCM constructor

Browse Source 
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
2021-04-14 15:40:24 -04:00
committed by GitHub
parent 3679f7fd51
commit 1344ce06fd
1 changed files with 11 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/Euler.hpp
+11 -14
View File
@@ -91,23 +91,20 @@ public:
*/
Euler(const Dcm<Type> &dcm)
{
Type phi_val = Type(atan2(dcm(2, 1), dcm(2, 2)));
Type theta_val = Type(asin(-dcm(2, 0)));
Type psi_val = Type(atan2(dcm(1, 0), dcm(0, 0)));
Type pi = Type(M_PI);
theta() = asin(-dcm(2, 0));
if (Type(fabs(theta_val - pi / Type(2))) < Type(1.0e-3)) {
phi_val = Type(0);
psi_val = Type(atan2(dcm(1, 2), dcm(0, 2)));
if ((fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
phi() = 0;
psi() = atan2(dcm(1, 2), dcm(0, 2));
} else if (Type(fabs(theta_val + pi / Type(2))) < Type(1.0e-3)) {
phi_val = Type(0);
psi_val = Type(atan2(-dcm(1, 2), -dcm(0, 2)));
} else if ((fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
phi() = 0;
psi() = atan2(-dcm(1, 2), -dcm(0, 2));
} else {
phi() = atan2(dcm(2, 1), dcm(2, 2));
psi() = atan2(dcm(1, 0), dcm(0, 0));
}
phi() = phi_val;
theta() = theta_val;
psi() = psi_val;
}
/**