ekf2: refactor airspeed fusion control logic

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -57,7 +57,7 @@ void Ekf::fuseAirspeed()
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
 			       math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
 
-	// determine if we need the sideslip fusion to correct states other than wind
+	// determine if we need the airspeed fusion to correct states other than wind
 	const bool update_wind_only = !_is_wind_dead_reckoning;
 
 	// Intermediate variables
@@ -165,7 +165,6 @@ void Ekf::fuseAirspeed()
 
 	if (is_fused) {
 		_time_last_arsp_fuse = _time_last_imu;
-		_control_status.flags.fuse_aspd = true;
 	}
 }
 

src/modules/ekf2/EKF/control.cpp

@@ -1302,29 +1302,53 @@ void Ekf::controlAirDataFusion()
 	const bool airspeed_timed_out = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
 	const bool sideslip_timed_out = isTimedOut(_time_last_beta_fuse, (uint64_t)10e6);
 
-	if (_control_status.flags.wind &&
-	    (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)))) {
+	if (_using_synthetic_position || (airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG))) {
 		_control_status.flags.wind = false;
 	}
 
-	if (_control_status.flags.fuse_aspd && airspeed_timed_out) {
-		_control_status.flags.fuse_aspd = false;
+	if (_params.arsp_thr <= 0.f) {
+		stopAirspeedFusion();
+		return;
 	}
 
-	// Always try to fuse airspeed data if available and we are in flight
-	if (!_using_synthetic_position && _tas_data_ready && _control_status.flags.in_air) {
-		// If starting wind state estimation, reset the wind states and covariances before fusing any data
-		if (!_control_status.flags.wind) {
-			// activate the wind states
-			_control_status.flags.wind = true;
-			// reset the timout timer to prevent repeated resets
+	if (_tas_data_ready) {
+		const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position;
+		const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr;
+		const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant;
+
+		if (_control_status.flags.fuse_aspd) {
+			if (continuing_conditions_passing) {
+				if (is_airspeed_significant) {
+					fuseAirspeed();
+				}
+
+				const bool is_fusion_failing = isTimedOut(_time_last_arsp_fuse, (uint64_t)10e6);
+
+				if (is_fusion_failing) {
+					stopAirspeedFusion();
+				}
+
+			} else {
+				stopAirspeedFusion();
+			}
+
+		} else if (starting_conditions_passing) {
+			// If starting wind state estimation, reset the wind states and covariances before fusing any data
+			if (!_control_status.flags.wind) {
+				// activate the wind states
+				_control_status.flags.wind = true;
+				// reset the wind speed states and corresponding covariances
+				resetWindStates();
+				resetWindCovariance();
+			}
+
+			startAirspeedFusion();
 			_time_last_arsp_fuse = _time_last_imu;
-			// reset the wind speed states and corresponding covariances
-			resetWindStates();
-			resetWindCovariance();
 		}
 
-		fuseAirspeed();
+	} else if (_control_status.flags.fuse_aspd && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us > (uint64_t) 1e6)) {
+		ECL_WARN("Airspeed data stopped");
+		stopAirspeedFusion();
 	}
 }
 

src/modules/ekf2/EKF/ekf.h

@@ -954,6 +954,9 @@ private:
 		return sensor_timestamp + acceptance_interval > _time_last_imu;
 	}
 
+	void startAirspeedFusion();
+	void stopAirspeedFusion();
+
 	void startGpsFusion();
 	void stopGpsFusion();
 	void stopGpsPosFusion();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1480,6 +1480,14 @@ void Ekf::loadMagCovData()
 	P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
 }
 
+void Ekf::startAirspeedFusion() {
+	_control_status.flags.fuse_aspd = true;
+}
+
+void Ekf::stopAirspeedFusion() {
+	_control_status.flags.fuse_aspd = false;
+}
+
 void Ekf::startGpsFusion()
 {
 	resetHorizontalPositionToGps();
@@ -1723,13 +1731,15 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
 
 void Ekf::runYawEKFGSF()
 {
-	float TAS;
+	float TAS = 0.f;
 
-	if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000) && _control_status.flags.fixed_wing) {
-		TAS = _params.EKFGSF_tas_default;
+	if (_control_status.flags.fixed_wing) {
+		if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000)) {
+			TAS = _params.EKFGSF_tas_default;
 
-	} else {
-		TAS = _airspeed_sample_delayed.true_airspeed;
+		} else if (_airspeed_sample_delayed.true_airspeed >= _params.arsp_thr) {
+			TAS = _airspeed_sample_delayed.true_airspeed;
+		}
 	}
 
 	const Vector3f imu_gyro_bias = getGyroBias();