Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Start EKF-GSF yaw estimator before fixed wing takeoff

Browse Source
This commit is contained in:
Paul Riseborough 2020-06-13 17:45:00 +10:00 committed by Peter Barker
parent b469a80633
commit d1a0c2eb30
3 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

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

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

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

6
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

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