From 8004e9fe7e32693fa99c651cec00ba3c73503bda Mon Sep 17 00:00:00 2001
From: devbharat <bharat.tak8@gmail.com>
Date: Wed, 30 Nov 2016 15:02:07 +0100
Subject: [PATCH] EKF: Make range finder data continuous check more robust

Use a filtered arrival time delta to determine if range data is continuous
---
 EKF/ekf.cpp               |  2 ++
 EKF/ekf.h                 |  4 ++++
 EKF/terrain_estimator.cpp | 27 ++++++++++++++++++++++++++-
 3 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 0b9aeea5fe..1ea22084f8 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -183,6 +183,7 @@ 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;
@@ -214,6 +215,7 @@ bool Ekf::update()
 		predictCovariance();
 
 		// perform state and variance prediction for the terrain estimator
+		checkRangeDataContinuity();
 		if (!_terrain_initialised) {
 			_terrain_initialised = initHagl();
 
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 1f59b1d525..faf0074e8c 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -318,6 +318,7 @@ private:
 	float _sin_tilt_rng;		// sine of the range finder tilt rotation about the Y body axis
 	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
 
 	// height sensor fault status
 	bool _baro_hgt_faulty;		// true if valid baro data is unavailable for use
@@ -479,4 +480,7 @@ private:
 	// perform a reset of the wind states
 	void resetWindStates();
 
+	// check that the range finder data is continuous
+	void checkRangeDataContinuity();
+
 };
diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
index b5748adc46..016a502783 100644
--- a/EKF/terrain_estimator.cpp
+++ b/EKF/terrain_estimator.cpp
@@ -41,6 +41,7 @@
 
 #include "ekf.h"
 #include "mathlib.h"
+#include <drivers/drv_hrt.h>
 
 bool Ekf::initHagl()
 {
@@ -135,7 +136,7 @@ bool Ekf::get_terrain_vert_pos(float *ret)
 {
 	memcpy(ret, &_terrain_vpos, sizeof(float));
 
-	if (_terrain_initialised && (_time_last_imu - _time_last_hagl_fuse < 1e6)) {
+	if (_terrain_initialised && _range_data_continuous) {
 		return true;
 
 	} else {
@@ -153,3 +154,27 @@ void Ekf::get_hagl_innov_var(float *hagl_innov_var)
 {
 	memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
 }
+
+// check that the range finder data is continuous
+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);
+
+	if (range_update_interval < 5e5f) {
+		_range_data_continuous = true;
+	} else {
+		_range_data_continuous = false;
+	}
+}