ekf2: add vehicle_local_position dist_bottom_var

PX4 · Jul 9, 2024 · 5d08b97 · 5d08b97
1 parent 3e3b886
commit 5d08b97
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 0 deletions.
diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg
@@ -56,6 +56,7 @@ float32 ref_alt				# Reference altitude AMSL, (metres)
 
 # Distance to surface
 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
 uint8 dist_bottom_sensor_bitfield	# bitfield indicating what type of sensor is used to estimate dist_bottom
 uint8 DIST_BOTTOM_SENSOR_NONE = 0

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
@@ -1643,6 +1643,7 @@ 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 = math::max(_ekf.getHagl(), 0.f);
+	lpos.dist_bottom_var = _ekf.getTerrainVariance();
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 
 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;