Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use use_EKFGSFYaw to reduce code duplication

Browse Source
This commit is contained in:
Peter Barker 2020-12-17 11:47:13 +11:00 committed by Randy Mackay
parent a31ea88ff5
commit 6dfd2f2ff9
1 changed files with 2 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -1464,12 +1464,8 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
return false;
};
float yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
is_positive(yawVarianceEKFGSF) &&
yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
(assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
float yawEKFGSF, yawVarianceEKFGSF;
if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
// keep roll and pitch and reset yaw
rotationOrder order;
bestRotationOrder(order);