fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 18:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Add local position ground clearance reporting

Browse Source
This commit is contained in:
Paul Riseborough
2016-03-13 17:47:56 +01:00
committed by Lorenz Meier
parent 70e9047f5d
commit dcb47e425d
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+3 -2
View File
@@ -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