fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-02 19:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

add constraint to ground distance to avoid values below rng_gnd_clearance (#7662)

Browse Source
This commit is contained in:
ChristophTobler
2017-07-21 23:49:00 +02:00
committed by Paul Riseborough
parent a6c682ce50
commit bc951f6f15
1 changed files with 6 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+6
View File
@@ -942,6 +942,12 @@ void Ekf2::run()
lpos.dist_bottom_valid = _ekf.get_terrain_valid();
_ekf.get_terrain_vert_pos(&terrain_vpos);
lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters
// constrain the distance to ground to _params->rng_gnd_clearance
if (lpos.dist_bottom < _params->rng_gnd_clearance) {
lpos.dist_bottom = _params->rng_gnd_clearance;
}
lpos.dist_bottom_rate = -velocity[2]; // Distance to bottom surface (ground) change rate
lpos.surface_bottom_timestamp = now; // Time when new bottom surface found