From 5d08b97fd707b6153d1f162de8dae6eb365888ab Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 28 Jun 2024 15:24:39 -0400 Subject: [PATCH] ekf2: add vehicle_local_position dist_bottom_var --- msg/VehicleLocalPosition.msg | 1 + src/modules/ekf2/EKF2.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index c1a0dffe14f6..25551ab4fb70 100644 --- 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 index 62a0af99ad70..64725d2bec7f 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1643,6 +1643,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) #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;