Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Fix bugs and consolidate aiding switch logic 

Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.
This commit is contained in:
priseborough 2016-07-15 13:09:48 +10:00 committed by Andrew Tridgell
parent 18c4327a2a
commit 9a23152ee4
6 changed files with 82 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp
View File

@ -157,33 +157,82 @@ void NavEKF2_core::setWindMagStateLearningMode()
// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
// Determine when to commence aiding for inertial navigation
// Save the previous status so we can detect when it has changed
prevIsAiding = isAiding;
// perform aiding checks if not aiding
if (!isAiding) {
PV_AidingModePrev = PV_AidingMode;
// Determine if we should start aiding
if (PV_AidingMode == AID_NONE) {
// 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 && 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
// Latch to on
isAiding = (readyToUseGPS() || useFlowAiding) && filterIsStable;
// GPS aiding is the perferred option unless excluded by the user
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit) {
PV_AidingMode = AID_ABSOLUTE;
} else if ((frontend->_fusionModeGPS == 3) && optFlowDataPresent()) {
PV_AidingMode = AID_RELATIVE;
}
}
// Determine if we should exit optical flow aiding
if (PV_AidingMode == AID_RELATIVE) {
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
if (flowSensorTimeout || flowFusionTimeout) {
PV_AidingMode = AID_NONE;
}
}
// Determine if we should exit GPS aiding
if (PV_AidingMode == AID_ABSOLUTE) {
// 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 is not available and that a timeout has occurred
posTimeout = true;
velTimeout = true;
gpsNotAvailable = true;
// If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
// 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) {
// attempt optical flow navigation
PV_AidingMode = AID_RELATIVE;
} else {
// put the filter into constant position mode
PV_AidingMode = AID_NONE;
}
}
} else if (gpsInhibit) {
// put the filter into constant position mode in response to an exernal request
PV_AidingMode = AID_NONE;
}
}
// check to see if we are starting or stopping aiding and set states and modes as required
if (isAiding != prevIsAiding) {
// We have transitioned either into or out of aiding
// zero stored velocities used to do dead-reckoning
heldVelNE.zero();
if (PV_AidingMode != PV_AidingModePrev) {
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
if (!isAiding) {
if (PV_AidingMode == AID_NONE) {
// We have ceased aiding
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
// Reset the normalised innovation to avoid false failing bad fusion tests
velTestRatio = 0.0f;
posTestRatio = 0.0f;
// store the current position to be used to keep reporting the last known position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
@ -192,20 +241,18 @@ void NavEKF2_core::setAidingMode()
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct.position.z = -meaHgtAtTakeOff;
} else if (frontend->_fusionModeGPS == 3) {
} else if (PV_AidingMode == AID_RELATIVE) {
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
hal.console->printf("EKF2 IMU%u is using optical flow\n",(unsigned)imu_index);
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
posTimeout = true;
velTimeout = true;
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
} else {
} else if (PV_AidingMode == AID_ABSOLUTE) {
// We have commenced aiding and GPS usage is allowed
hal.console->printf("EKF2 IMU%u is using GPS\n",(unsigned)imu_index);
PV_AidingMode = AID_ABSOLUTE; // we have GPS data and can estimate all vehicle states
posTimeout = false;
velTimeout = false;
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
@ -215,18 +262,13 @@ void NavEKF2_core::setAidingMode()
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime_ms = imuSampleTime_ms;
}
// Reset the position and velocity
// Always reset the position and velocity when changing mode
ResetVelocity();
ResetPosition();
}
// Always turn aiding off when the vehicle is disarmed
if (!isAiding) {
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
}
}
// Check the tilt and yaw alignmnent status
@ -299,7 +341,7 @@ bool NavEKF2_core::assume_zero_sideslip(void) const
// set the LLH location of the filters NED origin
bool NavEKF2_core::setOriginLLH(struct Location &loc)
{
if (isAiding) {
if (PV_AidingMode == AID_ABSOLUTE) {
return false;
}
EKF_origin = loc;
@ -346,16 +388,13 @@ bool NavEKF2_core::checkGyroCalStatus(void)
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t NavEKF2_core::setInhibitGPS(void)
{
if(!isAiding) {
if(PV_AidingMode == AID_ABSOLUTE && motorsArmed) {
return 0;
}
if (optFlowDataPresent()) {
frontend->_fusionModeGPS = 3;
//#error writing to a tuning parameter
return 2;
} else {
gpsInhibit = true;
return 1;
}
// option 2 is not yet implemented as it requires a deeper integration of optical flow and GPS operation
}
// Update the filter status

50
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -454,54 +454,6 @@ void NavEKF2_core::readGpsData()
gpsCheckStatus.bad_fix = true;
}
}
// 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 is not available and that a timeout has occurred
posTimeout = true;
velTimeout = true;
gpsNotAvailable = true;
// If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
// 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 (PV_AidingMode == AID_ABSOLUTE && !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 the velocity and position states
ResetVelocity();
ResetPosition();
// Reset the normalised innovation to avoid false failing bad fusion tests
velTestRatio = 0.0f;
posTestRatio = 0.0f;
}
}
}
}
// read the delta angle and corresponding time interval from the IMU
@ -534,7 +486,7 @@ void NavEKF2_core::readBaroData()
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
if (isAiding && getTakeoffExpected()) {
if (getTakeoffExpected()) {
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
}

20
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -39,33 +39,17 @@ void NavEKF2_core::SelectFlowFusion()
// Perform Data Checks
// Check if the optical flow data is still valid
flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// check is the terrain offset estimate is still valid
gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000);
// Perform tilt check
bool tiltOK = (Tnb_flow.c.z > frontend->DCM33FlowMin);
// Constrain measurements to zero if we are using optical flow and are on the ground
if (frontend->_fusionModeGPS == 3 && !takeOffDetected && isAiding) {
// Constrain measurements to zero if we are on the ground
if (frontend->_fusionModeGPS == 3 && !takeOffDetected) {
ofDataDelayed.flowRadXYcomp.zero();
ofDataDelayed.flowRadXY.zero();
flowDataValid = true;
}
// If the flow measurements have been rejected for too long and we are relying on them, then revert to constant position mode
if ((flowSensorTimeout || flowFusionTimeout) && PV_AidingMode == AID_RELATIVE) {
PV_AidingMode = AID_NONE;
// reset the velocity
ResetVelocity();
// 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
ResetPosition();
}
// if we do have valid flow measurements, fuse data into a 1-state EKF to estimate terrain height
// we don't do terrain height estimation in optical flow only mode as the ground becomes our zero height reference
if ((flowDataToFuse || rangeDataToFuse) && tiltOK) {

4
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -27,7 +27,7 @@ void NavEKF2_core::ResetVelocity(void)
if (PV_AidingMode != AID_ABSOLUTE) {
stateStruct.velocity.zero();
} else if (!gpsNotAvailable) {
} else {
// reset horizontal velocity states to the GPS velocity
stateStruct.velocity.x = gpsDataNew.vel.x; // north velocity from blended accel data
stateStruct.velocity.y = gpsDataNew.vel.y; // east velocity from blended accel data
@ -68,7 +68,7 @@ void NavEKF2_core::ResetPosition(void)
// reset all position state history to the last known position
stateStruct.position.x = lastKnownPositionNE.x;
stateStruct.position.y = lastKnownPositionNE.y;
} else if (!gpsNotAvailable) {
} else {
// write to state vector and compensate for offset between last GPs measurement and the EKF time horizon
stateStruct.position.x = gpsDataNew.pos.x + 0.001f*gpsDataNew.vel.x*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));
stateStruct.position.y = gpsDataNew.pos.y + 0.001f*gpsDataNew.vel.y*(float(imuDataDelayed.time_ms) - float(gpsDataNew.time_ms));

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -176,10 +176,9 @@ void NavEKF2_core::InitialiseVariables()
flowGyroBias.y = 0;
heldVelNE.zero();
PV_AidingMode = AID_NONE;
PV_AidingModePrev = AID_NONE;
posTimeout = true;
velTimeout = true;
isAiding = false;
prevIsAiding = false;
memset(&faultStatus, 0, sizeof(faultStatus));
hgtRate = 0.0f;
mag_state.q0 = 1;
@ -264,6 +263,7 @@ void NavEKF2_core::InitialiseVariables()
magFieldLearned = false;
delAngBiasLearned = false;
memset(&filterStatus, 0, sizeof(filterStatus));
gpsInhibit = false;
// zero data buffers
storedIMU.reset();

4
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -733,8 +733,6 @@ private:
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool gpsNotAvailable; // bool true when valid GPS data is not available
bool isAiding; // true when the filter is fusing position, velocity or flow measurements
bool prevIsAiding; // isAiding from previous frame
struct Location EKF_origin; // LLH origin of the NED axis system - do not change unless filter is reset
bool validOrigin; // true when the EKF origin is valid
float gpsSpdAccuracy; // estimated speed accuracy in m/s returned by the GPS receiver
@ -884,6 +882,8 @@ private:
AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative
};
AidingMode PV_AidingMode; // Defines the preferred mode for aiding of velocity and position estimates from the INS
AidingMode PV_AidingModePrev; // Value of PV_AidingMode from the previous frame - used to detect transitions
bool gpsInhibit; // externally set flag informing the EKF not to use the GPS
bool gndOffsetValid; // true when the ground offset state can still be considered valid
Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
float delTimeOF; // time that delAngBodyOF is summed across