From 6480fcc3d54433f04174edcfaec145b4d40ab553 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 5 Jan 2017 12:19:53 +0100 Subject: [PATCH] terrain estimator: fixed computation of filtered time since last range update - do not use hrt to compute delta time - limit filter state - do not use static variables Signed-off-by: Roman --- EKF/ekf.cpp | 4 +++- EKF/ekf.h | 4 ++++ EKF/terrain_estimator.cpp | 16 ++++++---------- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 37e5c9c511..64d2354649 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -62,6 +62,7 @@ const float Ekf::_k_earth_rate = 0.000072921f; const float Ekf::_gravity_mss = 9.80665f; Ekf::Ekf(): + _dt_update(0.01f), _filter_initialised(false), _earth_rate_initialised(false), _fuse_height(false), @@ -115,6 +116,8 @@ Ekf::Ekf(): _hagl_innov_var(0.0f), _time_last_hagl_fuse(0), _terrain_initialised(false), + _range_data_continuous(false), + _dt_last_range_update_filt_us(0.0f), _baro_hgt_faulty(false), _gps_hgt_faulty(false), _rng_hgt_faulty(false), @@ -183,7 +186,6 @@ bool Ekf::init(uint64_t timestamp) _terrain_initialised = false; _sin_tilt_rng = sinf(_params.rng_sens_pitch); _cos_tilt_rng = cosf(_params.rng_sens_pitch); - _range_data_continuous = false; _control_status.value = 0; _control_status_prev.value = 0; diff --git a/EKF/ekf.h b/EKF/ekf.h index 905f80d01a..8403daf28a 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -209,6 +209,8 @@ private: } _state_reset_status; float _dt_ekf_avg; // average update rate of the ekf + float _dt_update; // delta time since last ekf update. This time can be used for filters + // which run at the same rate as the Ekf::update() function stateSample _state; // state struct of the ekf running at the delayed time horizon @@ -319,6 +321,8 @@ private: float _cos_tilt_rng; // cosine of the range finder tilt rotation about the Y body axis float _R_rng_to_earth_2_2; // 2,2 element of the rotation matrix from sensor frame to earth frame bool _range_data_continuous; // true when we are receiving range finder data faster than a 2Hz average + float _dt_last_range_update_filt_us; // filtered value of the delta time elapsed since the last range measurement came into + // the filter (microseconds) // height sensor fault status bool _baro_hgt_faulty; // true if valid baro data is unavailable for use diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index ec180e087c..e7951554c0 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -175,20 +175,16 @@ void Ekf::checkRangeDataContinuity() { // update range data continuous flag (2Hz ie 500 ms) /* Timing in micro seconds */ - static hrt_abstime t = 0; - static hrt_abstime t_prev = 0; - static float dt = 0.0f; - t = hrt_absolute_time(); - dt = t_prev != 0 ? (t - t_prev) * 1.0f : 0.0f; - t_prev = t; - dt = math::min(dt, 1.0f); - static float range_update_interval = 0.0f; /* Apply a 1.0 sec low pass filter to the time delta from the last range finder updates */ - range_update_interval = range_update_interval * (1.0f - dt) + dt * (_time_last_imu - _time_last_range); + _dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - _dt_update) + _dt_update * + (_time_last_imu - _time_last_range); - if (range_update_interval < 5e5f) { + _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 1e6f); + + if (_dt_last_range_update_filt_us < 5e5f) { _range_data_continuous = true; + } else { _range_data_continuous = false; }