ekf2_terrain: handle height reset

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

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

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

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)

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