diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 474350566e..292ac5e647 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/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 -bool NavEKF2_core::imuCalCompleted(void) +// return true and set the class variable true if the delta angle bias has been learned +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 gyroCalComplete; - + return delAngBiasLearned; } // Commands the EKF to not use GPS. diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 1249c5f16e..62a7c02ce0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/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 gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign; + bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3) && delAngBiasLearned; + 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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index ad95e86e28..7ae9da8c20 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/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(); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 5f4f4cb619..174a4c5bd1 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/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 - bool imuCalCompleted(void); + // return true and set the class variable true if the delta angle bias has been learned + 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;