diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index aa2a169813..cc464f1046 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -707,7 +707,7 @@ void NavEKF3_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 (getTakeoffExpected()) { + if (getTakeoffExpected() && !assume_zero_sideslip()) { baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff); } diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index c8ab996abe..585b776980 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1071,7 +1071,7 @@ void NavEKF3_core::selectHeightForFusion() } // filtered baro data used to provide a reference for takeoff // it is is reset to last height measurement on disarming in performArmingChecks() - if (!getTakeoffExpected()) { + if (!getTakeoffExpected() || !assume_zero_sideslip()) { const float gndHgtFiltTC = 0.5f; const float dtBaro = frontend->hgtAvg_ms*1.0e-3f; float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); @@ -1132,12 +1132,12 @@ void NavEKF3_core::selectHeightForFusion() // set the observation noise posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f)); // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect - if (getTakeoffExpected() || getTouchdownExpected()) { + if ((getTakeoffExpected() && !assume_zero_sideslip()) || getTouchdownExpected()) { posDownObsNoise *= frontend->gndEffectBaroScaler; } // 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 && getTakeoffExpected()) { + if (motorsArmed && (getTakeoffExpected() && !assume_zero_sideslip())) { hgtMea = MAX(hgtMea, meaHgtAtTakeOff); } velPosObs[5] = -hgtMea; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index ac82c39a11..e245c9127f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -412,7 +412,7 @@ void NavEKF3_core::detectFlight() if (!prevOnGround && onGround) { // landed so disable filter bank EKFGSF_run_filterbank = false; - } else if (!prevInFlight && inFlight) { + } else if (!EKFGSF_run_filterbank && ((!prevInFlight && inFlight) || getTakeoffExpected())) { // started flying so reset counters and enable filter bank EKFGSF_yaw_reset_ms = 0; EKFGSF_yaw_reset_request_ms = 0; @@ -429,8 +429,8 @@ void NavEKF3_core::detectFlight() } - -// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect +// determine if a takeoff is expected so that we can compensate for expected barometer errors due to rotor wash ground interaction +// also used when in fixed wing mode so that GSG yaw estimator can be started before throw or takeoff roll bool NavEKF3_core::getTakeoffExpected() { if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) {