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

AP_NavEKF: Decouple takeoff ground effect compensation from arm transtion

Browse Source 
Thsi fixes a potential bug where the vehicle could land at a lower location without disarming and re-enter ground effect takeoff mode wiht a baro height floor above the current altitude, causing unpredictable height gain
This commit is contained in:
Paul Riseborough 2015-04-28 07:02:23 +10:00 committed by Randy Mackay
parent c4c0f819b5
commit 2cba60c731
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4177,8 +4177,8 @@ void NavEKF::readHgtData()
// 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 (!vehicleArmed || !getTakeoffExpected()) { if (!getTakeoffExpected()) {
static const float gndHgtFiltTC = 1.0f; static const float gndHgtFiltTC = 0.5f;
static const float dtBaro = msecHgtAvg*1.0e-3f; static const float dtBaro = msecHgtAvg*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);
meaHgtAtTakeOff += (hgtMea-meaHgtAtTakeOff)*alpha; meaHgtAtTakeOff += (hgtMea-meaHgtAtTakeOff)*alpha;