From 0ce64e1b92945186c6cdba519a972fd064435fde Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 12 Jun 2024 03:20:40 +0200 Subject: [PATCH] ekf2: don't fuse optical flow samples when the current distance to the ground is larger than the reported maximum flow sensor distance Signed-off-by: RomanBapst --- .../EKF/aid_sources/optical_flow/optical_flow_control.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 f8218e883b24..160b0b24308e 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 @@ -55,9 +55,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate); const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); + // don't enforce this condition if terrain estimate is not valid as we have logic below to coast through bad range finder data + const bool is_within_max_sensor_dist = isTerrainEstimateValid() ? (_terrain_vpos - _state.pos(2) <= _flow_max_distance) : true; + if (is_quality_good && is_magnitude_good - && is_tilt_good) { + && is_tilt_good + && is_within_max_sensor_dist) { // compensate for body motion to give a LOS rate calcOptFlowBodyRateComp(imu_delayed); _flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();