Skip to content
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -696,14 +696,13 @@ void NavEKF3_core::SelectVelPosFusion()
}
} else {
fusePosData = true;
// When stationary on ground or armed before takeoff, fuse zero velocity
// When stationary on ground, fuse zero velocity
// to constrain gyro bias and Z-axis accel bias learning. XY accel biases
// remain unobservable until the vehicle accelerates and are separately
// inhibited by dvelBiasAxisInhibit in CovariancePrediction.
// Use onGroundNotMoving to avoid fusing zero velocity when the vehicle
// is being moved (e.g. on a boat or carried by hand).
// takeoff_expected covers the armed-on-ground case before liftoff.
const bool onGroundNotFlying = onGroundNotMoving || dal.get_takeoff_expected();
const bool onGroundNotFlying = onGroundNotMoving;
if (onGroundNotFlying && tiltAlignComplete) {
fuseVelData = true;
fusingStationaryZeroVel = true;
Expand Down
Loading