From c1589ddb18a3b24dd568f77cd414834b79c2c421 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 3 Jan 2025 10:23:03 +0100 Subject: [PATCH] ekf2-rng: do not continuously reset terrain in rng height ref When the height reference datum is ground level (range finder height ref), the terrain state is not updated through fusion and should stay at a constant altitude. --- src/modules/ekf2/EKF/terrain_control.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index afb845a662..d005022289 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -63,13 +63,12 @@ void Ekf::controlTerrainFakeFusion() initTerrain(); } - if (!_control_status.flags.in_air - && !_control_status.flags.rng_terrain - && !_control_status.flags.opt_flow_terrain) { + if (!_control_status.flags.in_air) { + bool no_terrain_aiding = !_control_status.flags.rng_terrain + && !_control_status.flags.opt_flow_terrain + && isTimedOut(_time_last_terrain_fuse, (uint64_t)1e6); - bool recent_terrain_aiding = isRecent(_time_last_terrain_fuse, (uint64_t)1e6); - - if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) { + if (no_terrain_aiding && (_height_sensor_ref != HeightSensor::RANGE)) { initTerrain(); } }