AP_NavEKF2: Don't declare ready to do aiding unless gyro bias is learned

libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@@ -164,7 +164,7 @@ void NavEKF2_core::setAidingMode()
      if (!isAiding) {
         // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
         // and IMU gyro bias estimates have stabilised
-        bool filterIsStable = tiltAlignComplete && yawAlignComplete && imuCalCompleted();
+        bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
         // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
         bool useFlowAiding = (frontend->_fusionModeGPS == 3) && optFlowDataPresent();
         // Start aiding if we have a source of aiding data and the filter attitude algnment is complete
@@ -327,17 +327,15 @@ void NavEKF2_core::recordYawReset()
     }
 }
 
-// return true when IMU calibration completed
+// return true and set the class variable true if the delta angle bias has been learned
-bool NavEKF2_core::imuCalCompleted(void)
+bool NavEKF2_core::checkGyroCalStatus(void)
 {
     // check delta angle bias variances
     const float delAngBiasVarMax = sq(radians(0.1f * dtEkfAvg));
-    bool gyroCalComplete =  (P[9][9] <= delAngBiasVarMax) &&
+    delAngBiasLearned =  (P[9][9] <= delAngBiasVarMax) &&
                             (P[10][10] <= delAngBiasVarMax) &&
                             (P[11][11] <= delAngBiasVarMax);
-
+    return delAngBiasLearned;
-    return gyroCalComplete;
-
 }
 
 // Commands the EKF to not use GPS.

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -459,14 +459,13 @@ void  NavEKF2_core::getFilterStatus(nav_filter_status &status) const
 {
     // init return value
     status.value = 0;
-
     bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
     bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
     bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
     bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
     bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
-    bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3);
+    bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3) && delAngBiasLearned;
-    bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign;
+    bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
     bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
     // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
     bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -262,6 +262,7 @@ void NavEKF2_core::InitialiseVariables()
     yawInnovAtLastMagReset = 0.0f;
     quatAtLastMagReset = stateStruct.quat;
     magFieldLearned = false;
+    delAngBiasLearned = false;
 
     // zero data buffers
     storedIMU.reset();

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -586,8 +586,8 @@ private:
     // Assess GPS data quality and return true if good enough to align the EKF
     bool calcGpsGoodToAlign(void);
 
-    // return true when IMU calibration completed
+    // return true and set the class variable true if the delta angle bias has been learned
-    bool imuCalCompleted(void);
+    bool checkGyroCalStatus(void);
 
     // update inflight calculaton that determines if GPS data is good enough for reliable navigation
     void calcGpsGoodForFlight(void);
@@ -801,6 +801,7 @@ private:
     bool magFieldLearned;           // true when the magnetic field has been learned
     Vector3f earthMagFieldVar;      // NED earth mag field variances for last learned field (mGauss^2)
     Vector3f bodyMagFieldVar;       // XYZ body mag field variances for last learned field (mGauss^2)
+    bool delAngBiasLearned;         // true when the gyro bias has been learned
 
     Vector3f outputTrackError;