From 370e04ee606364370a87416b31cb259a686bdbbe Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 3 Oct 2019 18:01:02 +0200
Subject: [PATCH] terrain_est: Continuously reset terrain height on ground
 using known clearance. This is the best estimate as we should not rely on a
 distance sensor while on the ground. This also helps when the drone is
 carried over as it avoids starting with a crazy downward distance for optical
 flow scaling.

---
 EKF/terrain_estimator.cpp | 35 +++++++++++++++++++++++------------
 1 file changed, 23 insertions(+), 12 deletions(-)

diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
index 9b64a5902d..53c3b74e6f 100644
--- a/EKF/terrain_estimator.cpp
+++ b/EKF/terrain_estimator.cpp
@@ -45,33 +45,44 @@
 
 bool Ekf::initHagl()
 {
+	bool initialized = false;
 	// get most recent range measurement from buffer
 	const rangeSample &latest_measurement = _range_buffer.get_newest();
 
-	if (!_rng_hgt_faulty && (_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
+	if (!_control_status.flags.in_air) {
+		// if on ground, do not trust the range sensor, but assume a ground clearance
+		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
+		// use the ground clearance value as our uncertainty
+		_terrain_var = sq(_params.rng_gnd_clearance);
+		initialized = true;
+
+	} else if (!_rng_hgt_faulty
+		   && (_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5
+		   && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
 		// if we have a fresh measurement, use it to initialise the terrain estimator
 		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _R_rng_to_earth_2_2;
 		// initialise state variance to variance of measurement
 		_terrain_var = sq(_params.range_noise);
 		// success
-		return true;
+		initialized = true;
+
 	} else if (_flow_for_terrain_data_ready) {
 		// initialise terrain vertical position to origin as this is the best guess we have
 		_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
 		_terrain_var = 100.0f;
-		return true;
-	} else if (!_control_status.flags.in_air) {
-		// if on ground we assume a ground clearance
-		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
-		// Use the ground clearance value as our uncertainty
-		_terrain_var = sq(_params.rng_gnd_clearance);
-		// this is a guess
-		return true;
+		initialized = true;
 
 	} else {
 		// no information - cannot initialise
-		return false;
+		initialized = false;
+	}
+
+	if (initialized) {
+		// has initialized with valid data
+		_time_last_hagl_fuse = _time_last_imu;
 	}
+
+	return initialized;
 }
 
 void Ekf::runTerrainEstimator()
@@ -80,7 +91,7 @@ void Ekf::runTerrainEstimator()
 	checkRangeDataContinuity();
 
 	// Perform initialisation check
-	if (!_terrain_initialised) {
+	if (!_terrain_initialised || !_control_status.flags.in_air) {
 		_terrain_initialised = initHagl();
 
 	} else {