AP_NavEKF: Prevent touchdown baro errors tripping height innovation check
Ground effect baro errors can cause a spike in height innovation on disarming if ground effect compensation was used during the landing. This causes a transient AHRS fault message if this innovation is outside the pre-arm check limits. Resetting the vertical position state to the measured height after disarming prevents this.
This commit is contained in:
parent
2f38dd1b67
commit
8dc6354a54
@ -4809,6 +4809,8 @@ void NavEKF::performArmingChecks()
|
||||
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
|
||||
// this reduces the time required for the filter to settle before the estimate can be used
|
||||
meaHgtAtTakeOff = hgtMea;
|
||||
// reset the vertical position state to faster recover from baro errors experienced during touchdown
|
||||
state.position.z = -hgtMea;
|
||||
} else if (_fusionModeGPS == 3) { // arming when GPS useage has been prohibited
|
||||
if (optFlowDataPresent()) {
|
||||
PV_AidingMode = AID_RELATIVE; // we have optical flow data and can estimate all vehicle states
|
||||
|
Loading…
Reference in New Issue
Block a user