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

Add helper function to alter heading in rotation matrices

Browse Source
This commit is contained in:
Kamil Ritz
2020-08-27 09:51:56 +10:00
committed by Paul Riseborough
parent fdc86c247a
commit 2be738e806
5 changed files with 33 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/EKFGSF_yaw.cpp
+6 -16
View File
@@ -225,24 +225,14 @@ void EKFGSF_yaw::ahrsAlignYaw()
// Align yaw angle for each model
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
// get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence
Eulerf euler_init(_ahrs_ekf_gsf[model_index].R);
// set the yaw angle
euler_init(2) = wrap_pi(_ekf_gsf[model_index].X(2));
// update the rotation matrix
_ahrs_ekf_gsf[model_index].R = Dcmf(euler_init);
// update the rotation matrix with 321 rotation sequence
_ahrs_ekf_gsf[model_index].R = updateEuler321YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
_ahrs_ekf_gsf[model_index].R);
} else {
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
const Vector3f rot312(wrap_pi(_ekf_gsf[model_index].X(2)), // yaw
asinf(_ahrs_ekf_gsf[model_index].R(2, 1)), // roll
atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0),
_ahrs_ekf_gsf[model_index].R(2, 2))); // pitch
// Calculate the body to earth frame rotation matrix
_ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312);
// update the rotation matrix with 312 rotation sequence
_ahrs_ekf_gsf[model_index].R = updateEuler312YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
_ahrs_ekf_gsf[model_index].R);
}
_ahrs_ekf_gsf[model_index].aligned = true;