AP_NavEKF: Fix failure to start mag cal due to gyro noise

Vibration in the 400Hz delta angles could cause the angular rate condition check for in-flight magnetic field alignment to fail.
The symptons were failure to start magnetic field learning as expected when EK2_MAG_CAL=3 was set.
The calculation of a delta rotation between consecutive magnetometer samples has been introduced instead of the most recent IMU delta angle as this is less affected by noise and give an upper bound on the angular error.
the check has been moved into the magnetometer fusion control function so that any reset will be performed using fresh magnetometer data

libraries/AP_NavEKF/AP_NavEKF.cpp

@@ -1021,6 +1021,34 @@ void NavEKF::SelectMagFusion()
     // determine if conditions are right to start a new fusion cycle
     bool dataReady = statesInitialised && use_compass() && newDataMag;
     if (dataReady) {
+        // Calculate change in angle since last magetoemter fusion - used to check if in-flight alignment can be performed
+        // Use a quaternion division to calcualte the delta quaternion between the rotation at the current and last time
+        Quaternion deltaQuat = state.quat / prevQuatMagReset;
+        prevQuatMagReset = state.quat;
+        // convert the quaternion to a rotation vector and find its length
+        Vector3f deltaRotVec;
+        deltaQuat.to_axis_angle(deltaRotVec);
+        float deltaRot = deltaRotVec.length();
+
+        // Check if the magnetic field states should be reset
+        if (vehicleArmed && !firstMagYawInit && (state.position.z  - posDownAtArming) < -1.5f && !assume_zero_sideslip() && deltaRot < 0.1745f) {
+            // 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
+            // Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
+            Vector3f eulerAngles;
+            getEulerAngles(eulerAngles);
+            state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+            firstMagYawInit = true;
+        } else if (vehicleArmed && !secondMagYawInit && (state.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip() && deltaRot < 0.1745f) {
+            // 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
+            // Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
+            Vector3f eulerAngles;
+            getEulerAngles(eulerAngles);
+            state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
+            secondMagYawInit = true;
+        }
+
         // reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
         memset(&magIncrStateDelta[0], 0, sizeof(magIncrStateDelta));
         magUpdateCount = 0;
@@ -5169,22 +5197,6 @@ void NavEKF::performArmingChecks()
             StoreStatesReset();
         }
 
-    } else if (vehicleArmed && !firstMagYawInit && (state.position.z  - posDownAtArming) < -1.5f && !assume_zero_sideslip() && state.omega.length() < 1.0f) {
-        // 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
-        // Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
-        Vector3f eulerAngles;
-        getEulerAngles(eulerAngles);
-        state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
-        firstMagYawInit = true;
-    } else if (vehicleArmed && !secondMagYawInit && (state.position.z - posDownAtArming) < -5.0f && !assume_zero_sideslip() && state.omega.length() < 1.0f) {
-        // 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
-        // Do not do this alignment if the vehicle is rotating rapidly as timing erors in the mag data will cause significant errors
-        Vector3f eulerAngles;
-        getEulerAngles(eulerAngles);
-        state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
-        secondMagYawInit = true;
     }
 
     // Always turn aiding off when the vehicle is disarmed

libraries/AP_NavEKF/AP_NavEKF.h

@@ -728,6 +728,7 @@ private:
     uint32_t lastVelReset_ms;       // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
     Vector3f delAngBiasAtArming;    // value of the gyro delta angle bias at arming
     float hgtInnovFiltState;        // state used for fitering of the height innovations used for pre-flight checks
+    Quaternion prevQuatMagReset;    // Quaternion from the previous frame that the magnetic field state reset condition test was performed
 
     // Used by smoothing of state corrections
     Vector10 gpsIncrStateDelta;    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement