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

AP_NavEKF: Fix bug preventing GPS vertical velocity being used 

This fixes a bug that prevents GPS velocity being used permanently if there is a temporary unavailability
This commit is contained in:
Paul Riseborough 2015-06-16 16:16:34 +10:00 committed by Andrew Tridgell
parent a1d478909a
commit 6ba8cd05ff
2 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -874,7 +874,7 @@ void NavEKF::SelectVelPosFusion()
// If we haven't received height data for a while, then declare the height data as being timed out
// set timeout period based on whether we have vertical GPS velocity available to constrain drift
hgtRetryTime = (_fusionModeGPS == 0 && !velTimeout) ? hgtRetryTimeMode0 : hgtRetryTimeMode12;
hgtRetryTime = (useGpsVertVel && !velTimeout) ? hgtRetryTimeMode0 : hgtRetryTimeMode12;
if (imuSampleTime_ms - lastHgtMeasTime > hgtRetryTime) {
hgtTimeout = true;
}
@ -1974,7 +1974,7 @@ void NavEKF::FuseVelPosNED()
// if vertical GPS velocity data is being used, check to see if the GPS vertical velocity and barometer
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
if (_fusionModeGPS == 0 && fuseVelData && (imuSampleTime_ms - lastHgtMeasTime) < (2 * msecHgtAvg)) {
if (useGpsVertVel && fuseVelData && (imuSampleTime_ms - lastHgtMeasTime) < (2 * msecHgtAvg)) {
// calculate innovations for height and vertical GPS vel measurements
float hgtErr = statesAtHgtTime.position.z - observation[5];
float velDErr = statesAtVelTime.velocity.z - observation[2];
@ -2124,7 +2124,7 @@ void NavEKF::FuseVelPosNED()
}
// set range for sequential fusion of velocity and position measurements depending on which data is available and its health
if (fuseVelData && _fusionModeGPS == 0 && velHealth && !constPosMode && PV_AidingMode == AID_ABSOLUTE) {
if (fuseVelData && useGpsVertVel && velHealth && !constPosMode && PV_AidingMode == AID_ABSOLUTE) {
fuseData[0] = true;
fuseData[1] = true;
fuseData[2] = true;
@ -4140,11 +4140,10 @@ void NavEKF::readGpsData()
}
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
if (!_ahrs->get_gps().have_vertical_velocity()) {
// vertical velocity should not be fused
if (_fusionModeGPS == 0) {
_fusionModeGPS = 1;
}
if (_ahrs->get_gps().have_vertical_velocity() && _fusionModeGPS == 0) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data for alignment
@ -4778,7 +4777,7 @@ void NavEKF::getFilterStatus(nav_filter_status &status) const
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool notDeadReckoning = !constVelMode && !constPosMode;
bool someVertRefData = (!velTimeout && (_fusionModeGPS == 0)) || !hgtTimeout;
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && (_fusionModeGPS == 3);
bool gpsNavPossible = !gpsNotAvailable && (_fusionModeGPS <= 2);

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -667,6 +667,7 @@ private:
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 scel factor errors could cause loss of heading reference
float yawRateFilt; // filtered yaw rate used to determine when the vehicle is doing rapid yaw rotation where gyro scel factor errors could cause loss of heading reference
bool useGpsVertVel; // true if GPS vertical velocity should be used
// 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