Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Fail absolute position status if GPS repeatedly rejected

Browse Source
This commit is contained in:
Paul Riseborough 2015-04-15 16:09:04 +10:00 committed by Randy Mackay
parent 77d3798278
commit 1008c6390c
2 changed files with 32 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -813,6 +813,10 @@ void NavEKF::SelectVelPosFusion()
decayGpsOffset();
ResetPosition();
ResetVelocity();
// record the fail time
lastPosFailTime = imuSampleTime_ms;
// Reset the normalised innovation to avoid false failing the bad position fusion test
posTestRatio = 0.0f;
}
} else {
fuseVelData = false;
@ -892,6 +896,21 @@ void NavEKF::SelectVelPosFusion()
states[i] += hgtIncrStateDelta[i];
}
}
// Detect and declare bad GPS aiding status for minimum 10 seconds if a GPS rejection occurs after
// rejection of GPS and reset to GPS position. This addresses failure case where errors cause ongoing rejection
// of GPS and severe loss of position accuracy.
uint32_t gpsRetryTime;
if (useAirspeed()) {
gpsRetryTime = gpsRetryTimeUseTAS;
} else {
gpsRetryTime = gpsRetryTimeNoTAS;
}
if ((posTestRatio > 2.0f) && ((imuSampleTime_ms - lastPosFailTime) < gpsRetryTime) && ((imuSampleTime_ms - lastPosFailTime) > gpsRetryTime/2) && fusePosData) {
lastGpsAidBadTime_ms = imuSampleTime_ms;
gpsAidingBad = true;
}
gpsAidingBad = gpsAidingBad && ((imuSampleTime_ms - lastGpsAidBadTime_ms) < 10000);
}
// select fusion of magnetometer data
@ -1992,6 +2011,10 @@ void NavEKF::FuseVelPosNED()
ResetVelocity();
// don't fuse data on this time step
fusePosData = false;
// record the fail time
lastPosFailTime = imuSampleTime_ms;
// Reset the normalised innovation to avoid false failing the bad position fusion test
posTestRatio = 0.0f;
}
}
} else {
@ -4428,6 +4451,7 @@ void NavEKF::InitialiseVariables()
lastAirspeedUpdate = 0;
lastVelPassTime = imuSampleTime_ms;
lastPosPassTime = imuSampleTime_ms;
lastPosFailTime = 0;
lastHgtPassTime = imuSampleTime_ms;
lastTasPassTime = imuSampleTime_ms;
lastStateStoreTime_ms = imuSampleTime_ms;
@ -4442,6 +4466,7 @@ void NavEKF::InitialiseVariables()
gndHgtValidTime_ms = 0;
ekfStartTime_ms = imuSampleTime_ms;
lastGpsVelFail_ms = 0;
lastGpsAidBadTime_ms = 0;
// initialise other variables
gpsNoiseScaler = 1.0f;
@ -4521,6 +4546,7 @@ void NavEKF::InitialiseVariables()
gndEffectMode = false;
gpsSpdAccuracy = 0.0f;
baroHgtOffset = 0.0f;
gpsAidingBad = false;
}
// return true if we should use the airspeed sensor
@ -4670,7 +4696,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 || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid
status.flags.horiz_pos_abs = doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid
status.flags.horiz_pos_abs = !gpsAidingBad && doingNormalGpsNav && notDeadReckoning && filterHealthy; // 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
@ -4771,6 +4797,8 @@ void NavEKF::performArmingChecks()
secondLastFixTime_ms = imuSampleTime_ms;
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime = imuSampleTime_ms;
// reset the fail time to prevent premature reporting of loss of position accruacy
lastPosFailTime = 0;
}
}
// Reset filter positon, height and velocity states on arming or disarming

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -583,6 +583,7 @@ private:
uint16_t hgtRetryTime; // time allowed without use of height measurements before a height timeout is declared
uint32_t lastVelPassTime; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_t lastPosPassTime; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastPosFailTime; // time stamp when GPS position measurement last failed innovation consistency check (msec)
uint32_t lastHgtPassTime; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint8_t storeIndex; // State vector storage index
@ -629,6 +630,8 @@ private:
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the UBlox GPS receiver
uint32_t lastGpsVelFail_ms; // time of last GPS vertical velocity consistency check fail
Vector3f lastMagOffsets; // magnetometer offsets returned by compass object from previous update
bool gpsAidingBad; // true when GPS position measurements have been consistently rejected by the filter
uint32_t lastGpsAidBadTime_ms; // time in msec gps aiding was last detected to be bad
// Used by smoothing of state corrections
Vector10 gpsIncrStateDelta; // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement