add protections for EV and flow starting

src/modules/ekf2/EKF/common.h

@@ -67,7 +67,7 @@ using math::Utilities::updateYawInRotMat;
 
 // maximum sensor intervals in usec
 #define BARO_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between pressure altitude measurements (uSec)
-#define EV_MAX_INTERVAL   (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
+#define EV_MAX_INTERVAL   (uint64_t)1e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
 #define GPS_MAX_INTERVAL  (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec)
 #define RNG_MAX_INTERVAL  (uint64_t)2e5 ///< Maximum allowable time interval between range finder  measurements (uSec)
 

src/modules/ekf2/EKF/ekf.cpp

@@ -204,8 +204,6 @@ bool Ekf::initialiseFilter()
 	_time_last_hgt_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
 	_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
-	_time_last_hagl_fuse = _imu_sample_delayed.time_us;
-	_time_last_flow_terrain_fuse = _imu_sample_delayed.time_us;
 
 	// reset the output predictor state history to match the EKF initial values
 	alignOutputFilter();

src/modules/ekf2/EKF/ekf.h

@@ -590,6 +590,9 @@ private:
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	float _gps_alt_ref{NAN};		///< WGS-84 height (m)
 
+	uint64_t _last_ev_fail_us{0};		///< last system time in usec that the EV failed it's checks
+	uint64_t _last_ev_pass_us{0};		///< last system time in usec that the EV passed it's checks
+
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
 	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation

src/modules/ekf2/EKF/ev_control.cpp

@@ -52,10 +52,22 @@ void Ekf::controlExternalVisionFusion()
 		// determine if we should use the horizontal position observations
 		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
 
+		if (quality_sufficient) {
+			_last_ev_pass_us = _imu_sample_delayed.time_us;
+
+		} else {
+			_last_ev_fail_us = _imu_sample_delayed.time_us;
+		}
+
+		const bool ev_quality_passing = isTimedOut(_last_ev_fail_us, (uint64_t)1e6);
+		const bool ev_quality_failing = isTimedOut(_last_ev_pass_us, (uint64_t)1e6);
+
 		bool starting_conditions_passing = quality_sufficient
+						   && ev_quality_passing
+						   && !ev_quality_failing
+						   && !ev_reset
 						   && ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
 						   && ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
-						   && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
 						   && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
 
 		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -282,7 +282,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, bool starting_con
 
 			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
 
-			_nb_ev_pos_reset_available = 5;
+			_nb_ev_pos_reset_available = 3;
 			_information_events.flags.starting_vision_pos_fusion = true;
 			_control_status.flags.ev_pos = true;
 		}

src/modules/ekf2/EKF/ev_vel_control.cpp

@@ -214,7 +214,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_con
 
 			aid_src.time_last_fuse = _imu_sample_delayed.time_us;
 
-			_nb_ev_vel_reset_available = 5;
+			_nb_ev_vel_reset_available = 3;
 			_information_events.flags.starting_vision_vel_fusion = true;
 			_control_status.flags.ev_vel = true;
 		}

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -169,7 +169,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, bool starting_con
 			}
 
 			if (_control_status.flags.ev_yaw) {
-				_nb_ev_yaw_reset_available = 5;
+				_nb_ev_yaw_reset_available = 3;
 			}
 		}
 	}

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -50,6 +50,7 @@ void Ekf::controlOpticalFlowFusion()
 
 	// Accumulate autopilot gyro data across the same time interval as the flow sensor
 	const Vector3f delta_angle(_imu_sample_delayed.delta_ang - (getGyroBias() * _imu_sample_delayed.delta_ang_dt));
+
 	if (_delta_time_of < 0.1f) {
 		_imu_del_ang_of += delta_angle;
 		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
@@ -142,10 +143,14 @@ void Ekf::controlOpticalFlowFusion()
 		const bool preflight_motion_not_ok = !_control_status.flags.in_air
 						     && ((_imu_sample_delayed.time_us > (_time_good_motion_us + (uint64_t)1E5))
 								     || (_imu_sample_delayed.time_us < (_time_bad_motion_us + (uint64_t)5E6)));
+
 		const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid();
 
+		const bool starting_conditions_passing = (_flow_sample_delayed.quality >= _params.flow_qual_min)
+				&& isTerrainEstimateValid();
+
 		const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required)
-				    || !_control_status.flags.tilt_align;
+					      || !_control_status.flags.tilt_align;
 
 		// Handle cases where we are using optical flow but we should not use it anymore
 		if (_control_status.flags.opt_flow) {
@@ -160,7 +165,8 @@ void Ekf::controlOpticalFlowFusion()
 		// optical flow fusion mode selection logic
 		if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user
 		    && !_control_status.flags.opt_flow // we are not yet using flow data
-		    && !inhibit_flow_use) {
+		    && !inhibit_flow_use
+		    && starting_conditions_passing) {
 
 			// set the flag and reset the fusion timeout
 			ECL_INFO("starting optical flow fusion");
@@ -195,7 +201,7 @@ void Ekf::controlOpticalFlowFusion()
 			if (_imu_sample_delayed.time_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) {
 				// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
-				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
+				if (isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) {
 					fuseOptFlow();
 					_last_known_pos.xy() = _state.pos.xy();
 				}
@@ -204,8 +210,10 @@ void Ekf::controlOpticalFlowFusion()
 			}
 
 			// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
-			if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)
-			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) {
+			if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)
+			    && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)
+			    && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)
+			   ) {
 
 				ECL_INFO("reset velocity to flow");
 				_information_events.flags.reset_vel_to_flow = true;