AP_NavEKF: Prevent baro drift causing toilet bowling

The magnetic field states are reset once at 1.5 metres and again at 5 metres. This height check was using the height at the first arm event as the reference. In the situation where there is baro drift and extgended time between the first arm event and flight, this can cause the magnetic field state to be reset when on the ground. If this happens when flying off a metallic surface, the resultant heading errors can cause sever toilet bowling.

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -4403,7 +4403,6 @@ void NavEKF::InitialiseVariables()
     dtIMUactual = 0.0025f;
     dt = 0;
     hgtMea = 0;
-    firstArmPosD = 0.0f;
     storeIndex = 0;
     lastGyroBias.zero();
 	prevDelAng.zero();
@@ -4667,7 +4666,6 @@ void NavEKF::performArmingChecks()
         if (vehicleArmed && !firstArmComplete) {
             firstArmComplete = true;
             state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
-            firstArmPosD = state.position.z;
         }
         // zero stored velocities used to do dead-reckoning
         heldVelNE.zero();
@@ -4723,12 +4721,12 @@ void NavEKF::performArmingChecks()
         ResetHeight();
         StoreStatesReset();
 
-    } else if (vehicleArmed && !firstMagYawInit && firstArmPosD - state.position.z > 1.5f && !assume_zero_sideslip()) {
+    } else if (vehicleArmed && !firstMagYawInit && state.position.z < -1.5f && !assume_zero_sideslip()) {
         // Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
         // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
         state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
         firstMagYawInit = true;
-    } else if (vehicleArmed && !secondMagYawInit && firstArmPosD - state.position.z > 5.0f && !assume_zero_sideslip()) {
+    } else if (vehicleArmed && !secondMagYawInit && state.position.z < -5.0f && !assume_zero_sideslip()) {
         // Do the second and final yaw and earth mag field initialisation when the vehicle has gained 5.0m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)
         // This second and final correction is needed for flight from large metal structures where the magnetic field distortion can extend up to 5m
         state.quat = calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);

libraries/AP_NavEKF/AP_NavEKF.h

@@ -611,7 +611,6 @@ private:
     float tasTestRatio;             // sum of squares of true airspeed innovation divided by fail threshold
     bool inhibitWindStates;         // true when wind states and covariances are to remain constant
     bool inhibitMagStates;          // true when magnetic field states and covariances are to remain constant
-    float firstArmPosD;             // vertical position at the first arming (transition from sttatic mode) after start up
     bool firstArmComplete;          // true when first transition out of static mode has been performed after start up
     bool firstMagYawInit;           // true when the first post takeoff initialisation of earth field and yaw angle has been performed
     bool secondMagYawInit;          // true when the second post takeoff initialisation of earth field and yaw angle has been performed