aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix vertical velocity reset

Browse Source 
Fixes bug that prevents the vertical velocity being reset to the GPS if the position aiding has already timed out and improves sensitivity of the bad IMU data check.
This commit is contained in:
Paul Riseborough 2021-09-21 11:47:51 +10:00 committed by Andrew Tridgell
parent 17c2ba5ac1
commit c0d88e2673
6 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -344,12 +344,12 @@ void NavEKF3_core::setAidingMode()
posTimeout = true;
velTimeout = true;
tasTimeout = true;
gpsNotAvailable = true;
gpsIsInUse = false;
} else if (posAidLossCritical) {
// if the loss of position is critical, declare all sources of position aiding as being timed out
posTimeout = true;
velTimeout = !optFlowUsed && !gpsVelUsed && !bodyOdmUsed;
gpsNotAvailable = true;
gpsIsInUse = false;
}
break;

4
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -692,8 +692,8 @@ void NavEKF3_core::readGpsData()
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
}
storedGPS.push(gpsDataNew);
// declare GPS available for use
gpsNotAvailable = false;
// declare GPS in use
gpsIsInUse = true;
}
}

2
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -76,7 +76,7 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
// don't fuse flow data if it exceeds validity limits
// don't update terrain offset if ground is being used as the zero height datum in the main filter
bool cantFuseFlowData = ((frontend->_flowUse != FLOW_USE_TERRAIN)
|| gpsNotAvailable
|| !gpsIsInUse
|| PV_AidingMode == AID_RELATIVE
|| velHorizSq < 25.0f
|| (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate));

10
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -243,8 +243,11 @@ void NavEKF3_core::ResetHeight(void)
// Reset the vertical velocity state using GPS vertical velocity if we are airborne
// Check that GPS vertical velocity data is available and can be used
if (inFlight && !gpsNotAvailable && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
gpsDataNew.have_vz) {
if (inFlight &&
(gpsIsInUse || badIMUdata) &&
frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS) &&
gpsDataNew.have_vz &&
(imuSampleTime_ms - gpsDataDelayed.time_ms < 500)) {
stateStruct.velocity.z = gpsDataNew.vel.z;
#if EK3_FEATURE_EXTERNAL_NAV
} else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
@ -1225,8 +1228,7 @@ void NavEKF3_core::selectHeightForFusion()
hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms ||
(badIMUdata &&
(imuSampleTime_ms - goodIMUdata_ms > BAD_IMU_DATA_TIMEOUT_MS) &&
(imuSampleTime_ms - lastPosResetD_ms > BAD_IMU_DATA_TIMEOUT_MS))) {
(imuSampleTime_ms - goodIMUdata_ms > BAD_IMU_DATA_TIMEOUT_MS))) {
hgtTimeout = true;
} else {
hgtTimeout = false;

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -290,7 +290,7 @@ void NavEKF3_core::InitialiseVariables()
velErrintegral.zero();
posErrintegral.zero();
gpsGoodToAlign = false;
gpsNotAvailable = true;
gpsIsInUse = false;
motorsArmed = false;
prevMotorsArmed = false;
memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus));

2
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1053,7 +1053,7 @@ private:
bool needMagBodyVarReset; // we need to reset mag body variances at next CovariancePrediction
bool needEarthBodyVarReset; // we need to reset mag earth variances at next CovariancePrediction
bool inhibitDelAngBiasStates; // true when IMU delta angle bias states are inactive
bool gpsNotAvailable; // bool true when valid GPS data is not available
bool gpsIsInUse; // bool true when GPS data is being used to correct states estimates
struct Location EKF_origin; // LLH origin of the NED axis system, internal only
struct Location &public_origin; // LLH origin of the NED axis system, public functions
bool validOrigin; // true when the EKF origin is valid