diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 5490c134e5..ec8b6bb424 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4284,6 +4284,9 @@ void NavEKF::readGpsData() // Monitor quality of the GPS velocity data for alignment gpsGoodToAlign = calcGpsGoodToAlign(); + // Monitor qulaity of GPS data inflight + calcGpsGoodForFlight(); + // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin // If we don't have an origin, then set it to the current GPS coordinates const struct Location &gpsloc = _ahrs->get_gps().location(); @@ -4953,7 +4956,7 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && (_fusionModeGPS == 3); - bool gpsNavPossible = !gpsNotAvailable && (_fusionModeGPS <= 2) && gpsGoodToAlign; + bool gpsNavPossible = !gpsNotAvailable && (_fusionModeGPS <= 2) && gpsGoodToAlign && gpsAccuracyGood; bool filterHealthy = healthy(); bool gyroHealthy = checkGyroHealthPreFlight(); @@ -4962,7 +4965,7 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const status.flags.horiz_vel = someHorizRefData && notDeadReckoning && filterHealthy; // horizontal velocity estimate valid status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid - status.flags.horiz_pos_abs = !gpsAidingBad && doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid + status.flags.horiz_pos_abs = !gpsAidingBad && doingNormalGpsNav && notDeadReckoning && filterHealthy && gpsAccuracyGood; // absolute horizontal position estimate valid status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid status.flags.const_pos_mode = constPosMode && filterHealthy; // constant position mode @@ -5531,4 +5534,66 @@ bool NavEKF::getGpsGlitchStatus(void) const return !gpsAccuracyGood; } +// update inflight calculaton that determines if GPS data is good enough for reliable navigation +void NavEKF::calcGpsGoodForFlight(void) +{ + // use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks + static bool gpsSpdAccPass = false; + static bool ekfInnovationsPass = false; + + // set up varaibles and constants used by filter that is applied to GPS speed accuracy + const float alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data + const float tau = 10.0f; // time constant (sec) of peak hold decay + static float lpfFilterState = 0.0f; // first stage LPF filter state + static float peakHoldFilterState = 0.0f; // peak hold with exponential decay filter state + static uint32_t lastTime_ms = 0; + if (lastTime_ms == 0) { + lastTime_ms = imuSampleTime_ms; + } + float dtLPF = (imuSampleTime_ms - lastTime_ms) * 1e-3f; + lastTime_ms = imuSampleTime_ms; + float alpha2 = constrain_float(dtLPF/tau,0.0f,1.0f); + + // get the receivers reported speed accuracy + float gpsSpdAccRaw; + if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) { + gpsSpdAccRaw = 0.0f; + } + + // filter the raw speed accuracy using a LPF + lpfFilterState = constrain_float((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * lpfFilterState),0.0f,10.0f); + + // apply a peak hold filter to the LPF output + peakHoldFilterState = max(lpfFilterState,((1.0f - alpha2) * peakHoldFilterState)); + + // Apply a threshold test with hysteresis to the filtered GPS speed accuracy data + if (peakHoldFilterState > 1.5f ) { + gpsSpdAccPass = false; + } else if(peakHoldFilterState < 1.0f) { + gpsSpdAccPass = true; + } + + // Apply a threshold test with hysteresis to the normalised position and velocity innovations + // Require a fail for one second and a pass for 10 seconds to transition + static uint32_t lastInnovPassTime_ms = 0; + static uint32_t lastInnovFailTime_ms = 0; + if (lastInnovFailTime_ms == 0) { + lastInnovFailTime_ms = imuSampleTime_ms; + lastInnovPassTime_ms = imuSampleTime_ms; + } + if (velTestRatio < 1.0f && posTestRatio < 1.0f) { + lastInnovPassTime_ms = imuSampleTime_ms; + } else if (velTestRatio > 0.7f || posTestRatio > 0.7f) { + lastInnovFailTime_ms = imuSampleTime_ms; + } + if ((imuSampleTime_ms - lastInnovPassTime_ms) > 1000) { + ekfInnovationsPass = false; + } else if ((imuSampleTime_ms - lastInnovFailTime_ms) > 10000) { + ekfInnovationsPass = true; + } + + // both GPS speed accuracy and EKF innovations must pass + gpsAccuracyGood = gpsSpdAccPass && ekfInnovationsPass; +} + #endif // HAL_CPU_CLASS diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 186c20d95c..04e693c626 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -481,6 +481,9 @@ private: // Check for signs of bad gyro health before flight bool checkGyroHealthPreFlight(void) const; + // update inflight calculaton that determines if GPS data is good enough for reliable navigation + void calcGpsGoodForFlight(void); + // EKF Mavlink Tuneable Parameters AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s AP_Float _gpsVertVelNoise; // GPS vertical velocity measurement noise : m/s