From 514e0330e5e4d030fe017de24529351f57df2600 Mon Sep 17 00:00:00 2001
From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)>
Date: Mon, 25 Sep 2023 10:45:18 +0200
Subject: [PATCH] ekf2_terrain: handle height reset

---
 src/modules/ekf2/EKF/ekf.h                    |  1 +
 src/modules/ekf2/EKF/ekf_helper.cpp           |  4 +++
 src/modules/ekf2/EKF/terrain_estimator.cpp    |  4 +++
 src/modules/ekf2/test/CMakeLists.txt          |  2 +-
 .../ekf2/test/test_EKF_terrain_estimator.cpp  | 28 +++++++++++++++++++
 5 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 8eb1ba235d..96269ae78a 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -917,6 +917,7 @@ private:
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
 	void controlHaglFakeFusion();
+	void terrainHandleVerticalPositionReset(float delta_z);
 
 # if defined(CONFIG_EKF2_RANGE_FINDER)
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index 42ec11c4f0..73c8286d19 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -211,6 +211,10 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_TERRAIN)
+	terrainHandleVerticalPositionReset(delta_z);
+#endif
+
 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;
 }
diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp
index c8fd3dc88c..9ffc5d3f65 100644
--- a/src/modules/ekf2/EKF/terrain_estimator.cpp
+++ b/src/modules/ekf2/EKF/terrain_estimator.cpp
@@ -452,3 +452,7 @@ bool Ekf::isTerrainEstimateValid() const
 
 	return false;
 }
+
+void Ekf::terrainHandleVerticalPositionReset(const float delta_z) {
+	_terrain_vpos += delta_z;
+}
diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt
index 152aafc944..7d0af8e502 100644
--- a/src/modules/ekf2/test/CMakeLists.txt
+++ b/src/modules/ekf2/test/CMakeLists.txt
@@ -58,7 +58,7 @@ px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF e
 px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_sideslip_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
-px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
index 9ea52878b6..72d0016160 100644
--- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
+++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
@@ -40,6 +40,7 @@
 #include "EKF/ekf.h"
 #include "sensor_simulator/sensor_simulator.h"
 #include "sensor_simulator/ekf_wrapper.h"
+#include "test_helper/reset_logging_checker.h"
 
 class EkfTerrainTest : public ::testing::Test
 {
@@ -182,3 +183,30 @@ TEST_F(EkfTerrainTest, testRngForTerrainFusion)
 	const float estimated_distance_to_ground = _ekf->getTerrainVertPos();
 	EXPECT_NEAR(estimated_distance_to_ground, rng_height, 0.01f);
 }
+
+TEST_F(EkfTerrainTest, testHeightReset)
+{
+	// GIVEN: rng for terrain but not flow
+	_ekf_wrapper.disableTerrainFlowFusion();
+	_ekf_wrapper.enableTerrainRngFusion();
+
+	const float rng_height = 1.f;
+	const float flow_height = 1.f;
+	runFlowAndRngScenario(rng_height, flow_height);
+
+	const float estimated_distance_to_ground = _ekf->getTerrainVertPos() - _ekf->getPosition()(2);
+
+	ResetLoggingChecker reset_logging_checker(_ekf);
+	reset_logging_checker.capturePreResetState();
+
+	// WHEN: the baro height is suddenly changed to trigger a height reset
+	const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
+	_sensor_simulator._baro.setData(new_baro_height);
+	_sensor_simulator.stopGps(); // prevent from switching to GNSS height
+	_sensor_simulator.runSeconds(6);
+
+	// THEN: a height reset occured and the estimated distance to the ground remains constant
+	reset_logging_checker.capturePostResetState();
+	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
+	EXPECT_NEAR(estimated_distance_to_ground, _ekf->getTerrainVertPos() - _ekf->getPosition()(2), 1e-3f);
+}