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

ekf2: utils add getEulerYaw() that uses the best rotation sequence

Browse Source
This commit is contained in:
Daniel Agar
2022-02-02 08:54:03 -05:00
parent c3e0b93fc8
commit 88ffc177f7
9 changed files with 24 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/EKFGSF_yaw.cpp
+1 -4
View File
@@ -245,10 +245,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
}
// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
const Dcmf &R = _ahrs_ekf_gsf[model_index].R;
_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
getEuler321Yaw(R) :
getEuler312Yaw(R);
_ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R);
// calculate delta velocity in a horizontal front-right frame
const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;