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

add functions to compute yaw (321 and 312 sequence)

Browse Source 
from quaternion and rotation matrix
This commit is contained in:
Kamil Ritz
2020-08-27 09:51:56 +10:00
committed by Paul Riseborough
parent 15afa8ae17
commit fdc86c247a
9 changed files with 60 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/EKFGSF_yaw.cpp
+2 -4
View File
@@ -261,11 +261,9 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
// use 321 Tait-Bryan rotation to define yaw state
_ekf_gsf[model_index].X(2) = atan2f(_ahrs_ekf_gsf[model_index].R(1, 0), _ahrs_ekf_gsf[model_index].R(0, 0));
_ekf_gsf[model_index].X(2) = getEuler321Yaw(_ahrs_ekf_gsf[model_index].R);
} else {
// use 312 Tait-Bryan rotation to define yaw state
_ekf_gsf[model_index].X(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(0, 1), _ahrs_ekf_gsf[model_index].R(1, 1)); // first rotation (yaw)
_ekf_gsf[model_index].X(2) = getEuler312Yaw(_ahrs_ekf_gsf[model_index].R);
}
// calculate delta velocity in a horizontal front-right frame