aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Use common method for handling baro ground effect

Browse Source
This commit is contained in:
Paul Riseborough 2021-05-31 16:57:24 +10:00 committed by Randy Mackay
parent 77e566c6ed
commit 76d0dcc25c
2 changed files with 3 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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);

5
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -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:
// |/