Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: don't reset to stale GPS or ExtNav vel data

Browse Source
This commit is contained in:
Randy Mackay
2026-06-09 09:40:37 +10:00
committed by Andrew Tridgell
parent fe6e489750
commit 03df252925
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+2 -2
View File
@@ -46,7 +46,7 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
} else {
// reset horizontal velocity states to the GPS velocity if available
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::GPS) {
if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250) && (velResetSource == resetDataSource::DEFAULT || velResetSource == resetDataSource::GPS)) {
// correct for antenna position
gps_elements gps_corrected = gpsDataNew;
CorrectGPSForAntennaOffset(gps_corrected);
@@ -55,7 +55,7 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
// set the variances using the reported GPS speed accuracy
P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
#if EK3_FEATURE_EXTERNAL_NAV
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) {
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250) && (velResetSource == resetDataSource::DEFAULT || velResetSource == resetDataSource::EXTNAV)) {
// use external nav data as the 2nd preference
// already corrected for sensor position
stateStruct.velocity.x = extNavVelDelayed.vel.x;