AP_NavEKF2: Improve protection against ground based mag interference

Reset the mag field states and yaw earlier than the normal 5m height threshold if toilet bowling is detected.

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -30,9 +30,18 @@ void NavEKF2_core::controlMagYawReset()
 
     // In-Flight reset for vehicle that cannot use a zero sideslip assumption
     // Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
+    // Perform the reset earlier if high yaw and velocity innovations indicate that 'toilet bowling' is occurring
     // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
     // Delay if rotated too far since the last check as rapid rotations will produce errors in the magnetic field states
-    if (!assume_zero_sideslip() && inFlight && !firstMagYawInit && (stateStruct.position.z  - posDownAtTakeoff) < -5.0f && deltaRot < 0.1745f) {
+    if (!firstMagYawInit && !assume_zero_sideslip() && inFlight && deltaRot < 0.1745f) {
+        // check that we have reached a height where ground magnetic interference effects are insignificant
+        bool hgtCheckPassed = (stateStruct.position.z  - posDownAtTakeoff) < -5.0f;
+
+        // check for 'toilet bowling' which is characterised by large yaw and velocity innovations and caused by bad yaw alignment
+        // this can occur if there is severe magnetic interference on the ground
+        bool toiletBowling = (yawTestRatio > 1.0f) && (velTestRatio > 1.0f);
+
+        if (hgtCheckPassed || toiletBowling) {
             firstMagYawInit = true;
             // reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
             magFuseTiltInhibit_ms =  imuSampleTime_ms;
@@ -46,6 +55,7 @@ void NavEKF2_core::controlMagYawReset()
             tempQuat = stateStruct.quat/tempQuat;
             StoreQuatRotate(tempQuat);
         }
+    }
 
     // In-Flight reset for vehicles that can use a zero sideslip assumption (Planes)
     // this is done to protect against unrecoverable heading alignment errors due to compass faults