From 61c66a723ce2599c419e1161887a380ca4dac185 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 23 Feb 2022 16:07:27 +0100
Subject: [PATCH] ekf rng fusion: use terrain state as offset directly

---
 src/modules/ekf2/EKF/ekf_helper.cpp    | 8 ++------
 src/modules/ekf2/EKF/height_fusion.cpp | 2 +-
 2 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index e509e7238c..81833cfac5 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -289,7 +289,7 @@ void Ekf::resetHeightToRng()
 
 	// update the state and associated variance
 	const float z_pos_before_reset = _state.pos(2);
-	resetVerticalPositionTo(-dist_bottom + _hgt_sensor_offset);
+	resetVerticalPositionTo(-dist_bottom + _state.posd_terrain);
 
 	// the state variance is the same as the observation
 	P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
@@ -1289,7 +1289,7 @@ void Ekf::startRngHgtFusion()
 
 		// Range finder is the primary height source, the ground is now the datum used
 		// to compute the local vertical position
-		_hgt_sensor_offset = 0.f;
+		resetHaglFake();
 
 		if (!_control_status_prev.flags.ev_hgt) {
 			// EV and range finders are using the same height datum
@@ -1302,10 +1302,6 @@ void Ekf::startRngAidHgtFusion()
 {
 	if (!_control_status.flags.rng_hgt) {
 		setControlRangeHeight();
-
-		// calculate height sensor offset such that current
-		// measurement matches our current height estimate
-		_hgt_sensor_offset = _state.posd_terrain;
 	}
 }
 
diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp
index bc33085669..3aed764fee 100644
--- a/src/modules/ekf2/EKF/height_fusion.cpp
+++ b/src/modules/ekf2/EKF/height_fusion.cpp
@@ -90,7 +90,7 @@ void Ekf::fuseRngHgt()
 {
 	// use range finder with tilt correction
 	_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
-					  _params.rng_gnd_clearance)) - _hgt_sensor_offset;
+					  _params.rng_gnd_clearance)) - _state.posd_terrain;
 
 	// innovation gate size
 	float innov_gate = fmaxf(_params.range_innov_gate, 1.f);