Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Remove unnecessary state resets on arm and disarm

Browse Source 
Resetting states unnecessarily creates transients due to presence of bias errors
This commit is contained in:
Paul Riseborough 2015-04-18 20:12:49 +10:00 committed by Randy Mackay
parent 3e67080002
commit fd7fdc1ad9
1 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4822,12 +4822,18 @@ void NavEKF::performArmingChecks()
lastPosFailTime = 0;
}
}
// Reset filter positon, height and velocity states on arming or disarming
ResetVelocity();
ResetPosition();
baroHgtOffset = 0.0f;
ResetHeight();
StoreStatesReset();
if (vehicleArmed) {
// Reset filter position to GPS when transitioning into flight mode
// We need to do this becasue the vehicle may have moved since the EKF origin was set
ResetPosition();
StoreStatesReset();
} else {
// Reset all position and velocity states when transitioning out of flight mode
// We need to do this becasue we are going into a mode that assumes zero position and velocity
ResetVelocity();
ResetPosition();
StoreStatesReset();
}
} else if (vehicleArmed && !firstMagYawInit && state.position.z < -1.5f && !assume_zero_sideslip()) {
// Do the first in-air yaw and earth mag field initialisation when the vehicle has gained 1.5m of altitude after arming if it is a non-fly forward vehicle (vertical takeoff)