Browse Source

AP_NavEKF2: don't limit baro change when we are in fixed wing mode

zr-v5.1
Andrew Tridgell 4 years ago committed by Randy Mackay
parent
commit
ea6724aca5
  1. 2
      libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
  2. 2
      libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@ -672,7 +672,7 @@ void NavEKF2_core::readBaroData() @@ -672,7 +672,7 @@ void NavEKF2_core::readBaroData()
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
if (dal.get_takeoff_expected()) {
if (dal.get_takeoff_expected() && !assume_zero_sideslip()) {
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
}

2
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@ -1096,7 +1096,7 @@ void NavEKF2_core::selectHeightForFusion() @@ -1096,7 +1096,7 @@ void NavEKF2_core::selectHeightForFusion()
}
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
if (motorsArmed && dal.get_takeoff_expected()) {
if (motorsArmed && dal.get_takeoff_expected() && !assume_zero_sideslip()) {
hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
}
} else {

Loading…
Cancel
Save