From a5a67315fd447d6703a89462510f6bcf27b650c1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 12 Jul 2024 13:13:36 -0400 Subject: [PATCH] ekf2: optical flow magnitude check compensated - additionally don't use flow for reset if magnitude isn't acceptable --- .../EKF/aid_sources/optical_flow/optical_flow_control.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 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 0e4b8b668e..89bbb6add0 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 @@ -71,8 +71,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) : _params.flow_qual_min_gnd; const bool is_quality_good = (flow_sample.quality >= min_quality); - const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite() - && !flow_sample.flow_rate.longerThan(_flow_max_rate); bool is_tilt_good = true; @@ -120,6 +118,10 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance; + const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite() + && !flow_sample.flow_rate.longerThan(_flow_max_rate) + && !flow_compensated.longerThan(_flow_max_rate); + const bool continuing_conditions_passing = (_params.flow_ctrl == 1) && _control_status.flags.tilt_align && is_within_max_sensor_dist; @@ -142,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) { - if (is_flow_required && is_quality_good) { + if (is_flow_required && is_quality_good && is_magnitude_good) { resetFlowFusion(); if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {