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-terrain: do not consider negative hagl as valid

Browse Source
This commit is contained in:
bresch 2024-10-30 11:51:24 +01:00 committed by Daniel Agar
parent 4d1c65d722
commit d579fb540c
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,7 +79,8 @@ void Ekf::updateTerrainValidity()
{
bool valid_opt_flow_terrain = false;
bool valid_rng_terrain = false;
bool valid_hagl_var = false;
bool positive_hagl_var = false;
bool small_relative_hagl_var = false;
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
@ -106,21 +107,29 @@ void Ekf::updateTerrainValidity()
float hagl_var = INFINITY;
sym::ComputeHaglInnovVar(P, 0.f, &hagl_var);
if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) {
valid_hagl_var = true;
positive_hagl_var = hagl_var > 0.f;
if (positive_hagl_var
&& (hagl_var < sq(fmaxf(0.1f * getHagl(), 0.5f)))
) {
small_relative_hagl_var = true;
}
}
const bool positive_hagl = getHagl() >= 0.f;
if (!_terrain_valid) {
// require valid RNG or optical flow (+valid variance) to initially consider terrain valid
if (valid_rng_terrain
|| (valid_opt_flow_terrain && valid_hagl_var)
if (positive_hagl
&& positive_hagl_var
&& (valid_rng_terrain
|| (valid_opt_flow_terrain && small_relative_hagl_var))
) {
_terrain_valid = true;
}
} else {
// terrain was previously valid, continue considering valid if variance is good
_terrain_valid = valid_hagl_var;
_terrain_valid = positive_hagl && positive_hagl_var && small_relative_hagl_var;
}
}