From 3bd09fdcd35a86b47cee6939bc8962244d4cbcc7 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Thu, 26 Mar 2020 10:22:32 +0100 Subject: [PATCH] EKF: fix range height sensor offset (#787) The offset needs to be updated directly when switching to range height --- EKF/control.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index c4655de046..134e17014e 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -984,18 +984,19 @@ void Ekf::controlHeightFusion() setControlRangeHeight(); fuse_height = _range_data_ready; - } else if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - // we have just switched to using range finder, calculate height sensor offset such that current - // measurement matches our current height estimate - // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar) - if (_control_status.flags.in_air && isTerrainEstimateValid()) { - _hgt_sensor_offset = _terrain_vpos; + if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { + // we have just switched to using range finder, calculate height sensor offset such that current + // measurement matches our current height estimate + // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar) + if (_control_status.flags.in_air && isTerrainEstimateValid()) { + _hgt_sensor_offset = _terrain_vpos; - } else if (_control_status.flags.in_air) { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } else if (_control_status.flags.in_air) { + _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); - } else { - _hgt_sensor_offset = _params.rng_gnd_clearance; + } else { + _hgt_sensor_offset = _params.rng_gnd_clearance; + } } } else if (_baro_data_ready && !_baro_hgt_faulty) {