From 9d6c2baa908026af000cbdbf0680621f2b0fa777 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 a6c0569ac7..390a957992 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 @@ -116,7 +116,7 @@ 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 is_magnitude_good = flow_sample.flow_rate.isAllFinite() && !flow_sample.flow_rate.longerThan(_flow_max_rate) @@ -124,12 +124,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) 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.