From 76d0dcc25c4ec2ac2d4a172065e0362c57a5943d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 31 May 2021 16:57:24 +1000 Subject: [PATCH] AP_NavEKF3: Use common method for handling baro ground effect --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 ------ libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 5 +++-- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 416368549c..d2c3bdec8d 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -735,12 +735,6 @@ void NavEKF3_core::readBaroData() baroDataNew.hgt = baro.get_altitude(selected_baro); - // 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 rotor wash ground interaction corrupting the EKF altitude during initial ascent - if (dal.get_takeoff_expected() && !assume_zero_sideslip()) { - baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff); - } - // time stamp used to check for new measurement lastBaroReceived_ms = baro.get_last_update(selected_baro); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index a3ce94b4ef..f6bc6f24af 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -851,8 +851,9 @@ void NavEKF3_core::FuseVelPosNED() const float gndMaxBaroErr = 4.0f; const float gndBaroInnovFloor = -0.5f; - if (dal.get_touchdown_expected() && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) { - // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor + if ((dal.get_touchdown_expected() || dal.get_takeoff_expected()) && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) { + // when baro positive pressure error due to ground effect is expected, + // floor the barometer innovation at gndBaroInnovFloor // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr // this function looks like this: // |/