ekf2: let controlAirDataFusion() update yaw estimator TAS

src/modules/ekf2/EKF/EKFGSF_yaw.cpp

@@ -44,12 +44,8 @@ EKFGSF_yaw::EKFGSF_yaw()
 
 void EKFGSF_yaw::update(const imuSample &imu_sample,
 			bool run_EKF,			// set to true when flying or movement is suitable for yaw estimation
-			float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
 			const Vector3f &imu_gyro_bias)  // estimated rate gyro bias (rad/sec)
 {
-	// copy to class variables
-	_true_airspeed = airspeed;
-
 	// to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant
 	const float filter_coef = fminf(10.f * imu_sample.delta_vel_dt * _tilt_gain, 1.f);
 	const Vector3f accel = imu_sample.delta_vel / fmaxf(imu_sample.delta_vel_dt, 0.001f);
@@ -177,7 +173,7 @@ void EKFGSF_yaw::ahrsPredict(const uint8_t model_index, const Vector3f &delta_an
 
 		Vector3f accel = _ahrs_accel;
 
-		if (_true_airspeed > FLT_EPSILON) {
+		if (PX4_ISFINITE(_true_airspeed) && (_true_airspeed > FLT_EPSILON)) {
 			// Calculate body frame centripetal acceleration with assumption X axis is aligned with the airspeed vector
 			// Use cross product of body rate and body frame airspeed vector
 			const Vector3f centripetal_accel_bf = Vector3f(0.0f, _true_airspeed * ang_rate(2), - _true_airspeed * ang_rate(1));
@@ -428,10 +424,9 @@ float EKFGSF_yaw::ahrsCalcAccelGain() const
 	// see https://www.desmos.com/calculator/dbqbxvnwfg
 
 	float attenuation = 2.f;
-	const bool centripetal_accel_compensation_enabled = (_true_airspeed > FLT_EPSILON);
+	const bool centripetal_accel_compensation_enabled = PX4_ISFINITE(_true_airspeed) && (_true_airspeed > FLT_EPSILON);
 
-	if (centripetal_accel_compensation_enabled
+	if (centripetal_accel_compensation_enabled && (_ahrs_accel_norm > CONSTANTS_ONE_G)) {
-	    && _ahrs_accel_norm > CONSTANTS_ONE_G) {
 		attenuation = 1.f;
 	}
 

src/modules/ekf2/EKF/EKFGSF_yaw.h

@@ -62,12 +62,14 @@ public:
 	// Update Filter States - this should be called whenever new IMU data is available
 	void update(const imuSample &imu_sample,
 		    bool run_EKF,  			// set to true when flying or movement is suitable for yaw estimation
-		    float airspeed,			// true airspeed used for centripetal accel compensation - set to 0 when not required.
 		    const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
 
 	void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
 			 float accuracy);	   // 1-sigma accuracy of velocity measurement (m/s)
 
+
+	void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; }
+
 	// get solution data for logging
 	bool getLogData(float *yaw_composite,
 			float *yaw_composite_variance,
@@ -77,6 +79,7 @@ public:
 			float weight[N_MODELS_EKFGSF]) const;
 
 	bool isActive() const { return _ekf_gsf_vel_fuse_started; }
+
 	float getYaw() const { return _gsf_yaw; }
 	float getYawVar() const { return _gsf_yaw_variance; }
 
@@ -89,7 +92,7 @@ private:
 	const float _gyro_bias_gain{0.04f};	// gain applied to integral of gyro correction for complementary filter (1/sec)
 
 	// Declarations used by the bank of N_MODELS_EKFGSF AHRS complementary filters
-	float _true_airspeed{};	// true airspeed used for centripetal accel compensation (m/s)
+	float _true_airspeed{NAN};	// true airspeed used for centripetal accel compensation (m/s)
 
 	struct {
 		Dcmf R{matrix::eye<float, 3>()}; // matrix that rotates a vector from body to earth frame

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -61,18 +61,27 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		_control_status.flags.wind = false;
 	}
 
+	// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
+	if (_control_status.flags.fixed_wing) {
+		if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
+			if (!_control_status.flags.fuse_aspd) {
+				_yawEstimator.setTrueAirspeed(_params.EKFGSF_tas_default);
+			}
+
+		} else {
+			_yawEstimator.setTrueAirspeed(0.f);
+		}
+	}
+
 	if (_params.arsp_thr <= 0.f) {
 		stopAirspeedFusion();
 		return;
 	}
 
-	if (_airspeed_buffer) {
+	if (_airspeed_buffer && _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed)) {
-		_tas_data_ready = _airspeed_buffer->pop_first_older_than(imu_delayed.time_us, &_airspeed_sample_delayed);
-	}
 
-	const airspeedSample &airspeed_sample = _airspeed_sample_delayed;
+		const airspeedSample &airspeed_sample = _airspeed_sample_delayed;
 
-	if (_tas_data_ready) {
 		updateAirspeed(airspeed_sample, _aid_src_airspeed);
 
 		_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
@@ -89,6 +98,8 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 					fuseAirspeed(airspeed_sample, _aid_src_airspeed);
 				}
 
+				_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
+
 				const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
 
 				if (is_fusion_failing) {
@@ -113,7 +124,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			_control_status.flags.fuse_aspd = true;
 		}
 
-	} else if (_control_status.flags.fuse_aspd && !isRecent(airspeed_sample.time_us, (uint64_t)1e6)) {
+	} else if (_control_status.flags.fuse_aspd && !isRecent(_airspeed_sample_delayed.time_us, (uint64_t)1e6)) {
 		ECL_WARN("Airspeed data stopped");
 		stopAirspeedFusion();
 	}
@@ -206,6 +217,7 @@ void Ekf::stopAirspeedFusion()
 	if (_control_status.flags.fuse_aspd) {
 		ECL_INFO("stopping airspeed fusion");
 		resetEstimatorAidStatus(_aid_src_airspeed);
+		_yawEstimator.setTrueAirspeed(NAN);
 		_control_status.flags.fuse_aspd = false;
 	}
 }

src/modules/ekf2/EKF/ekf.h

@@ -489,7 +489,6 @@ private:
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
-	bool _tas_data_ready{false};	///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
 
 	uint64_t _time_prev_gps_us{0};	///< time stamp of previous GPS data retrieved from the buffer (uSec)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1348,19 +1348,7 @@ bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_M
 
 void Ekf::runYawEKFGSF(const imuSample &imu_delayed)
 {
-	float TAS = 0.f;
+	_yawEstimator.update(imu_delayed, _control_status.flags.in_air, getGyroBias());
-
-	if (_control_status.flags.fixed_wing) {
-		if (isTimedOut(_airspeed_sample_delayed.time_us, 1000000)) {
-			TAS = _params.EKFGSF_tas_default;
-
-		} else if (_airspeed_sample_delayed.true_airspeed >= _params.arsp_thr) {
-			TAS = _airspeed_sample_delayed.true_airspeed;
-		}
-	}
-
-	const Vector3f imu_gyro_bias = getGyroBias();
-	_yawEstimator.update(imu_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
 }
 
 void Ekf::resetGpsDriftCheckFilters()