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 afb845a662ec..d00502228964 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) { - - bool recent_terrain_aiding = isRecent(_time_last_terrain_fuse, (uint64_t)1e6); + 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); - if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) { + if (no_terrain_aiding && (_height_sensor_ref != HeightSensor::RANGE)) { initTerrain(); } }