mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
AP_NavEKF: Fixed bug in pos/vel/hgt reset on timout
This commit is contained in:
parent
f74cce8b4e
commit
28fb5e364b
@ -1584,6 +1584,7 @@ void NavEKF::FuseVelPosNED()
|
||||
if (velTimeout)
|
||||
{
|
||||
ResetVelocity();
|
||||
fuseVelData = false;
|
||||
}
|
||||
|
||||
}
|
||||
@ -1609,6 +1610,7 @@ void NavEKF::FuseVelPosNED()
|
||||
if (posTimeout)
|
||||
{
|
||||
ResetPosition();
|
||||
fusePosData = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -1636,6 +1638,7 @@ void NavEKF::FuseVelPosNED()
|
||||
if (hgtTimeout)
|
||||
{
|
||||
ResetHeight();
|
||||
fuseHgtData = false;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
Loading…
Reference in New Issue
Block a user