From 8dd5d0d1a3a16c846de906645967c706653311b0 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 8 Aug 2022 12:10:37 +0200 Subject: [PATCH] ekf2: don't reset height when in conditional range aiding --- src/modules/ekf2/EKF/range_height_control.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 3e3d450f6e..d654caf4ca 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -137,7 +137,12 @@ bool Ekf::isConditionalRangeAidSuitable() void Ekf::startRngHgtFusion() { if (!_control_status.flags.rng_hgt) { - if (_params.height_sensor_ref == HeightSensor::RANGE) { + if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl == RngCtrl::CONDITIONAL)) { + // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. + _rng_hgt_b_est.setBias(_state.pos(2) + _range_sensor.getDistBottom()); + _height_sensor_ref = HeightSensor::RANGE; + + } else if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl != RngCtrl::CONDITIONAL)) { // Range finder is the primary height source, the ground is now the datum used // to compute the local vertical position _rng_hgt_b_est.reset();