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 <bapstroman@gmail.com>

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;

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

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;
 	}