From dcb47e425d993b24288ed9591d58ee69b92b8211 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 Mar 2016 20:44:11 +1100 Subject: [PATCH] ekf2: Add local position ground clearance reporting --- src/modules/ekf2/ekf2_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index b8d36d2095..a882390eea 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -482,10 +482,11 @@ void Ekf2::task_main() // The rotation of the tangent plane vs. geographical north lpos.yaw = att.yaw; - lpos.dist_bottom = -pos[2] + lpos.ref_alt; // Distance to bottom surface (ground) in meters + float terrain_vpos; + lpos.dist_bottom_valid = _ekf->get_terrain_vert_pos(&terrain_vpos); + lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found - lpos.dist_bottom_valid = true; // true if distance to bottom surface is valid // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres // TODO: Should use sqrt of filter position variances