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