fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2-rng: do not continuously reset terrain in rng height ref

Browse Source 
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.
This commit is contained in:
bresch 2025-01-03 10:23:03 +01:00 committed by Mathieu Bresciani
parent 0b1838f233
commit c1589ddb18
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/ekf2/EKF/terrain_control.cpp
View File

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