From 56823b5ac9a0b8e1b25634c9506d1ff68966ba2d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 14 Oct 2021 09:39:10 -0400 Subject: [PATCH] ekf2: EKF control don't allow invalid flow gyro to propagate --- src/modules/ekf2/EKF/control.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 3cd981cc6a..8d4b8d5428 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -396,6 +396,13 @@ void Ekf::controlOpticalFlowFusion() _flow_sample_delayed.dt = delta_time_min; } + // don't allow invalid flow gyro_xyz to propagate + if (!is_body_rate_comp_available) { + if (!PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(1)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_xyz(2))) { + _flow_sample_delayed.gyro_xyz.zero(); + } + } + // when on the ground with poor flow quality, // assume zero ground relative velocity and LOS rate _flow_compensated_XY_rad.setZero();