diff --git a/EKF/control.cpp b/EKF/control.cpp index 4405fc2401..878032861e 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -357,52 +357,51 @@ void Ekf::controlOpticalFlowFusion() _time_good_motion_us = _imu_sample_delayed.time_us; } - // Inhibit flow use if motion is un-suitable or we have good quality GPS - // Apply hysteresis to prevent rapid mode switching - float gps_err_norm_lim; - if (_control_status.flags.opt_flow) { - gps_err_norm_lim = 0.7f; - } else { - gps_err_norm_lim = 1.0f; - } - - // If we are in-air and doing inertial dead reckoning or are using bad GPS, then optical flow use - // is necessary to control position drift - bool flow_required = _control_status.flags.in_air && - (_is_dead_reckoning || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); - - if (!_inhibit_flow_use) { - // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation - bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5); - bool flight_motion_not_ok = _control_status.flags.in_air && !_range_aid_mode_enabled; - if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) { - _inhibit_flow_use = true; + // New optical flow data has fallen behind the fusion time horizon and is ready to be fused + if (_flow_data_ready) { + // Inhibit flow use if motion is un-suitable or we have good quality GPS + // Apply hysteresis to prevent rapid mode switching + float gps_err_norm_lim; + if (_control_status.flags.opt_flow) { + gps_err_norm_lim = 0.7f; + } else { + gps_err_norm_lim = 1.0f; } - } else { - // allow use of optical flow if motion is suitable or we are reliant on it for flight navigation - bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6); - bool flight_motion_ok = _control_status.flags.in_air && _range_aid_mode_enabled; - if (preflight_motion_ok || flight_motion_ok || flow_required) { - _inhibit_flow_use = false; + // Check if we are in-air and require optical flow to control position drift + bool flow_required = _control_status.flags.in_air && + (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently + || (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos) // is completely reliant on optical flow + || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad + + if (!_inhibit_flow_use && _control_status.flags.opt_flow) { + // inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation + bool preflight_motion_not_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5); + bool flight_motion_not_ok = _control_status.flags.in_air && !_range_aid_mode_enabled; + if ((preflight_motion_not_ok || flight_motion_not_ok) && !flow_required) { + _inhibit_flow_use = true; + } + } else if (_inhibit_flow_use && !_control_status.flags.opt_flow){ + // allow use of optical flow if motion is suitable or we are reliant on it for flight navigation + bool preflight_motion_ok = !_control_status.flags.in_air && ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6); + bool flight_motion_ok = _control_status.flags.in_air && _range_aid_mode_enabled; + if (preflight_motion_ok || flight_motion_ok || flow_required) { + _inhibit_flow_use = false; + } } - } - // Handle cases where we are using optical flow but are no longer able to because data is old - // or its use has been inhibited. - if (_control_status.flags.opt_flow) { - if (_inhibit_flow_use) { - _control_status.flags.opt_flow = false; - _time_last_of_fuse = 0; + // Handle cases where we are using optical flow but are no longer able to because data is old + // or its use has been inhibited. + if (_control_status.flags.opt_flow) { + if (_inhibit_flow_use) { + _control_status.flags.opt_flow = false; + _time_last_of_fuse = 0; - } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) { - _control_status.flags.opt_flow = false; + } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) { + _control_status.flags.opt_flow = false; + } } - } - - if (_flow_data_ready) { - // New optical flow data has fallen behind the fusion time horizon and is ready to be fused // Accumulate autopilot gyro data across the same time interval as the flow sensor _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;