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

AP_NavEKF3: pos timeout or glitch does not reset vel if fusing successfully 

vel is not reset if GPS velocity, optical flow, visual odometry or wheel
encoders are successfully being fused
This commit is contained in:
Randy Mackay 2020-10-26 10:49:30 +09:00 committed by Andrew Tridgell
parent 3b38aa6d7b
commit b726630ef4
2 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

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

11
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -670,11 +670,8 @@ void NavEKF3_core::FuseVelPosNED()
if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
// reset the position to the current external sensor position
ResetPosition(resetDataSource::DEFAULT);
// reset the velocity to the external sensor velocity
ResetVelocity(resetDataSource::DEFAULT);
// don't fuse external sensor data on this time step
fusePosData = false;
fuseVelData = false;
// Reset the position variances and corresponding covariances to a value that will pass the checks
zeroRows(P,7,8);
zeroCols(P,7,8);
@ -682,7 +679,13 @@ void NavEKF3_core::FuseVelPosNED()
P[8][8] = P[7][7];
// Reset the normalised innovation to avoid failing the bad fusion tests
posTestRatio = 0.0f;
velTestRatio = 0.0f;
// also reset velocity if it has timed out
if (velTimeout) {
// reset the velocity to the external sensor velocity
ResetVelocity(resetDataSource::DEFAULT);
fuseVelData = false;
velTestRatio = 0.0f;
}
}
} else {
posHealth = false;