Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Ensure postion timeout flag and timer is always reset

Browse Source
This commit is contained in:
Paul Riseborough 2021-06-18 08:31:15 +10:00 committed by Andrew Tridgell
parent 43e84c4061
commit 18e949d035
1 changed files with 4 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -100,9 +100,6 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
stateStruct.position.y = gps_corrected.pos.y + 0.001f*gps_corrected.vel.y*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
// set the variances using the position measurement noise parameter
P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
// clear the timeout flags and counters
posTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::RNGBCN) {
// use the range beacon data as a second preference
stateStruct.position.x = receiverPos.x;
@ -110,9 +107,6 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
// set the variances from the beacon alignment filter
P[7][7] = receiverPosCov[0][0];
P[8][8] = receiverPosCov[1][1];
// clear the timeout flags and counters
rngBcnTimeout = false;
lastRngBcnPassTime_ms = imuSampleTime_ms;
#if EK3_FEATURE_EXTERNAL_NAV
} else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250 && posResetSource == resetDataSource::DEFAULT) || posResetSource == resetDataSource::EXTNAV) {
// use external nav data as the third preference
@ -120,9 +114,6 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
stateStruct.position.y = extNavDataDelayed.pos.y;
// set the variances as received from external nav system data
P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr);
// clear the timeout flags and counters
posTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms;
#endif // EK3_FEATURE_EXTERNAL_NAV
}
}
@ -141,6 +132,10 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
// clear the timeout flags and counters
posTimeout = false;
lastPosPassTime_ms = imuSampleTime_ms;
}
// reset the stateStruct's NE position to the specified position
@ -711,7 +706,6 @@ void NavEKF3_core::FuseVelPosNED()
// Always fuse data if bad IMU to prevent aliasing and clipping pulling the state estimate away
// from the measurement un-opposed if test threshold is exceeded.
if (posCheckPassed || posTimeout || badIMUdata) {
lastPosPassTime_ms = imuSampleTime_ms;
// if timed out or outside the specified uncertainty radius, reset to the external sensor
if (posTimeout || ((P[8][8] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
// reset the position to the current external sensor position