From 24d19fd6a55792c22b8a80f501dee70e029988a6 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 15 Jul 2024 14:38:45 +0200 Subject: [PATCH] ekf2-flow: only allow flow when in range Also, as the flow makes the link between range and horizontal velocity, only allow it to start if at least one of the two is known. Otherwise the EKF will struggle to estimate both values at the same time. --- .../EKF/aid_sources/optical_flow/optical_flow_control.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 0e4b8b668ed6..d02087f1e53d 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -118,16 +118,17 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)); - const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance; + const bool is_within_sensor_dist = (getHagl() >= _flow_min_distance) && (getHagl() <= _flow_max_distance); const bool continuing_conditions_passing = (_params.flow_ctrl == 1) && _control_status.flags.tilt_align - && is_within_max_sensor_dist; + && is_within_sensor_dist; const bool starting_conditions_passing = continuing_conditions_passing && is_quality_good && is_magnitude_good && is_tilt_good + && (isTerrainEstimateValid() || isHorizontalAidingActive()) && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching // If the height is relative to the ground, terrain height cannot be observed.