Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -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 // 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 // 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); 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 // filtered baro data used to provide a reference for takeoff
// it is is reset to last height measurement on disarming in performArmingChecks() // 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 gndHgtFiltTC = 0.5f;
const float dtBaro = frontend->hgtAvg_ms*1.0e-3f; const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f); float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
@ -1132,12 +1132,12 @@ void NavEKF3_core::selectHeightForFusion()
// set the observation noise // set the observation noise
posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f)); 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 // 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; 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 // 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 // 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); hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
} }
velPosObs[5] = -hgtMea; velPosObs[5] = -hgtMea;

6
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -412,7 +412,7 @@ void NavEKF3_core::detectFlight()
if (!prevOnGround && onGround) { if (!prevOnGround && onGround) {
// landed so disable filter bank // landed so disable filter bank
EKFGSF_run_filterbank = false; 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 // started flying so reset counters and enable filter bank
EKFGSF_yaw_reset_ms = 0; EKFGSF_yaw_reset_ms = 0;
EKFGSF_yaw_reset_request_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 rotor wash ground interaction
// determine if a takeoff is expected so that we can compensate for expected barometer errors due to ground effect // also used when in fixed wing mode so that GSG yaw estimator can be started before throw or takeoff roll
bool NavEKF3_core::getTakeoffExpected() bool NavEKF3_core::getTakeoffExpected()
{ {
if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) { if (expectGndEffectTakeoff && imuSampleTime_ms - takeoffExpectedSet_ms > frontend->gndEffectTimeout_ms) {