diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a4f4868ec0..40a55bbdef 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -364,20 +364,24 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // check magnitude is within sensor limits + // use this to prevent use of a saturated flow sensor when there are other aiding sources available float flow_rate_magnitude; bool flow_magnitude_good = true; - if (delta_time_good) { flow_rate_magnitude = flow->flowdata.norm() / delta_time; flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max); } + bool relying_on_flow = _control_status.flags.opt_flow + && !_control_status.flags.gps + && !_control_status.flags.ev_pos; + // check quality metric bool flow_quality_good = (flow->quality >= _params.flow_qual_min); // Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling // If flow quality fails checks on ground, assume zero flow rate after body rate compensation - if ((delta_time_good && flow_quality_good && flow_magnitude_good) || !_control_status.flags.in_air) { + if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) { flowSample optflow_sample_new; // calculate the system time-stamp for the mid point of the integration period optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;