Browse Source

ekf2: fix dist bottom constraint

release/1.12
bresch 4 years ago committed by Daniel Agar
parent
commit
b23d8244f0
  1. 4
      src/modules/ekf2/EKF2.cpp

4
src/modules/ekf2/EKF2.cpp

@ -711,8 +711,8 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp) @@ -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;

Loading…
Cancel
Save