From 4a8689aa97b61f3e2fd255ff82a992f5a678a0bd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 25 Jun 2016 21:07:35 +1000 Subject: [PATCH] AP_NavEKF2: remove duplicate if statements from mag fusion --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index bee86f8809..ba22669f93 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -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);