ekf2: add kconfig option to enable/disable GNSS yaw

boards/px4/fmu-v2/default.px4board

@@ -25,6 +25,7 @@ CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set
+# CONFIG_EKF2_GNSS_YAW is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y

src/modules/ekf2/CMakeLists.txt

@@ -88,7 +88,6 @@ list(APPEND EKF_SRCS
 	EKF/gnss_height_control.cpp
 	EKF/gps_checks.cpp
 	EKF/gps_control.cpp
-	EKF/gps_yaw_fusion.cpp
 	EKF/gravity_fusion.cpp
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
@@ -111,6 +110,10 @@ if(CONFIG_EKF2_DRAG_FUSION)
 	list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_GNSS_YAW)
+	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
+endif()
+
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2

src/modules/ekf2/EKF/CMakeLists.txt

@@ -53,7 +53,6 @@ list(APPEND EKF_SRCS
 	gnss_height_control.cpp
 	gps_checks.cpp
 	gps_control.cpp
-	gps_yaw_fusion.cpp
 	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
@@ -76,6 +75,10 @@ if(CONFIG_EKF2_DRAG_FUSION)
 	list(APPEND EKF_SRCS drag_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_GNSS_YAW)
+	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
+endif()
+
 add_library(ecl_EKF
 	${EKF_SRCS}
 )

src/modules/ekf2/EKF/common.h

@@ -359,8 +359,10 @@ struct parameters {
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
 	float mag_yaw_rate_gate{0.20f};         ///< yaw rate threshold used by mode select logic (rad/sec)
 
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	// GNSS heading fusion
 	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	// airspeed fusion
 	float tas_innov_gate{5.0f};             ///< True Airspeed innovation consistency gate size (STD)

src/modules/ekf2/EKF/ekf.cpp

@@ -130,7 +130,10 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_gnss_hgt);
 	resetEstimatorAidStatus(_aid_src_gnss_pos);
 	resetEstimatorAidStatus(_aid_src_gnss_vel);
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	resetEstimatorAidStatus(_aid_src_gnss_yaw);
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	resetEstimatorAidStatus(_aid_src_mag_heading);
 	resetEstimatorAidStatus(_aid_src_mag);

src/modules/ekf2/EKF/ekf.h

@@ -123,15 +123,24 @@ public:
 	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov = _aid_src_mag_heading.innovation;
+			return;
+		}
 
-		} else if (_control_status.flags.mag_3D) {
+		if (_control_status.flags.mag_3D) {
 			heading_innov = Vector3f(_aid_src_mag.innovation).max();
+			return;
+		}
 
-		} else if (_control_status.flags.gps_yaw) {
+#if defined(CONFIG_EKF2_GNSS_YAW)
+		if (_control_status.flags.gps_yaw) {
 			heading_innov = _aid_src_gnss_yaw.innovation;
+			return;
+		}
+#endif // CONFIG_EKF2_GNSS_YAW
 
-		} else if (_control_status.flags.ev_yaw) {
+		if (_control_status.flags.ev_yaw) {
 			heading_innov = _aid_src_ev_yaw.innovation;
+			return;
 		}
 	}
 
@@ -139,15 +148,24 @@ public:
 	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_var = _aid_src_mag_heading.innovation_variance;
+			return;
+		}
 
-		} else if (_control_status.flags.mag_3D) {
+		if (_control_status.flags.mag_3D) {
 			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
+			return;
+		}
 
-		} else if (_control_status.flags.gps_yaw) {
+#if defined(CONFIG_EKF2_GNSS_YAW)
+		if (_control_status.flags.gps_yaw) {
 			heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
+			return;
+		}
+#endif // CONFIG_EKF2_GNSS_YAW
 
-		} else if (_control_status.flags.ev_yaw) {
+		if (_control_status.flags.ev_yaw) {
 			heading_innov_var = _aid_src_ev_yaw.innovation_variance;
+			return;
 		}
 	}
 
@@ -155,15 +173,24 @@ public:
 	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
+			return;
+		}
 
-		} else if (_control_status.flags.mag_3D) {
+		if (_control_status.flags.mag_3D) {
 			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
+			return;
+		}
 
-		} else if (_control_status.flags.gps_yaw) {
+#if defined(CONFIG_EKF2_GNSS_YAW)
+		if (_control_status.flags.gps_yaw) {
 			heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
+			return;
+		}
+#endif // CONFIG_EKF2_GNSS_YAW
 
-		} else if (_control_status.flags.ev_yaw) {
+		if (_control_status.flags.ev_yaw) {
 			heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
+			return;
 		}
 	}
 
@@ -434,7 +461,10 @@ public:
 	const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
 	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
 	const auto &aid_src_mag() const { return _aid_src_mag; }
@@ -506,7 +536,6 @@ private:
 	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
 	uint64_t _time_last_healthy_rng_data{0};
-	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 
 	uint8_t _nb_ev_pos_reset_available{0};
 	uint8_t _nb_ev_vel_reset_available{0};
@@ -574,7 +603,11 @@ private:
 	estimator_aid_source1d_s _aid_src_gnss_hgt{};
 	estimator_aid_source2d_s _aid_src_gnss_pos{};
 	estimator_aid_source3d_s _aid_src_gnss_vel{};
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	estimator_aid_source1d_s _aid_src_gnss_yaw{};
+	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	estimator_aid_source1d_s _aid_src_mag_heading{};
 	estimator_aid_source3d_s _aid_src_mag{};
@@ -668,6 +701,9 @@ private:
 	bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const;
 
+#if defined(CONFIG_EKF2_GNSS_YAW)
+	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
+
 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();
 
@@ -675,6 +711,13 @@ private:
 	// return true if the reset was successful
 	bool resetYawToGps(const float gnss_yaw);
 
+	void updateGpsYaw(const gpsSample &gps_sample);
+
+	void startGpsYawFusion(const gpsSample &gps_sample);
+
+#endif // CONFIG_EKF2_GNSS_YAW
+	void stopGpsYawFusion();
+
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
 	bool fuseDeclination(float decl_sigma);
@@ -741,8 +784,6 @@ private:
 	void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
 	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
 
-	void updateGpsYaw(const gpsSample &gps_sample);
-
 	// calculate optical flow body angular rate compensation
 	// returns false if bias corrected body rate data is unavailable
 	bool calcOptFlowBodyRateComp();
@@ -894,8 +935,6 @@ private:
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;
 
-	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
-
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 
@@ -1034,9 +1073,6 @@ private:
 	void stopGpsPosFusion();
 	void stopGpsVelFusion();
 
-	void startGpsYawFusion(const gpsSample &gps_sample);
-	void stopGpsYawFusion();
-
 	void stopEvVelFusion();
 	void stopEvYawFusion();
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -619,19 +619,22 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	status = _innov_check_fail_status.value;
 
 	// return the largest magnetometer innovation test ratio
+	mag = 0.f;
+
 	if (_control_status.flags.mag_hdg) {
-		mag = sqrtf(_aid_src_mag_heading.test_ratio);
+		mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
-
-	} else if (_control_status.flags.mag_3D) {
-		mag = sqrtf(Vector3f(_aid_src_mag.test_ratio).max());
-
-	} else if (_control_status.flags.gps_yaw) {
-		mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
-
-	} else {
-		mag = NAN;
 	}
 
+	if (_control_status.flags.mag_3D) {
+		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
+	}
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
+	if (_control_status.flags.gps_yaw) {
+		mag = math::max(mag, sqrtf(_aid_src_gnss_yaw.test_ratio));
+	}
+#endif // CONFIG_EKF2_GNSS_YAW
+
 	// return the largest velocity and position innovation test ratio
 	vel = NAN;
 	pos = NAN;

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -168,7 +168,17 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 
 		gps_sample_new.hgt = (float)gps.alt * 1e-3f;
 
-		gps_sample_new.yaw = gps.yaw;
+#if defined(CONFIG_EKF2_GNSS_YAW)
+
+		if (PX4_ISFINITE(gps.yaw)) {
+			_time_last_gps_yaw_buffer_push = _time_latest_us;
+			gps_sample_new.yaw = gps.yaw;
+			gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
+
+		} else {
+			gps_sample_new.yaw = NAN;
+			gps_sample_new.yaw_acc = 0.f;
+		}
 
 		if (PX4_ISFINITE(gps.yaw_offset)) {
 			_gps_yaw_offset = gps.yaw_offset;
@@ -177,12 +187,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 			_gps_yaw_offset = 0.0f;
 		}
 
-		if (PX4_ISFINITE(gps.yaw_accuracy)) {
+#endif // CONFIG_EKF2_GNSS_YAW
-			gps_sample_new.yaw_acc = gps.yaw_accuracy;
-
-		} else {
-			gps_sample_new.yaw_acc = 0.f;
-		}
 
 		// Only calculate the relative position if the WGS-84 location of the origin is set
 		if (collect_gps(gps)) {
@@ -196,9 +201,6 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 		_gps_buffer->push(gps_sample_new);
 		_time_last_gps_buffer_push = _time_latest_us;
 
-		if (PX4_ISFINITE(gps.yaw)) {
-			_time_last_gps_yaw_buffer_push = _time_latest_us;
-		}
 
 	} else {
 		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);

src/modules/ekf2/EKF/estimator_interface.h

@@ -312,10 +312,12 @@ protected:
 	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
 	MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
 	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
-
 	// innovation consistency check monitoring ratios
 	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
+	uint64_t _time_last_gps_yaw_buffer_push{0};
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio
 
@@ -353,7 +355,6 @@ protected:
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
 	uint64_t _time_last_gps_buffer_push{0};
-	uint64_t _time_last_gps_yaw_buffer_push{0};
 	uint64_t _time_last_mag_buffer_push{0};
 	uint64_t _time_last_baro_buffer_push{0};
 	uint64_t _time_last_range_buffer_push{0};

src/modules/ekf2/EKF/gps_control.cpp

@@ -82,7 +82,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 		const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
 		const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
 
+#if defined(CONFIG_EKF2_GNSS_YAW)
 		controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
+#endif // CONFIG_EKF2_GNSS_YAW
 
 		// GNSS velocity
 		const Vector3f velocity{gps_sample.vel};
@@ -297,6 +299,7 @@ bool Ekf::isYawFailure() const
 	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)
 {
 	if (!(_params.gnss_ctrl & GnssCtrl::YAW)
@@ -387,6 +390,31 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 	}
 }
 
+void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
+{
+	if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
+		ECL_INFO("starting GPS yaw fusion");
+		_control_status.flags.yaw_align = true;
+		_control_status.flags.mag_dec = false;
+		stopEvYawFusion();
+		stopMagHdgFusion();
+		stopMag3DFusion();
+		_control_status.flags.gps_yaw = true;
+	}
+}
+#endif // CONFIG_EKF2_GNSS_YAW
+
+void Ekf::stopGpsYawFusion()
+{
+#if defined(CONFIG_EKF2_GNSS_YAW)
+	if (_control_status.flags.gps_yaw) {
+		ECL_INFO("stopping GPS yaw fusion");
+		_control_status.flags.gps_yaw = false;
+		resetEstimatorAidStatus(_aid_src_gnss_yaw);
+	}
+#endif // CONFIG_EKF2_GNSS_YAW
+}
+
 void Ekf::stopGpsFusion()
 {
 	if (_control_status.flags.gps) {
@@ -421,25 +449,3 @@ void Ekf::stopGpsVelFusion()
 
 	resetEstimatorAidStatus(_aid_src_gnss_vel);
 }
-
-void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
-{
-	if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
-		ECL_INFO("starting GPS yaw fusion");
-		_control_status.flags.yaw_align = true;
-		_control_status.flags.mag_dec = false;
-		stopEvYawFusion();
-		stopMagHdgFusion();
-		stopMag3DFusion();
-		_control_status.flags.gps_yaw = true;
-	}
-}
-
-void Ekf::stopGpsYawFusion()
-{
-	if (_control_status.flags.gps_yaw) {
-		ECL_INFO("stopping GPS yaw fusion");
-		_control_status.flags.gps_yaw = false;
-		resetEstimatorAidStatus(_aid_src_gnss_yaw);
-	}
-}

src/modules/ekf2/EKF2.cpp

@@ -264,10 +264,14 @@ bool EKF2::multi_init(int imu, int mag)
 		_estimator_aid_src_gnss_vel_pub.advertise();
 	}
 
+#if defined(CONFIG_EKF2_GNSS_YAW)
+
 	if (_param_ekf2_gps_ctrl.get() & static_cast<int32_t>(GnssCtrl::YAW)) {
 		_estimator_aid_src_gnss_yaw_pub.advertise();
 	}
 
+#endif // CONFIG_EKF2_GNSS_YAW
+
 	// RNG advertise
 	if (_param_ekf2_rng_ctrl.get()) {
 		_estimator_aid_src_rng_hgt_pub.advertise();
@@ -823,7 +827,9 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	PublishAidSourceStatus(_ekf.aid_src_gnss_hgt(), _status_gnss_hgt_pub_last, _estimator_aid_src_gnss_hgt_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub);
 	PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub);
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	// mag heading
 	PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);

src/modules/ekf2/EKF2.hpp

@@ -282,7 +282,9 @@ private:
 	hrt_abstime _status_gnss_hgt_pub_last{0};
 	hrt_abstime _status_gnss_pos_pub_last{0};
 	hrt_abstime _status_gnss_vel_pub_last{0};
-	hrt_abstime _status_gnss_yaw_pub_last{0};
+#if defined(CONFIG_EKF2_GNSS_YAW)
+	hrt_abstime _status_gnss_yaw_pub_last {0};
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	hrt_abstime _status_mag_pub_last{0};
 	hrt_abstime _status_mag_heading_pub_last{0};
@@ -375,7 +377,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<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub{ORB_ID(estimator_aid_src_gnss_yaw)};
+#if defined(CONFIG_EKF2_GNSS_YAW)
+	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)};
+#endif // CONFIG_EKF2_GNSS_YAW
 
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)};
 	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};

src/modules/ekf2/Kconfig

@@ -27,6 +27,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 drag fusion support.
 
+menuconfig EKF2_GNSS_YAW
+depends on MODULES_EKF2
+        bool "GNSS yaw fusion support"
+        default y
+	---help---
+		EKF2 GNSS yaw fusion support.
+
 menuconfig USER_EKF2
 	bool "ekf2 running as userspace module"
 	default n