From c5d464b821054490ace947ba2645cd70dbc27dbb Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 27 Apr 2017 17:24:04 +0200 Subject: [PATCH] make range sensor height offset computation more robust - when switching to range finder use the current terrain estimate as height sensor offset, otherwise spikes in the range measurements could lead to a wrong offset Signed-off-by: Roman --- EKF/control.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8b292ca516..47b3f186d0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -713,7 +713,11 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurment matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + if (_terrain_initialised) { + _hgt_sensor_offset = _terrain_vpos; + } else { + _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } } } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) { @@ -745,7 +749,11 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurment matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + if (_terrain_initialised) { + _hgt_sensor_offset = _terrain_vpos; + } else { + _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } } } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) {