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

ekf2 - preflt checks: scale flow innovation checks

Browse Source 
Opt flow raw innovations can be really large on ground due to the small
distance to the ground (vel = flow / dist). To make the pre-flight check
more meaningful, scale it with the current distance.
This commit is contained in:
bresch
2023-07-03 13:35:44 +02:00
committed by Ramon Roche
parent 672b29d555
commit f5edff2647
3 changed files with 7 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+4
View File
@@ -1296,6 +1296,10 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
// set dist bottom to scale flow innovation
const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2);
_preflt_checker.setDistBottom(dist_bottom);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)