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(); } }