ekf2: fully disable yaw estimator EKFGSF_yaw with CONFIG_EKF2_GNSS (#22233)

src/modules/ekf2/EKF/CMakeLists.txt

@@ -38,7 +38,6 @@ list(APPEND EKF_SRCS
 	covariance.cpp
 	ekf.cpp
 	ekf_helper.cpp
-	EKFGSF_yaw.cpp
 	estimator_interface.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
@@ -85,6 +84,7 @@ if(CONFIG_EKF2_GNSS)
 		gnss_height_control.cpp
 		gps_checks.cpp
 		gps_control.cpp
+		EKFGSF_yaw.cpp
 	)
 endif()
 

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -62,6 +62,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		_control_status.flags.wind = false;
 	}
 
+#if defined(CONFIG_EKF2_GNSS)
 	// 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) {
@@ -73,6 +74,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			_yawEstimator.setTrueAirspeed(0.f);
 		}
 	}
+#endif // CONFIG_EKF2_GNSS
 
 	if (_params.arsp_thr <= 0.f) {
 		stopAirspeedFusion();
@@ -99,7 +101,9 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 					fuseAirspeed(airspeed_sample, _aid_src_airspeed);
 				}
 
+#if defined(CONFIG_EKF2_GNSS)
 				_yawEstimator.setTrueAirspeed(airspeed_sample.true_airspeed);
+#endif // CONFIG_EKF2_GNSS
 
 				const bool is_fusion_failing = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
 
@@ -219,8 +223,11 @@ 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;
+
+#if defined(CONFIG_EKF2_GNSS)
+		_yawEstimator.setTrueAirspeed(NAN);
+#endif // CONFIG_EKF2_GNSS
 	}
 }
 

src/modules/ekf2/EKF/ekf.h

@@ -45,7 +45,10 @@
 
 #include "estimator_interface.h"
 
-#include "EKFGSF_yaw.h"
+#if defined(CONFIG_EKF2_GNSS)
+# include "EKFGSF_yaw.h"
+#endif // CONFIG_EKF2_GNSS
+
 #include "bias_estimator.hpp"
 #include "height_bias_estimator.hpp"
 #include "position_bias_estimator.hpp"
@@ -492,9 +495,6 @@ public:
 	Vector3f calcRotVecVariances() const;
 	float getYawVar() const;
 
-	// Returns true if the output of the yaw emergency estimator can be used for a reset
-	bool isYawEmergencyEstimateAvailable() const;
-
 	uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
 
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
@@ -537,11 +537,6 @@ public:
 
 	bool gps_checks_passed() const { return _gps_checks_passed; };
 
-	// get solution data from the EKF-GSF emergency yaw estimator
-	// returns false when data is not available
-	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
-			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
-
 	const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
 
 	const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
@@ -551,6 +546,15 @@ public:
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
 # endif // CONFIG_EKF2_GNSS_YAW
+
+	// Returns true if the output of the yaw emergency estimator can be used for a reset
+	bool isYawEmergencyEstimateAvailable() const;
+
+	// get solution data from the EKF-GSF emergency yaw estimator
+	// returns false when data is not available
+	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
+			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
+
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -1090,7 +1094,6 @@ private:
 	void stopGpsFusion();
 
 	bool shouldResetGpsFusion() const;
-	bool isYawFailure() const;
 
 	// return true id the GPS quality is good enough to set an origin and start aiding
 	bool gps_is_good(const gpsMessage &gps);
@@ -1098,11 +1101,6 @@ private:
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
 	void stopGpsHgtFusion();
 
-	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
-	// Resets the horizontal velocity and position to the default navigation sensor
-	// Returns true if the reset was successful
-	bool resetYawToEKFGSF();
-
 	void resetGpsDriftCheckFilters();
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
@@ -1119,6 +1117,17 @@ private:
 	void updateGpsYaw(const gpsSample &gps_sample);
 
 # endif // CONFIG_EKF2_GNSS_YAW
+
+	// Declarations used to control use of the EKF-GSF yaw estimator
+	bool isYawFailure() const;
+
+	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
+	// Returns true if the reset was successful
+	bool resetYawToEKFGSF();
+
+	// yaw estimator instance
+	EKFGSF_yaw _yawEstimator{};
+
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -1237,11 +1246,6 @@ private:
 	// yaw_variance : yaw error variance (rad^2)
 	void resetQuatStateYaw(float yaw, float yaw_variance);
 
-	// Declarations used to control use of the EKF-GSF yaw estimator
-
-	// yaw estimator instance
-	EKFGSF_yaw _yawEstimator{};
-
 	uint8_t _height_sensor_ref{HeightSensor::UNKNOWN};
 	uint8_t _position_sensor_ref{static_cast<uint8_t>(PositionSensor::GNSS)};
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1028,56 +1028,6 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 	_time_last_heading_fuse = _time_delayed_us;
 }
 
-#if defined(CONFIG_EKF2_GNSS)
-bool Ekf::resetYawToEKFGSF()
-{
-	if (!isYawEmergencyEstimateAvailable()) {
-		return false;
-	}
-
-	// don't allow reset if there's just been a yaw reset
-	const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
-	const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
-
-	if (yaw_alignment_changed || quat_reset) {
-		return false;
-	}
-
-	ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
-		 (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
-
-	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
-
-	_control_status.flags.yaw_align = true;
-	_information_events.flags.yaw_aligned_to_imu_gps = true;
-
-	return true;
-}
-#endif // CONFIG_EKF2_GNSS
-
-bool Ekf::isYawEmergencyEstimateAvailable() const
-{
-#if defined(CONFIG_EKF2_GNSS)
-	// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
-	// data and the yaw estimate has converged
-	if (!_yawEstimator.isActive()) {
-		return false;
-	}
-
-	return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
-#else
-	return false;
-#endif
-}
-
-#if defined(CONFIG_EKF2_GNSS)
-bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
-			float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
-{
-	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
-}
-#endif // CONFIG_EKF2_GNSS
-
 #if defined(CONFIG_EKF2_WIND)
 void Ekf::resetWind()
 {

src/modules/ekf2/EKF/gps_control.cpp

@@ -302,18 +302,6 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_inflight_nav_failure);
 }
 
-bool Ekf::isYawFailure() const
-{
-	if (!isYawEmergencyEstimateAvailable()) {
-		return false;
-	}
-
-	const float euler_yaw = getEulerYaw(_R_to_earth);
-	const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
-
-	return fabsf(yaw_error) > math::radians(25.f);
-}
-
 #if defined(CONFIG_EKF2_GNSS_YAW)
 void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
 {
@@ -436,3 +424,57 @@ void Ekf::stopGpsFusion()
 	stopGpsYawFusion();
 #endif // CONFIG_EKF2_GNSS_YAW
 }
+
+bool Ekf::isYawEmergencyEstimateAvailable() const
+{
+	// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
+	// data and the yaw estimate has converged
+	if (!_yawEstimator.isActive()) {
+		return false;
+	}
+
+	return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
+}
+
+bool Ekf::isYawFailure() const
+{
+	if (!isYawEmergencyEstimateAvailable()) {
+		return false;
+	}
+
+	const float euler_yaw = getEulerYaw(_R_to_earth);
+	const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
+
+	return fabsf(yaw_error) > math::radians(25.f);
+}
+
+bool Ekf::resetYawToEKFGSF()
+{
+	if (!isYawEmergencyEstimateAvailable()) {
+		return false;
+	}
+
+	// don't allow reset if there's just been a yaw reset
+	const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
+	const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
+
+	if (yaw_alignment_changed || quat_reset) {
+		return false;
+	}
+
+	ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
+		 (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
+
+	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
+
+	_control_status.flags.yaw_align = true;
+	_information_events.flags.yaw_aligned_to_imu_gps = true;
+
+	return true;
+}
+
+bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
+			float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
+{
+	return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
+}

src/modules/ekf2/EKF/mag_control.cpp

@@ -155,7 +155,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 						* Vector3f(_mag_strength_gps, 0, 0);
 
 		// mag_B: reset
+#if defined(CONFIG_EKF2_GNSS)
 		if (isYawEmergencyEstimateAvailable()) {
+
 			const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
 			const Dcmf R_to_body = R_to_earth.transpose();
 
@@ -165,6 +167,9 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 			ECL_INFO("resetMagStates using yaw estimator");
 
 		} else if (!reset_heading && _control_status.flags.yaw_align) {
+#else
+		if (!reset_heading && _control_status.flags.yaw_align) {
+#endif
 			// mag_B: reset using WMM
 			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
 			_state.mag_B = mag - (R_to_body * mag_earth_pred);

src/modules/ekf2/EKF2.hpp

@@ -507,7 +507,9 @@ private:
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_hgt_pub{ORB_ID(estimator_aid_src_gnss_hgt)};
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)};
 	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)};
+
 	uORB::PublicationMulti<yaw_estimator_status_s> _yaw_est_pub{ORB_ID(yaw_estimator_status)};
+
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	hrt_abstime _status_gnss_yaw_pub_last {0};
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};