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

AP_NavEKF2: Clean up loss of GPS logic

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-24 23:41:43 +10:00
parent df0eb9d9d7
commit 2fb72b6e6a
2 changed files with 71 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -81,8 +81,9 @@ bool NavEKF2_core::healthy(void) const
void NavEKF2_core::ResetPosition(void)
{
if (PV_AidingMode != AID_ABSOLUTE) {
stateStruct.position.x = 0;
stateStruct.position.y = 0;
// reset all position state history to the last known position
stateStruct.position.x = lastKnownPositionNE.x;
stateStruct.position.y = lastKnownPositionNE.y;
} else if (!gpsNotAvailable) {
// write to state vector and compensate for offset between last GPs measurement and the EKF time horizon
stateStruct.position.x = gpsDataNew.pos.x + gpsPosGlitchOffsetNE.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(lastTimeGpsReceived_ms));
@ -306,21 +307,6 @@ void NavEKF2_core::SelectVelPosFusion()
if (RecallGPS() && PV_AidingMode != AID_RELATIVE) {
fuseVelData = true;
fusePosData = true;
// If a long time since last GPS update, then reset position and velocity and reset stored state history
if ((imuSampleTime_ms - secondLastGpsTime_ms > 5000) && (PV_AidingMode == AID_ABSOLUTE)) {
// Apply an offset to the GPS position so that the position can be corrected gradually
gpsPosGlitchOffsetNE.x = stateStruct.position.x - gpsDataDelayed.pos.x;
gpsPosGlitchOffsetNE.y = stateStruct.position.y - gpsDataDelayed.pos.y;
// limit the radius of the offset to 100m and decay the offset to zero radially
decayGpsOffset();
ResetPosition();
ResetVelocity();
CovarianceInit();
// record the fail time
lastPosFailTime_ms = imuSampleTime_ms;
// Reset the normalised innovation to avoid false failing the bad position fusion test
posTestRatio = 0.0f;
}
} else {
fuseVelData = false;
fusePosData = false;
@ -349,21 +335,6 @@ void NavEKF2_core::SelectVelPosFusion()
if (!covPredStep) CovariancePrediction();
FuseVelPosNED();
}
// 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 = frontend.gpsRetryTimeUseTAS_ms;
} else {
gpsRetryTime = frontend.gpsRetryTimeNoTAS_ms;
}
if ((posTestRatio > 2.0f) && ((imuSampleTime_ms - lastPosFailTime_ms) < gpsRetryTime) && ((imuSampleTime_ms - lastPosFailTime_ms) > gpsRetryTime/2) && fusePosData) {
lastGpsAidBadTime_ms = imuSampleTime_ms;
gpsAidingBad = true;
}
gpsAidingBad = gpsAidingBad && ((imuSampleTime_ms - lastGpsAidBadTime_ms) < 10000);
}
// select fusion of magnetometer data
@ -477,7 +448,7 @@ void NavEKF2_core::SelectFlowFusion()
PV_AidingMode = AID_NONE;
// reset the velocity
ResetVelocity();
// store the current position to be used to keep reporting the last known position
// store the current position to be used to as a sythetic position measurement
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// reset the position
@ -1386,10 +1357,9 @@ void NavEKF2_core::FuseVelPosNED()
ResetVelocity();
// don't fuse data on this time step
fusePosData = false;
// record the fail time
lastPosFailTime_ms = imuSampleTime_ms;
// Reset the normalised innovation to avoid false failing the bad position fusion test
posTestRatio = 0.0f;
velTestRatio = 0.0f;
}
}
} else {
@ -1433,6 +1403,8 @@ void NavEKF2_core::FuseVelPosNED()
// if data is not healthy and timed out and position is unhealthy and we are using aiding, we reset the velocity, but do not fuse data on this time step
ResetVelocity();
fuseVelData = false;
// Reset the normalised innovation to avoid false failing the bad position fusion test
velTestRatio = 0.0f;
} else {
// if data is unhealthy and position is healthy, we do not fuse it
velHealth = false;
@ -3237,8 +3209,8 @@ bool NavEKF2_core::getPosNED(Vector3f &pos) const
return false;
} else {
// If no GPS fix is available, all we can do is provide the last known position
pos.x = outputDataNew.position.x + lastKnownPositionNE.x;
pos.y = outputDataNew.position.y + lastKnownPositionNE.y;
pos.x = outputDataNew.position.x;
pos.y = outputDataNew.position.y;
return false;
}
} else {
@ -3769,10 +3741,9 @@ void NavEKF2_core::readGpsData()
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
// We are by definition at the origin at the instant of alignment so set NE position to zero
gpsDataNew.pos.zero();
// If the vehicle is in flight (use arm status to determine) and GPS useage isn't explicitly prohibited, we switch to absolute position mode
// If GPS useage isn't explicitly prohibited, we switch to absolute position mode
if (filterArmed && frontend._fusionModeGPS != 3) {
PV_AidingMode = AID_ABSOLUTE;
gpsNotAvailable = false;
// Initialise EKF position and velocity states
ResetPosition();
ResetVelocity();
@ -3789,17 +3760,70 @@ void NavEKF2_core::readGpsData()
gpsNotAvailable = false;
}
// If not aiding we synthesise the GPS measurements at a zero position
if (PV_AidingMode == AID_NONE) {
// We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon
// If that happens we need to put the filter into a constant position mode, reset the velocity states to zero
// and use the last estimated position as a synthetic GPS position
// check if we can use opticalflow as a backup
bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend.gpsRetryTimeUseTAS_ms : frontend.gpsRetryTimeNoTAS_ms;
// Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend.gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
// If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
// Let other processes know that GPS i snota vailable and that a timeout has occurred
posTimeout = true;
velTimeout = true;
gpsNotAvailable = true;
// If we are currently reliying on GPS for navigation, then we need to switch to a non-GPS mode of operation
if (PV_AidingMode == AID_ABSOLUTE) {
// If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
// If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
if (!useAirspeed() && !assume_zero_sideslip()) {
if (optFlowBackupAvailable) {
// we can do optical flow only nav
frontend._fusionModeGPS = 3;
PV_AidingMode = AID_RELATIVE;
} else {
// store the current position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// put the filter into constant position mode
PV_AidingMode = AID_NONE;
// reset all glitch states
gpsPosGlitchOffsetNE.zero();
gpsVelGlitchOffset.zero();
// Reset the velocity and position states
ResetVelocity();
ResetPosition();
// Reset the normalised innovation to avoid false failing the bad position fusion test
velTestRatio = 0.0f;
posTestRatio = 0.0f;
}
}
}
}
// If not aiding we synthesise the GPS measurements at the last known position
if (PV_AidingMode == AID_NONE) {
if (imuSampleTime_ms - gpsDataNew.time_ms > 200) {
gpsDataNew.pos.zero();
gpsDataNew.pos.x = lastKnownPositionNE.x;
gpsDataNew.pos.y = lastKnownPositionNE.y;
gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
// save measurement to buffer to be fused later
StoreGPS();
}
} else if (PV_AidingMode == AID_RELATIVE) {
gpsNotAvailable = true;
}
}
@ -4143,7 +4167,6 @@ void NavEKF2_core::InitialiseVariables()
lastHgtReceived_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
lastPosPassTime_ms = imuSampleTime_ms;
lastPosFailTime_ms = 0;
lastHgtPassTime_ms = imuSampleTime_ms;
lastTasPassTime_ms = imuSampleTime_ms;
lastTimeGpsReceived_ms = 0;
@ -4219,7 +4242,6 @@ void NavEKF2_core::InitialiseVariables()
expectGndEffectTouchdown = false;
gpsSpdAccuracy = 0.0f;
baroHgtOffset = 0.0f;
gpsAidingBad = false;
highYawRate = false;
yawRateFilt = 0.0f;
yawResetAngle = 0.0f;
@ -4238,6 +4260,7 @@ void NavEKF2_core::InitialiseVariables()
delVelCorrection.zero();
velCorrection.zero();
gpsQualGood = false;
gpsNotAvailable = true;
}
@ -4393,7 +4416,7 @@ void NavEKF2_core::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 = 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 = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
@ -4481,7 +4504,7 @@ void NavEKF2_core::performArmingChecks()
heldVelNE.zero();
// reset the flag that indicates takeoff for use by optical flow navigation
takeOffDetected = false;
// set various useage modes based on the condition at arming. These are then held until the vehicle is disarmed.
// set various useage modes based on the condition at arming. These are then held until the filter is disarmed.
if (!filterArmed) {
PV_AidingMode = AID_NONE; // When dis-armed, we only estimate orientation & height using the constant position mode
posTimeout = true;
@ -4533,8 +4556,6 @@ void NavEKF2_core::performArmingChecks()
secondLastGpsTime_ms = imuSampleTime_ms;
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime_ms = imuSampleTime_ms;
// reset the fail time to prevent premature reporting of loss of position accruacy
lastPosFailTime_ms = 0;
}
}
// Reset all position, velocity and covariance

2
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -622,7 +622,6 @@ private:
uint16_t hgtRetryTime_ms; // time allowed without use of height measurements before a height timeout is declared
uint32_t lastVelPassTime_ms; // time stamp when GPS velocity measurement last passed innovation consistency check (msec)
uint32_t lastPosPassTime_ms; // time stamp when GPS position measurement last passed innovation consistency check (msec)
uint32_t lastPosFailTime_ms; // time stamp when GPS position measurement last failed innovation consistency check (msec)
uint32_t lastHgtPassTime_ms; // time stamp when height measurement last passed innovation consistency check (msec)
uint32_t lastTasPassTime_ms; // time stamp when airspeed measurement last passed innovation consistency check (msec)
uint32_t lastTimeGpsReceived_ms;// last time we recieved GPS data
@ -659,7 +658,6 @@ 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
float posDownAtArming; // flight vehicle vertical position at arming used as a reference point
bool highYawRate; // true when the vehicle is doing rapid yaw rotation where gyro scale factor errors could cause loss of heading reference