diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index a2c4ed6bfd..0b74b4ce59 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -201,7 +201,7 @@ bool NavEKF2_core::optFlowDataPresent(void) const // return true if the filter to be ready to use gps bool NavEKF2_core::readyToUseGPS(void) const { - return validOrigin && tiltAlignComplete && yawAlignComplete && gpsQualGood; + return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign; } // return true if we should use the compass diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 70487bca62..3fe286302f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -490,7 +490,7 @@ void NavEKF2_core::readGpsData() // Monitor quality of the GPS velocity data before and after alignment using separate checks if (PV_AidingMode != AID_ABSOLUTE) { // Pre-alignment checks - gpsQualGood = calcGpsGoodToAlign(); + gpsGoodToAlign = calcGpsGoodToAlign(); } else { // Post-alignment checks calcGpsGoodForFlight(); @@ -501,7 +501,7 @@ void NavEKF2_core::readGpsData() const struct Location &gpsloc = _ahrs->get_gps().location(); if (validOrigin) { gpsDataNew.pos = location_diff(EKF_origin, gpsloc); - } else if (gpsQualGood) { + } else if (gpsGoodToAlign) { // Set the NE origin to the current GPS position setOrigin(); // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index e69835e8d5..5aac1e53b7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -396,7 +396,7 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && (frontend._fusionModeGPS == 3); - bool gpsNavPossible = !gpsNotAvailable && (frontend._fusionModeGPS <= 2); + bool gpsNavPossible = !gpsNotAvailable && (PV_AidingMode == AID_ABSOLUTE) && gpsGoodToAlign; bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete; // set individual flags diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 62dccb989d..efced1ac6e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -167,7 +167,7 @@ void NavEKF2_core::InitialiseVariables() delAngCorrection.zero(); delVelCorrection.zero(); velCorrection.zero(); - gpsQualGood = false; + gpsGoodToAlign = false; gpsNotAvailable = true; motorsArmed = false; prevMotorsArmed = false; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index a85cd26ea1..d1408d2159 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -753,8 +753,8 @@ private: Vector3f delVelCorrection; // correction applied to earth frame delta velocities used by output observer to track the EKF Vector3f velCorrection; // correction applied to velocities used by the output observer to track the EKF float innovYaw; // compass yaw angle innovation (rad) - uint32_t timeTasReceived_ms; // tie last TAS data was received (msec) - bool gpsQualGood; // true when the GPS quality can be used to initialise the navigation system + uint32_t timeTasReceived_ms; // time last TAS data was received (msec) + bool gpsGoodToAlign; // true when the GPS quality can be used to initialise the navigation system uint32_t magYawResetTimer_ms; // timer in msec used to track how long good magnetometer data is failing innovation consistency checks bool consistentMagData; // true when the magnetometers are passing consistency checks bool motorsArmed; // true when the motors have been armed