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-flow: limit minimum flow hagl

Browse Source
This commit is contained in:
bresch 2025-03-21 14:19:08 +01:00 committed by Mathieu Bresciani
parent 53bdceb895
commit f73c7977dd
1 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
View File

@ -125,15 +125,13 @@ float Ekf::predictFlowHagl() const
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
// a positive offset in earth frame leads to a smaller height above the ground.
float height_above_gnd_est = getHagl() - pos_offset_earth(2);
const float height_above_gnd_est = fabsf(getHagl() - pos_offset_earth(2));
constexpr float min_hagl = FLT_EPSILON;
// Never return a really small value to avoid generating insanely large flow innovations
// that could destabilize the filter
constexpr float min_hagl = 1e-2f;
if (fabsf(height_above_gnd_est) < min_hagl) {
height_above_gnd_est = signNoZero(height_above_gnd_est) * min_hagl;
}
return fabsf(height_above_gnd_est);
return fmaxf(height_above_gnd_est, min_hagl);
}
float Ekf::predictFlowRange() const
{