mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-26 18:48:30 -04:00
AP_NavEKF: fixed static const bug
This commit is contained in:
parent
02d7867d79
commit
d24f6f4050
@ -4058,8 +4058,8 @@ void NavEKF_core::readHgtData()
|
||||
// filtered baro data used to provide a reference for takeoff
|
||||
// it is is reset to last height measurement on disarming in performArmingChecks()
|
||||
if (!getTakeoffExpected()) {
|
||||
static const float gndHgtFiltTC = 0.5f;
|
||||
static const float dtBaro = msecHgtAvg*1.0e-3f;
|
||||
const float gndHgtFiltTC = 0.5f;
|
||||
const float dtBaro = msecHgtAvg*1.0e-3f;
|
||||
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
||||
meaHgtAtTakeOff += (hgtMea-meaHgtAtTakeOff)*alpha;
|
||||
} else if (vehicleArmed && getTakeoffExpected()) {
|
||||
|
Loading…
Reference in New Issue
Block a user