AP_NavEKF2: 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.

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -70,9 +70,9 @@ void NavEKF2_core::setWindMagStateLearningMode()
     // Inhibit the magnetic field calibration if not requested or denied
     inhibitMagStates = (!magCalRequested || magCalDenied);
 
-    // If magnetometer states are inhibited, we clear the flag indicating that the magnetic field in-flight initialisation has been completed
+    // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
-    // because it will need to be done again
+    // because we want it re-done for each takeoff
-    if (inhibitMagStates) {
+    if (onGround) {
         firstMagYawInit = false;
     }
 

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -20,10 +20,18 @@ extern const AP_HAL::HAL& hal;
 // Control reset of yaw and magnetic field states
 void NavEKF2_core::controlMagYawReset()
 {
+    // Use a quaternion division to calcualte the delta quaternion between the rotation at the current and last time
+    Quaternion deltaQuat = stateStruct.quat / prevQuatMagReset;
+    prevQuatMagReset = stateStruct.quat;
+    // convert the quaternion to a rotation vector and find its length
+    Vector3f deltaRotVec;
+    deltaQuat.to_axis_angle(deltaRotVec);
+    float deltaRot = deltaRotVec.length();
+
     // Monitor the gain in height and reset the magnetic field states and heading when initial altitude has been gained
     // This is done to prevent magnetic field distoration from steel roofs and adjacent structures causing bad earth field and initial yaw values
-    // Delay if rotating rapidly as time offsets will produce errors in the magnetic field states
+    // Delay if rotated too far since the last check as rapid rotations will produce errors in the magnetic field states
-    if (inFlight && !firstMagYawInit && (stateStruct.position.z  - posDownAtTakeoff) < -5.0f && (imuDataDelayed.delVel.length())/imuDataDelayed.delAngDT < 1.0f) {
+    if (inFlight && !firstMagYawInit && (stateStruct.position.z  - posDownAtTakeoff) < -5.0f && deltaRot < 0.1745f) {
         firstMagYawInit = true;
         // reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
         magFuseTiltInhibit_ms =  imuSampleTime_ms;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -778,6 +778,7 @@ private:
     Vector2f velResetNE;            // Change in North/East velocity due to last in-flight reset in metres/sec. Returned by getLastVelNorthEastReset
     uint32_t lastVelReset_ms;       // System time at which the last velocity reset occurred. Returned by getLastVelNorthEastReset
     float yawTestRatio;             // square of magnetometer yaw angle innovation divided by fail threshold
+    Quaternion prevQuatMagReset;    // Quaternion from the last time the magnetic field state reset condition test was performed
 
     // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
     float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.