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();