From f3b2599d334223a1b6026d414dd684b21fdacf58 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 30 Sep 2025 11:14:26 -0800 Subject: [PATCH] ekf: range fusion: fix height ref (#25654) * ekf2: range height skip "unhealthy" samples, but respect timeout - we should never directly use an "unhealthy" range finder sample for state corrections or resets, but we also shouldn't immediately abort active rng_hgt until the timeout has passed * check starting_conditions_passing once * ekf2: conditional range aid change height ref --------- Co-authored-by: Daniel Agar --- .../aid_sources/range_finder/range_height_control.cpp | 10 +++++++++- src/modules/ekf2/EKF/height_control.cpp | 4 +++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 1d51a94509..3e092e8342 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -182,11 +182,12 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - if ((do_conditional_range_aid || do_range_aid)) { + if (do_conditional_range_aid || do_range_aid) { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); _control_status.flags.rng_hgt = true; if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { + ECL_INFO("starting %s height fusion, resetting terrain", HGT_SRC_NAME); resetTerrainToRng(aid_src); resetAidSourceStatusZeroInnovation(aid_src); } @@ -197,6 +198,13 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) { if (continuing_conditions_passing) { + if (do_conditional_range_aid) { + _height_sensor_ref = HeightSensor::RANGE; + + } else if (_height_sensor_ref == HeightSensor::RANGE) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } + if (_range_sensor.isDataHealthy() && _control_status.flags.rng_kin_consistent ) { diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index bc7f95f0e3..2f2e0b26be 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -112,8 +112,10 @@ void Ekf::checkHeightSensorRefFallback() || ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt) || ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt) || ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) { - ECL_INFO("fallback to secondary height reference"); + _height_sensor_ref = fallback_list[i]; + + ECL_WARN("fallback to secondary height reference %d", (int)_height_sensor_ref); break; } }