yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fix dist bottom constraint

Browse Source
This commit is contained in:
bresch
2020-12-16 15:09:49 +01:00
committed by Daniel Agar
parent acc647c518
commit b23d8244f0
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF2.cpp
View File

@@ -711,8 +711,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
float terrain_vpos = _ekf.getTerrainVertPos();
// Distance to bottom surface (ground) in meters
// constrain the distance to ground to _rng_gnd_clearance
lpos.dist_bottom = math::min(terrain_vpos - lpos.z, _param_ekf2_min_rng.get());
// constrain the distance to ground to _rng_gnd_clearance
lpos.dist_bottom = math::max(terrain_vpos - lpos.z, _param_ekf2_min_rng.get());
if (!_had_valid_terrain) {
_had_valid_terrain = lpos.dist_bottom_valid;