ekf2: add terrain/dist_bottom reset deltas (vehicle_local_position/ve…

…hicle_global_position)

msg/VehicleGlobalPosition.msg

@@ -14,8 +14,10 @@ float32 alt			# Altitude AMSL, (meters)
 float32 alt_ellipsoid		# Altitude above ellipsoid, (meters)
 
 float32 delta_alt 	# Reset delta for altitude
+float32 delta_terrain   # Reset delta for terrain
 uint8 lat_lon_reset_counter	# Counter for reset events on horizontal position coordinates
 uint8 alt_reset_counter 	# Counter for reset events on altitude
+uint8 terrain_reset_counter     # Counter for reset events on terrain
 
 float32 eph			# Standard deviation of horizontal position error, (metres)
 float32 epv			# Standard deviation of vertical position error, (metres)

msg/VehicleLocalPosition.msg

@@ -55,9 +55,13 @@ float64 ref_lon				# Reference point longitude, (degrees)
 float32 ref_alt				# Reference altitude AMSL, (metres)
 
 # Distance to surface
+bool dist_bottom_valid			# true if distance to bottom surface is valid
 float32 dist_bottom			# Distance from from bottom surface to ground, (metres)
 float32 dist_bottom_var                 # terrain estimate variance (m^2)
-bool dist_bottom_valid			# true if distance to bottom surface is valid
+
+float32 delta_dist_bottom               # Amount of vertical shift of dist bottom estimate in latest reset [m]
+uint8 dist_bottom_reset_counter         # Index of latest dist bottom estimate reset
+
 uint8 dist_bottom_sensor_bitfield	# bitfield indicating what type of sensor is used to estimate dist_bottom
 uint8 DIST_BOTTOM_SENSOR_NONE = 0
 uint8 DIST_BOTTOM_SENSOR_RANGE = 1	# (1 << 0) a range sensor is used to estimate dist_bottom field

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -219,15 +219,29 @@ void Ekf::resetFlowFusion()
 void Ekf::resetTerrainToFlow()
 {
 	ECL_INFO("reset hagl to flow");
+
 	// TODO: use the flow data
-	_state.terrain = fmaxf(0.0f, _state.pos(2));
+	const float new_terrain = fmaxf(0.0f, _state.pos(2));
+	const float delta_terrain = new_terrain - _state.terrain;
+	_state.terrain = new_terrain;
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
-	_terrain_vpos_reset_counter++;
 
 	resetAidSourceStatusZeroInnovation(_aid_src_optical_flow);
 
 	_innov_check_fail_status.flags.reject_optflow_X = false;
 	_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
 }
 
 void Ekf::stopFlowFusion()

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -267,9 +267,23 @@ float Ekf::getRngVar() const
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
 {
-	_state.terrain = _state.pos(2) + aid_src.observation;
+	const float new_terrain = _state.pos(2) + aid_src.observation;
+	const float delta_terrain = new_terrain - _state.terrain;
+
+	_state.terrain = new_terrain;
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
-	_terrain_vpos_reset_counter++;
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+
 
 	aid_src.time_last_fuse = _time_delayed_us;
 }

src/modules/ekf2/EKF/ekf.h

@@ -104,9 +104,6 @@ class Ekf final : public EstimatorInterface
 	float getTerrainVertPos() const { return _state.terrain; };
 	float getHagl() const { return _state.terrain - _state.pos(2); }
 
-	// get the number of times the vertical terrain position has been reset
-	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
-
 	// get the terrain variance
 	float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }
 
@@ -358,6 +355,13 @@ class Ekf final : public EstimatorInterface
 		*counter = _state_reset_status.reset_count.posD;
 	}
 
+	uint8_t get_hagl_reset_count() const { return _state_reset_status.reset_count.hagl; }
+	void get_hagl_reset(float *delta, uint8_t *counter) const
+	{
+		*delta = _state_reset_status.hagl_change;
+		*counter = _state_reset_status.reset_count.hagl;
+	}
+
 	// return the amount the local vertical velocity changed in the last reset and the number of reset events
 	uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; }
 	void get_velD_reset(float *delta, uint8_t *counter) const
@@ -551,6 +555,7 @@ class Ekf final : public EstimatorInterface
 		uint8_t posNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
 		uint8_t posD{0};	///< number of vertical position reset events (allow to wrap if count exceeds 255)
 		uint8_t quat{0};	///< number of quaternion reset events (allow to wrap if count exceeds 255)
+		uint8_t hagl{0};	///< number of height above ground level reset events (allow to wrap if count exceeds 255)
 	};
 
 	struct StateResets {
@@ -559,6 +564,7 @@ class Ekf final : public EstimatorInterface
 		Vector2f posNE_change;	///< North, East position change due to last reset (m)
 		float posD_change;	///< Down position change due to last reset (m)
 		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
+		float hagl_change;	///< Height above ground level change due to last reset (m)
 
 		StateResetCounts reset_count{};
 	};
@@ -599,8 +605,6 @@ class Ekf final : public EstimatorInterface
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Terrain height state estimation
-	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
-
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 #endif // CONFIG_EKF2_TERRAIN
 

src/modules/ekf2/EKF/position_fusion.cpp

@@ -167,7 +167,18 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	_state.terrain += delta_z;
-#endif
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_z;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_z;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+#endif // CONFIG_EKF2_TERRAIN
 
 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;

src/modules/ekf2/EKF2.cpp

@@ -1191,6 +1191,9 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 			global_pos.terrain_alt_valid = true;
 		}
 
+		float delta_hagl = 0.f;
+		_ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter);
+		global_pos.delta_terrain = -delta_z;
 #endif // CONFIG_EKF2_TERRAIN
 
 		global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
@@ -1619,9 +1622,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
+	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
-	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
+	_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);
 
 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;
 

src/modules/ekf2/EKF2Selector.cpp

@@ -479,18 +479,32 @@ void EKF2Selector::PublishVehicleLocalPosition()
 				_delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading);
 			}
 
+			// HAGL (dist_bottom) reset
+			if (!instance_change
+			    && (local_position.dist_bottom_reset_counter == _local_position_last.dist_bottom_reset_counter + 1)) {
+				++_hagl_reset_counter;
+				_delta_hagl_reset = local_position.delta_dist_bottom;
+
+			} else if (instance_change
+				   || (local_position.dist_bottom_reset_counter != _local_position_last.dist_bottom_reset_counter)) {
+				++_hagl_reset_counter;
+				_delta_hagl_reset = local_position.dist_bottom - _local_position_last.dist_bottom;
+			}
+
 		} else {
 			_xy_reset_counter = local_position.xy_reset_counter;
 			_z_reset_counter = local_position.z_reset_counter;
 			_vxy_reset_counter = local_position.vxy_reset_counter;
 			_vz_reset_counter = local_position.vz_reset_counter;
 			_heading_reset_counter = local_position.heading_reset_counter;
+			_hagl_reset_counter = local_position.dist_bottom_reset_counter;
 
 			_delta_xy_reset = Vector2f{local_position.delta_xy};
 			_delta_z_reset = local_position.delta_z;
 			_delta_vxy_reset = Vector2f{local_position.delta_vxy};
 			_delta_vz_reset = local_position.delta_vz;
 			_delta_heading_reset = local_position.delta_heading;
+			_delta_hagl_reset = local_position.delta_dist_bottom;
 		}
 
 		bool publish = true;
@@ -513,6 +527,7 @@ void EKF2Selector::PublishVehicleLocalPosition()
 			local_position.vxy_reset_counter = _vxy_reset_counter;
 			local_position.vz_reset_counter = _vz_reset_counter;
 			local_position.heading_reset_counter = _heading_reset_counter;
+			local_position.dist_bottom_reset_counter = _hagl_reset_counter;
 
 			_delta_xy_reset.copyTo(local_position.delta_xy);
 			local_position.delta_z = _delta_z_reset;
@@ -612,6 +627,16 @@ void EKF2Selector::PublishVehicleGlobalPosition()
 				_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
 			}
 
+			// terrain reset
+			if (!instance_change && (global_position.terrain_reset_counter == _global_position_last.terrain_reset_counter + 1)) {
+				++_terrain_reset_counter;
+				_delta_terrain_reset = global_position.delta_terrain;
+
+			} else if (instance_change || (global_position.terrain_reset_counter != _global_position_last.terrain_reset_counter)) {
+				++_terrain_reset_counter;
+				_delta_terrain_reset = global_position.delta_terrain - _global_position_last.delta_terrain;
+			}
+
 		} else {
 			_lat_lon_reset_counter = global_position.lat_lon_reset_counter;
 			_alt_reset_counter = global_position.alt_reset_counter;

src/modules/ekf2/EKF2Selector.hpp

@@ -202,11 +202,14 @@ class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem
 	matrix::Vector2f _delta_vxy_reset{};
 	float _delta_vz_reset{0.f};
 	float _delta_heading_reset{0};
+	float _delta_hagl_reset{0.f};
+
 	uint8_t _xy_reset_counter{0};
 	uint8_t _z_reset_counter{0};
 	uint8_t _vxy_reset_counter{0};
 	uint8_t _vz_reset_counter{0};
 	uint8_t _heading_reset_counter{0};
+	uint8_t _hagl_reset_counter{0};
 
 	// vehicle_odometry
 	vehicle_odometry_s _odometry_last{};
@@ -217,8 +220,11 @@ class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem
 	double _delta_lat_reset{0};
 	double _delta_lon_reset{0};
 	float _delta_alt_reset{0.f};
+	float _delta_terrain_reset{0.f};
+
 	uint8_t _lat_lon_reset_counter{0};
 	uint8_t _alt_reset_counter{0};
+	uint8_t _terrain_reset_counter{0};
 
 	// wind estimate
 	wind_s _wind_last{};

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -63,6 +63,11 @@ void FlightTask::_checkEkfResetCounters()
 		_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
 	}
 
+	if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) {
+		_ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom);
+		_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
+	}
+
 	if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
 		_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
 		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
@@ -138,7 +143,7 @@ void FlightTask::_evaluateVehicleLocalPosition()
 		// distance to bottom
 		if (_sub_vehicle_local_position.get().dist_bottom_valid
 		    && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
-			_dist_to_bottom =  _sub_vehicle_local_position.get().dist_bottom;
+			_dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom;
 		}
 
 		// global frame reference coordinates to enable conversions

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -61,6 +61,7 @@ struct ekf_reset_counters_s {
 	uint8_t z;
 	uint8_t vz;
 	uint8_t heading;
+	uint8_t hagl;
 };
 
 class FlightTask : public ModuleParams
@@ -191,6 +192,7 @@ class FlightTask : public ModuleParams
 	virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {};
 	virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {};
 	virtual void _ekfResetHandlerPositionZ(float delta_z) {};
+	virtual void _ekfResetHandlerHagl(float delta_hagl) {};
 	virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
 	virtual void _ekfResetHandlerHeading(float delta_psi) {};