diff --git a/EKF/common.h b/EKF/common.h index 28ec23a303..998c0c9493 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -243,6 +243,7 @@ struct parameters { float wind_vel_p_noise_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec) float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) + float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) // initialization errors float switch_on_gyro_bias{0.1f}; ///< 1-sigma gyro bias uncertainty at switch on (rad/sec) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 380fe1ff6f..75fa5a84fc 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -166,7 +166,8 @@ void Ekf::fuseHagl() } else { // If we have been rejecting range data for too long, reset to measurement - if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) { + const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); + if (isTimedOut(_time_last_hagl_fuse, timeout)) { _terrain_vpos = _state.pos(2) + meas_hagl; _terrain_var = obs_variance;