AP_NavEKF2: Don't start GPS aiding until gyro calibration has stabilised

If we start GPS aiding before the gyro bias variances have reduced, glitches on the GPS can cause attitude disturbances that degrade flight accuracy during early flight.

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -160,13 +160,17 @@ void NavEKF2_core::setAidingMode()
     // Determine when to commence aiding for inertial navigation
     // Save the previous status so we can detect when it has changed
     prevIsAiding = isAiding;
-    // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
+    // perform aiding checks if not aiding
-    bool filterIsStable = tiltAlignComplete && yawAlignComplete;
+     if (!isAiding) {
-    // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
+        // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
-    bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
+        // and IMU gyro bias estimates have stabilised
-    // Start aiding if we have a source of aiding data and the filter attitude algnment is complete
+        bool filterIsStable = tiltAlignComplete && yawAlignComplete && imuCalCompleted();
-    // Latch to on
+        // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
-    isAiding = ((readyToUseGPS() || useFlowAiding) && filterIsStable) || isAiding;
+        bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
+        // Start aiding if we have a source of aiding data and the filter attitude algnment is complete
+        // Latch to on
+        isAiding = (readyToUseGPS() || useFlowAiding) && filterIsStable;
+    }
 
     // check to see if we are starting or stopping aiding and set states and modes as required
     if (isAiding != prevIsAiding) {
@@ -323,6 +327,19 @@ void NavEKF2_core::recordYawReset()
     }
 }
 
+// return true when IMU calibration completed
+bool NavEKF2_core::imuCalCompleted(void)
+{
+    // check delta angle bias variances
+    const float delAngBiasVarMax = sq(radians(0.1f * dtEkfAvg));
+    bool gyroCalComplete =  (P[9][9] <= delAngBiasVarMax) &&
+                            (P[10][10] <= delAngBiasVarMax) &&
+                            (P[11][11] <= delAngBiasVarMax);
+
+    return gyroCalComplete;
+
+}
+
 // Commands the EKF to not use GPS.
 // This command must be sent prior to arming
 // This command is forgotten by the EKF each time the vehicle disarms

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -586,6 +586,9 @@ private:
     // Assess GPS data quality and return true if good enough to align the EKF
     bool calcGpsGoodToAlign(void);
 
+    // return true when IMU calibration completed
+    bool imuCalCompleted(void);
+
     // update inflight calculaton that determines if GPS data is good enough for reliable navigation
     void calcGpsGoodForFlight(void);