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

AP_NavEKF2: remove duplicate if statements from mag fusion

Browse Source
This commit is contained in:
Paul Riseborough 2016-06-25 21:07:35 +10:00 committed by Andrew Tridgell
parent 094dd9d769
commit 4a8689aa97
1 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -42,16 +42,14 @@ void NavEKF2_core::controlMagYawReset()
}
// check that we have reached a height where ground magnetic interference effects are insignificant
// and can perform a final reset of the yaw and field states
// Check if conditions for a interim or final yaw/mag reset are met
bool finalResetRequest = false;
if (flightResetAllowed && !assume_zero_sideslip()) {
finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
}
// Check for bad yaw casued by ground based magnetic anomaly
bool interimResetRequest = false;
if (flightResetAllowed && !assume_zero_sideslip()) {
// check that we have reached a height where ground magnetic interference effects are insignificant
// and can perform a final reset of the yaw and field states
finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -5.0f;
// check for increasing height
bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
float yawInnovIncrease = fabsf(innovYaw) - fabsf(yawInnovAtLastMagReset);