Skip to content

Commit

Permalink
ekf2-rng: do not continuously reset terrain in rng height ref
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
bresch committed Jan 3, 2025
1 parent d8b1b18 commit f16feb4
Showing 1 changed file with 5 additions and 6 deletions.
11 changes: 5 additions & 6 deletions src/modules/ekf2/EKF/terrain_control.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
}
Expand Down

0 comments on commit f16feb4

Please sign in to comment.