EKF2: distinguish airspeed source and use synthetic for wind

Enable wind-dead-reckoning with airspeed source to synthetic airspeed

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -131,7 +131,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) {
 				resetVelUsingAirspeed(airspeed_sample);
 
-			} else if (!_external_wind_init
+			} else if (!_external_wind_init && !_synthetic_airspeed
 				   && (!_control_status.flags.wind
 				       || getWindVelocityVariance().longerThan(sq(_params.initial_wind_uncertainty)))) {
 				resetWindUsingAirspeed(airspeed_sample);
@@ -212,6 +212,10 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
+	if (_synthetic_airspeed) {
+		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = 0.f;
+	}
+
 	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
 
 	aid_src.fused = true;

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1259,16 +1259,6 @@ void Ekf::clearInhibitedStateKalmanGains(VectorState &K) const
 	}
 
 #endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_WIND)
-
-	if (!_control_status.flags.wind) {
-		for (unsigned i = 0; i < State::wind_vel.dof; i++) {
-			K(State::wind_vel.idx + i) = 0.f;
-		}
-	}
-
-#endif // CONFIG_EKF2_WIND
 }
 
 float Ekf::getHeadingInnov() const

src/modules/ekf2/EKF/estimator_interface.h

@@ -105,6 +105,7 @@ public:
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	void setAirspeedData(const airspeedSample &airspeed_sample);
+	void setSyntheticAirspeed(const bool synthetic_airspeed) { _synthetic_airspeed = synthetic_airspeed; }
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
@@ -435,6 +436,7 @@ protected:
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	RingBuffer<airspeedSample> *_airspeed_buffer {nullptr};
+	bool _synthetic_airspeed{false};
 #endif // CONFIG_EKF2_AIRSPEED
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)

src/modules/ekf2/EKF2.cpp

@@ -2066,8 +2066,11 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 		if (_airspeed_validated_sub.update(&airspeed_validated)) {
 
 			if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
-			    && (airspeed_validated.selected_airspeed_index > 0)
+			    && (airspeed_validated.airspeed_source > airspeed_validated_s::GROUND_MINUS_WIND)
 			   ) {
+
+				_ekf.setSyntheticAirspeed(airspeed_validated.airspeed_source == airspeed_validated_s::SYNTHETIC);
+
 				float cas2tas = 1.f;
 
 				if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -218,7 +218,7 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning)
 
 	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
 	const Vector3f vel = _ekf->getVelocity();
-	EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 1e-2f);
+	EXPECT_NEAR(vel.norm(), airspeed_body.norm(), 0.05f);
 	const Vector2f vel_wind_earth = _ekf->getWindVelocity();
 	EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f);
 	EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f);