ekf2: add kconfig for magnetometer support (enabled by default)

src/modules/ekf2/CMakeLists.txt

@@ -84,12 +84,9 @@ list(APPEND EKF_SRCS
 	EKF/gravity_fusion.cpp
 	EKF/height_control.cpp
 	EKF/imu_down_sampler.cpp
-	EKF/mag_3d_control.cpp
-	EKF/mag_control.cpp
-	EKF/mag_fusion.cpp
-	EKF/mag_heading_control.cpp
 	EKF/output_predictor.cpp
 	EKF/vel_pos_fusion.cpp
+	EKF/yaw_fusion.cpp
 	EKF/zero_innovation_heading_update.cpp
 	EKF/zero_gyro_update.cpp
 	EKF/zero_velocity_update.cpp
@@ -121,6 +118,15 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_MAGNETOMETER)
+	list(APPEND EKF_SRCS
+		EKF/mag_3d_control.cpp
+		EKF/mag_control.cpp
+		EKF/mag_fusion.cpp
+		EKF/mag_heading_control.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
 		EKF/optical_flow_control.cpp

src/modules/ekf2/EKF/CMakeLists.txt

@@ -49,12 +49,9 @@ list(APPEND EKF_SRCS
 	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
-	mag_3d_control.cpp
-	mag_control.cpp
-	mag_fusion.cpp
-	mag_heading_control.cpp
 	output_predictor.cpp
 	vel_pos_fusion.cpp
+	yaw_fusion.cpp
 	zero_innovation_heading_update.cpp
 	zero_gyro_update.cpp
 	zero_velocity_update.cpp
@@ -86,6 +83,15 @@ if(CONFIG_EKF2_GNSS_YAW)
 	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
 endif()
 
+if(CONFIG_EKF2_MAGNETOMETER)
+	list(APPEND EKF_SRCS
+		mag_3d_control.cpp
+		mag_control.cpp
+		mag_fusion.cpp
+		mag_heading_control.cpp
+	)
+endif()
+
 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
 		optical_flow_control.cpp

src/modules/ekf2/EKF/control.cpp

@@ -102,8 +102,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 		}
 	}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control use of observations for aiding
 	controlMagFusion();
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	controlOpticalFlowFusion(imu_delayed);

src/modules/ekf2/EKF/covariance.cpp

@@ -178,23 +178,21 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	}
 
 	// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_I_sig;
+	float mag_I_sig = 0.0f;
 
 	if (_control_status.flags.mag && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) {
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
-
+#endif // CONFIG_EKF2_MAGNETOMETER
-	} else {
-		mag_I_sig = 0.0f;
 	}
 
 	// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
-	float mag_B_sig;
+	float mag_B_sig = 0.0f;
 
 	if (_control_status.flags.mag && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) {
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
-
+#endif // CONFIG_EKF2_MAGNETOMETER
-	} else {
-		mag_B_sig = 0.0f;
 	}
 
 	float wind_vel_nsd_scaled;
@@ -552,6 +550,7 @@ void Ekf::resetQuatCov(const float yaw_noise)
 
 void Ekf::resetMagCov()
 {
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_mag_decl_cov_reset) {
 		ECL_INFO("reset mag covariance");
 		_mag_decl_cov_reset = false;
@@ -561,6 +560,11 @@ void Ekf::resetMagCov()
 	P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
 
 	saveMagCovData();
+#else
+	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
+	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
+
+#endif
 }
 
 void Ekf::resetGyroBiasZCov()

src/modules/ekf2/EKF/ekf.cpp

@@ -110,7 +110,10 @@ void Ekf::reset()
 	_gps_alt_ref = NAN;
 
 	_baro_counter = 0;
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_mag_counter = 0;
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	_time_bad_vert_accel = 0;
 	_time_good_vert_accel = 0;
@@ -142,8 +145,10 @@ void Ekf::reset()
 	resetEstimatorAidStatus(_aid_src_gnss_yaw);
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	resetEstimatorAidStatus(_aid_src_mag_heading);
 	resetEstimatorAidStatus(_aid_src_mag);
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AUXVEL)
 	resetEstimatorAidStatus(_aid_src_aux_vel);

src/modules/ekf2/EKF/ekf.h

@@ -158,6 +158,7 @@ public:
 
 	void getHeadingInnov(float &heading_innov) const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov = _aid_src_mag_heading.innovation;
 			return;
@@ -166,6 +167,7 @@ public:
 			heading_innov = Vector3f(_aid_src_mag.innovation).max();
 			return;
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -184,6 +186,7 @@ public:
 
 	void getHeadingInnovVar(float &heading_innov_var) const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_var = _aid_src_mag_heading.innovation_variance;
 			return;
@@ -192,6 +195,7 @@ public:
 			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
 			return;
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -210,6 +214,7 @@ public:
 
 	void getHeadingInnovRatio(float &heading_innov_ratio) const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
 			return;
@@ -218,6 +223,7 @@ public:
 			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
 			return;
 		}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		if (_control_status.flags.gps_yaw) {
@@ -234,9 +240,11 @@ public:
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
 	void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
 	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
@@ -351,12 +359,16 @@ public:
 
 	bool isYawFinalAlignComplete() const
 	{
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
 		const bool is_mag_alignment_in_flight_complete = is_using_mag
 				&& _control_status.flags.mag_aligned_in_flight
 				&& ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6);
 		return _control_status.flags.yaw_align
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
+#else
+		return _control_status.flags.yaw_align;
+#endif
 	}
 
 	// gyro bias (states 10, 11, 12)
@@ -369,6 +381,7 @@ public:
 	Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)}; } // get the accelerometer bias variance in m/s**2
 	float getAccelBiasLimit() const { return _params.acc_bias_lim; }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	const Vector3f &getMagEarthField() const { return _state.mag_I; }
 
 	// mag bias (states 19, 20, 21)
@@ -382,6 +395,7 @@ public:
 		return _saved_mag_bf_covmat.diag();
 	}
 	float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; }
 	bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; }
@@ -497,8 +511,10 @@ public:
 	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
 	const auto &aid_src_mag() const { return _aid_src_mag; }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	const auto &aid_src_gravity() const { return _aid_src_gravity; }
 
@@ -545,9 +561,6 @@ private:
 
 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
 
-	float _mag_heading_prev{};                 ///< previous value of mag heading (rad)
-	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)
-
 	// 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
 
@@ -574,19 +587,9 @@ private:
 	Vector3f _zgup_delta_ang{};
 	float _zgup_delta_ang_dt{0.f};
 
-	// used by magnetometer fusion mode selection
 	Vector2f _accel_lpf_NE{};			///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
 	float _yaw_delta_ef{0.0f};		///< Recent change in yaw angle measured about the earth frame D axis (rad)
 	float _yaw_rate_lpf_ef{0.0f};		///< Filtered angular rate about earth frame D axis (rad/sec)
-	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
-	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
-	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
-	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
-	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
-	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
-	uint8_t _nb_mag_heading_reset_available{0};
-	uint8_t _nb_mag_3d_reset_available{0};
-	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
 
 	SquareMatrix24f P{};	///< state covariance matrix
 
@@ -668,9 +671,6 @@ private:
 	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{};
-
 	estimator_aid_source3d_s _aid_src_gravity{};
 
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -694,20 +694,40 @@ private:
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
 	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
-	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
 	// Variables used to perform in flight resets and switch between height sources
-	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
 	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	float _mag_heading_prev{};                 ///< previous value of mag heading (rad)
+	float _mag_heading_pred_prev{};            ///< previous value of yaw state used by mag heading fusion (rad)
+
+	// used by magnetometer fusion mode selection
+	bool _mag_bias_observable{false};	///< true when there is enough rotation to make magnetometer bias errors observable
+	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
+	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
+	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
+	float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad)
+	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
+	uint8_t _nb_mag_heading_reset_available{0};
+	uint8_t _nb_mag_3d_reset_available{0};
+	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
+
+	estimator_aid_source1d_s _aid_src_mag_heading{};
+	estimator_aid_source3d_s _aid_src_mag{};
+
+	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
+	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
+
 	// Variables used to control activation of post takeoff functionality
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	uint64_t _time_last_mag_check_failing{0};
 	Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
 	Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	gps_check_fail_status_u _gps_check_fail_status{};
 
@@ -744,9 +764,6 @@ private:
 	// predict ekf covariance
 	void predictCovariance(const imuSample &imu_delayed);
 
-	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
-
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
@@ -767,12 +784,17 @@ private:
 #endif // CONFIG_EKF2_GNSS_YAW
 	void stopGpsYawFusion();
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	// ekf sequential fusion of magnetometer measurements
+	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true);
+
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
 	bool fuseDeclination(float decl_sigma);
 
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_AIRSPEED)
 	// control fusion of air data observations
@@ -899,8 +921,10 @@ private:
 	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	void clearInhibitedStateKalmanGains(Vector24f &K) const
 	{
@@ -1017,6 +1041,7 @@ private:
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 	void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src);
@@ -1035,6 +1060,19 @@ private:
 	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
 
+	void stopMagHdgFusion();
+	void stopMagFusion();
+
+	// load and save mag field state covariance data for re-use
+	void loadMagCovData();
+	void saveMagCovData();
+
+	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
+	// sensor measurement
+	float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
+
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();
 
@@ -1064,9 +1102,6 @@ private:
 	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
 
-	void stopMagHdgFusion();
-	void stopMagFusion();
-
 	void stopBaroHgtFusion();
 	void stopGpsHgtFusion();
 
@@ -1087,19 +1122,11 @@ private:
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
 
-	// load and save mag field state covariance data for re-use
-	void loadMagCovData();
-	void saveMagCovData();
-
 	void resetGyroBiasZCov();
 
 	// uncorrelate quaternion states from other states
 	void uncorrelateQuatFromOtherStates();
 
-	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
-	// sensor measurement
-	float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);
-
 	bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const
 	{
 		return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -234,7 +234,10 @@ void Ekf::constrainStates()
 	_state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit);
 
 	_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
 }
 
@@ -671,6 +674,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	// return the largest magnetometer innovation test ratio
 	mag = 0.f;
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg) {
 		mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
 	}
@@ -678,6 +682,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	if (_control_status.flags.mag_3D) {
 		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_GNSS_YAW)
 	if (_control_status.flags.gps_yaw) {
@@ -791,6 +796,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 
 	bool mag_innov_good = true;
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_control_status.flags.mag_hdg) {
 		if (_aid_src_mag_heading.test_ratio < 1.f) {
 			mag_innov_good = false;
@@ -801,6 +807,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 			mag_innov_good = false;
 		}
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f;
 	const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f;
@@ -945,26 +952,6 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
 	P.slice<4, 4>(0, 0) += q_cov;
 }
 
-void Ekf::saveMagCovData()
-{
-	// save the NED axis covariance sub-matrix
-	_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
-
-	// save the XYZ body covariance sub-matrix
-	_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
-}
-
-void Ekf::loadMagCovData()
-{
-	// re-instate the NED axis covariance sub-matrix
-	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
-	P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
-
-	// re-instate the XYZ body axis covariance sub-matrix
-	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
-	P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
-}
-
 void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -47,7 +47,9 @@
 EstimatorInterface::~EstimatorInterface()
 {
 	delete _gps_buffer;
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	delete _mag_buffer;
+#endif // CONFIG_EKF2_MAGNETOMETER
 	delete _baro_buffer;
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	delete _range_buffer;
@@ -102,6 +104,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 #endif // CONFIG_EKF2_DRAG_FUSION
 }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
 	if (!_initialised) {
@@ -137,6 +140,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 void EstimatorInterface::setGpsData(const gpsMessage &gps)
 {
@@ -707,9 +711,11 @@ void EstimatorInterface::print_status()
 		printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
 	}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	if (_mag_buffer) {
 		printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	if (_baro_buffer) {
 		printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());

src/modules/ekf2/EKF/estimator_interface.h

@@ -86,7 +86,9 @@ public:
 
 	void setIMUData(const imuSample &imu_sample);
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	void setMagData(const magSample &mag_sample);
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	void setGpsData(const gpsMessage &gps);
 
@@ -231,6 +233,7 @@ public:
 	Vector3f getPosition() const { return _output_predictor.getPosition(); }
 	const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// Get the value of magnetic declination in degrees to be saved for use at the next startup
 	// Returns true when the declination can be saved
 	// At the next startup, set param.mag_declination_deg to the value saved
@@ -263,6 +266,7 @@ public:
 		strength_gs = _mag_strength;
 		strength_ref_gs = _mag_strength_gps;
 	}
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	// get EKF mode status
 	const filter_control_status_u &control_status() const { return _control_status; }
@@ -412,7 +416,12 @@ protected:
 	RingBuffer<imuSample> _imu_buffer{kBufferLengthDefault};
 
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	RingBuffer<magSample> *_mag_buffer{nullptr};
+	uint64_t _time_last_mag_buffer_push{0};
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
 
 #if defined(CONFIG_EKF2_AIRSPEED)
@@ -429,7 +438,6 @@ protected:
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};
 
 	uint64_t _time_last_gps_buffer_push{0};
-	uint64_t _time_last_mag_buffer_push{0};
 	uint64_t _time_last_baro_buffer_push{0};
 
 	uint64_t _time_last_gnd_effect_on{0};

src/modules/ekf2/EKF/ev_yaw_control.cpp

@@ -158,7 +158,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 
 			} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
 				// turn on fusion of external vision yaw measurements and disable all other heading fusion
-				stopMagFusion();
 				stopGpsYawFusion();
 				stopGpsFusion();
 

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -217,3 +217,22 @@ void Ekf::stopMagFusion()
 	}
 }
 
+void Ekf::saveMagCovData()
+{
+	// save the NED axis covariance sub-matrix
+	_saved_mag_ef_covmat = P.slice<3, 3>(16, 16);
+
+	// save the XYZ body covariance sub-matrix
+	_saved_mag_bf_covmat = P.slice<3, 3>(19, 19);
+}
+
+void Ekf::loadMagCovData()
+{
+	// re-instate the NED axis covariance sub-matrix
+	P.uncorrelateCovarianceSetVariance<3>(16, 0.f);
+	P.slice<3, 3>(16, 16) = _saved_mag_ef_covmat;
+
+	// re-instate the XYZ body axis covariance sub-matrix
+	P.uncorrelateCovarianceSetVariance<3>(19, 0.f);
+	P.slice<3, 3>(19, 19) = _saved_mag_bf_covmat;
+}

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -254,114 +254,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 	return false;
 }
 
-// update quaternion states and covariances using the yaw innovation and yaw observation variance
-bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
-{
-	Vector24f H_YAW;
-	computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
-
-	return fuseYaw(aid_src_status, H_YAW);
-}
-
-bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
-{
-	// define the innovation gate size
-	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
-
-	// innovation test ratio
-	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
-
-	if (aid_src_status.fusion_enabled) {
-
-		// check if the innovation variance calculation is badly conditioned
-		if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
-			// the innovation variance contribution from the state covariances is not negative, no fault
-			_fault_status.flags.bad_hdg = false;
-
-		} else {
-			// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-			_fault_status.flags.bad_hdg = true;
-
-			// we reinitialise the covariance matrix and abort this fusion step
-			initialiseCovariance();
-			ECL_ERR("yaw fusion numerical error - covariance reset");
-
-			return false;
-		}
-
-		// calculate the Kalman gains
-		// only calculate gains for states we are using
-		Vector24f Kfusion;
-		const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
-
-		for (uint8_t row = 0; row < _k_num_states; row++) {
-			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row, col) * H_YAW(col);
-			}
-
-			Kfusion(row) *= heading_innov_var_inv;
-		}
-
-		// set the magnetometer unhealthy if the test fails
-		if (aid_src_status.innovation_rejected) {
-			_innov_check_fail_status.flags.reject_yaw = true;
-
-			// if we are in air we don't want to fuse the measurement
-			// we allow to use it when on the ground because the large innovation could be caused
-			// by interference or a large initial gyro bias
-			if (!_control_status.flags.in_air
-			    && isTimedOut(_time_last_in_air, (uint64_t)5e6)
-			    && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
-			   ) {
-				// constrain the innovation to the maximum set by the gate
-				// we need to delay this forced fusion to avoid starting it
-				// immediately after touchdown, when the drone is still armed
-				float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
-				aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
-
-				// also reset the yaw gyro variance to converge faster and avoid
-				// being stuck on a previous bad estimate
-				resetGyroBiasZCov();
-
-			} else {
-				return false;
-			}
-
-		} else {
-			_innov_check_fail_status.flags.reject_yaw = false;
-		}
-
-		if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
-
-			_time_last_heading_fuse = _time_delayed_us;
-
-			aid_src_status.time_last_fuse = _time_delayed_us;
-			aid_src_status.fused = true;
-
-			_fault_status.flags.bad_hdg = false;
-
-			return true;
-
-		} else {
-			_fault_status.flags.bad_hdg = true;
-		}
-	}
-
-	// otherwise
-	aid_src_status.fused = false;
-	return false;
-}
-
-void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
-{
-	if (shouldUse321RotationSequence(_R_to_earth)) {
-		sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
-
-	} else {
-		sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
-	}
-}
-
 bool Ekf::fuseDeclination(float decl_sigma)
 {
 	if (!_control_status.flags.mag) {

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ekf.h"
+
+#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h"
+#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h"
+
+#include <mathlib/mathlib.h>
+
+// update quaternion states and covariances using the yaw innovation and yaw observation variance
+bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
+{
+	Vector24f H_YAW;
+	computeYawInnovVarAndH(aid_src_status.observation_variance, aid_src_status.innovation_variance, H_YAW);
+
+	return fuseYaw(aid_src_status, H_YAW);
+}
+
+bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW)
+{
+	// define the innovation gate size
+	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
+
+	// innovation test ratio
+	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
+
+	if (aid_src_status.fusion_enabled) {
+
+		// check if the innovation variance calculation is badly conditioned
+		if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
+			// the innovation variance contribution from the state covariances is not negative, no fault
+			_fault_status.flags.bad_hdg = false;
+
+		} else {
+			// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+			_fault_status.flags.bad_hdg = true;
+
+			// we reinitialise the covariance matrix and abort this fusion step
+			initialiseCovariance();
+			ECL_ERR("yaw fusion numerical error - covariance reset");
+
+			return false;
+		}
+
+		// calculate the Kalman gains
+		// only calculate gains for states we are using
+		Vector24f Kfusion;
+		const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;
+
+		for (uint8_t row = 0; row < _k_num_states; row++) {
+			for (uint8_t col = 0; col <= 3; col++) {
+				Kfusion(row) += P(row, col) * H_YAW(col);
+			}
+
+			Kfusion(row) *= heading_innov_var_inv;
+		}
+
+		// set the magnetometer unhealthy if the test fails
+		if (aid_src_status.innovation_rejected) {
+			_innov_check_fail_status.flags.reject_yaw = true;
+
+			// if we are in air we don't want to fuse the measurement
+			// we allow to use it when on the ground because the large innovation could be caused
+			// by interference or a large initial gyro bias
+			if (!_control_status.flags.in_air
+			    && isTimedOut(_time_last_in_air, (uint64_t)5e6)
+			    && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6)
+			   ) {
+				// constrain the innovation to the maximum set by the gate
+				// we need to delay this forced fusion to avoid starting it
+				// immediately after touchdown, when the drone is still armed
+				float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
+				aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
+
+				// also reset the yaw gyro variance to converge faster and avoid
+				// being stuck on a previous bad estimate
+				resetGyroBiasZCov();
+
+			} else {
+				return false;
+			}
+
+		} else {
+			_innov_check_fail_status.flags.reject_yaw = false;
+		}
+
+		if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) {
+
+			_time_last_heading_fuse = _time_delayed_us;
+
+			aid_src_status.time_last_fuse = _time_delayed_us;
+			aid_src_status.fused = true;
+
+			_fault_status.flags.bad_hdg = false;
+
+			return true;
+
+		} else {
+			_fault_status.flags.bad_hdg = true;
+		}
+	}
+
+	// otherwise
+	aid_src_status.fused = false;
+	return false;
+}
+
+void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const
+{
+	if (shouldUse321RotationSequence(_R_to_earth)) {
+		sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
+
+	} else {
+		sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW);
+	}
+}

src/modules/ekf2/EKF2.cpp

@@ -60,7 +60,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_params(_ekf.getParamHandle()),
 	_param_ekf2_predict_us(_params->filter_update_interval_us),
 	_param_ekf2_imu_ctrl(_params->imu_ctrl),
-	_param_ekf2_mag_delay(_params->mag_delay_ms),
 	_param_ekf2_baro_delay(_params->baro_delay_ms),
 	_param_ekf2_gps_delay(_params->gps_delay_ms),
 #if defined(CONFIG_EKF2_AUXVEL)
@@ -70,8 +69,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_acc_noise(_params->accel_noise),
 	_param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
 	_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
-	_param_ekf2_mag_e_noise(_params->mage_p_noise),
-	_param_ekf2_mag_b_noise(_params->magb_p_noise),
 	_param_ekf2_wind_nsd(_params->wind_vel_nsd),
 	_param_ekf2_gps_v_noise(_params->gps_vel_noise),
 	_param_ekf2_gps_p_noise(_params->gps_pos_noise),
@@ -93,6 +90,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_beta_noise(_params->beta_noise),
 	_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
 #endif // CONFIG_EKF2_SIDESLIP
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	_param_ekf2_mag_delay(_params->mag_delay_ms),
+	_param_ekf2_mag_e_noise(_params->mage_p_noise),
+	_param_ekf2_mag_b_noise(_params->magb_p_noise),
 	_param_ekf2_head_noise(_params->mag_heading_noise),
 	_param_ekf2_mag_noise(_params->mag_noise),
 	_param_ekf2_mag_decl(_params->mag_declination_deg),
@@ -102,6 +103,11 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_mag_type(_params->mag_fusion_type),
 	_param_ekf2_mag_acclim(_params->mag_acc_gate),
 	_param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate),
+	_param_ekf2_mag_check(_params->mag_check),
+	_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
+	_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
+	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
+#endif // CONFIG_EKF2_MAGNETOMETER
 	_param_ekf2_gps_check(_params->gps_check_mask),
 	_param_ekf2_req_eph(_params->req_hacc),
 	_param_ekf2_req_epv(_params->req_vacc),
@@ -189,10 +195,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
 	_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
 #endif // CONFIG_EKF2_BARO_COMPENSATION
-	_param_ekf2_mag_check(_params->mag_check),
-	_param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs),
-	_param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg),
-	_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
 	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
 {
 	// advertise expected minimal topic set immediately to ensure logging
@@ -225,7 +227,9 @@ EKF2::~EKF2()
 #if defined(CONFIG_EKF2_AUXVEL)
 	perf_free(_msg_missed_landing_target_pose_perf);
 #endif // CONFIG_EKF2_AUXVEL
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	perf_free(_msg_missed_magnetometer_perf);
+#endif // CONFIG_EKF2_MAGNETOMETER
 	perf_free(_msg_missed_odometry_perf);
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	perf_free(_msg_missed_optical_flow_perf);
@@ -312,6 +316,7 @@ bool EKF2::multi_init(int imu, int mag)
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 
 	// mag advertise
 	if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
@@ -319,13 +324,23 @@ bool EKF2::multi_init(int imu, int mag)
 		_estimator_aid_src_mag_pub.advertise();
 	}
 
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 	_attitude_pub.advertise();
 	_local_position_pub.advertise();
 	_global_position_pub.advertise();
 	_odometry_pub.advertise();
 	_wind_pub.advertise();
 
-	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
+	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu);
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+
+	if (!_magnetometer_sub.ChangeInstance(mag)) {
+		changed_instance = false;
+	}
+
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	const int status_instance = _estimator_states_pub.get_instance();
 
@@ -367,7 +382,9 @@ int EKF2::print_status()
 #if defined(CONFIG_EKF2_AUXVEL)
 	perf_print_counter(_msg_missed_landing_target_pose_perf);
 #endif // CONFIG_EKF2_AUXVEL
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	perf_print_counter(_msg_missed_magnetometer_perf);
+#endif // CONFIG_EKF2_MAGNETOMETER
 	perf_print_counter(_msg_missed_odometry_perf);
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	perf_print_counter(_msg_missed_optical_flow_perf);
@@ -435,6 +452,8 @@ void EKF2::Run()
 			}
 		}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+
 		// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
 		if (_params->mag_fusion_type != MagFuseType::NONE) {
 			float sens_mag_rate = 0.f;
@@ -450,6 +469,8 @@ void EKF2::Run()
 				}
 			}
 		}
+
+#endif // CONFIG_EKF2_MAGNETOMETER
 	}
 
 	if (!_callback_registered) {
@@ -678,7 +699,9 @@ void EKF2::Run()
 		UpdateFlowSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 		UpdateGpsSample(ekf2_timestamps);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		UpdateMagSample(ekf2_timestamps);
+#endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 		UpdateRangeSample(ekf2_timestamps);
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -719,7 +742,9 @@ void EKF2::Run()
 
 			UpdateAccelCalibration(now);
 			UpdateGyroCalibration(now);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 			UpdateMagCalibration(now);
+#endif // CONFIG_EKF2_MAGNETOMETER
 			PublishSensorBias(now);
 
 			PublishAidSourceStatus(now);
@@ -918,6 +943,7 @@ void EKF2::VerifyParams()
 
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 
 	// EKF2_MAG_TYPE obsolete options
 	if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO)
@@ -935,6 +961,8 @@ void EKF2::VerifyParams()
 		_param_ekf2_mag_type.set(0);
 		_param_ekf2_mag_type.commit();
 	}
+
+#endif // CONFIG_EKF2_MAGNETOMETER
 }
 
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
@@ -975,11 +1003,13 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 	PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub);
 #endif // CONFIG_EKF2_GNSS_YAW
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	// mag heading
 	PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub);
 
 	// mag 3d
 	PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub);
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	// gravity
 	PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub);
@@ -1291,7 +1321,9 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_ekf.getTerrainFlowInnov(innovations.terr_flow);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 	_ekf.getHeadingInnov(innovations.heading);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_ekf.getMagInnov(innovations.mag_field);
+#endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	_ekf.getDragInnov(innovations.drag);
 #endif // CONFIG_EKF2_DRAG_FUSION
@@ -1369,7 +1401,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 	_ekf.getHeadingInnovRatio(test_ratios.heading);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
+#endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	_ekf.getDragInnovRatio(&test_ratios.drag[0]);
 #endif // CONFIG_EKF2_DRAG_FUSION
@@ -1412,7 +1446,9 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_ekf.getTerrainFlowInnovVar(variances.terr_flow);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 	_ekf.getHeadingInnovVar(variances.heading);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	_ekf.getMagInnovVar(variances.mag_field);
+#endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_DRAG_FUSION)
 	_ekf.getDragInnovVar(variances.drag);
 #endif // CONFIG_EKF2_DRAG_FUSION
@@ -1583,12 +1619,17 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 	// estimator_sensor_bias
 	const Vector3f gyro_bias{_ekf.getGyroBias()};
 	const Vector3f accel_bias{_ekf.getAccelBias()};
-	const Vector3f mag_bias{_ekf.getMagBias()};
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	const Vector3f mag_bias {_ekf.getMagBias()};
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	// publish at ~1 Hz, or sooner if there's a change
 	if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
 	    || (accel_bias - _last_accel_bias_published).longerThan(0.001f)
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	    || (mag_bias - _last_mag_bias_published).longerThan(0.001f)
+#endif // CONFIG_EKF2_MAGNETOMETER
 	    || (timestamp >= _last_sensor_bias_published + 1_s)) {
 
 		estimator_sensor_bias_s bias{};
@@ -1619,6 +1660,8 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			_last_accel_bias_published = accel_bias;
 		}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+
 		if (_device_id_mag != 0) {
 			const Vector3f bias_var{_ekf.getMagBiasVariance()};
 
@@ -1631,6 +1674,8 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			_last_mag_bias_published = mag_bias;
 		}
 
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 		bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_sensor_bias_pub.publish(bias);
 
@@ -1695,10 +1740,13 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
 	status.accel_device_id = _device_id_accel;
 	status.baro_device_id = _device_id_baro;
 	status.gyro_device_id = _device_id_gyro;
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	status.mag_device_id = _device_id_mag;
 
 	_ekf.get_mag_checks(status.mag_inclination_deg, status.mag_inclination_ref_deg, status.mag_strength_gs,
 			    status.mag_strength_ref_gs);
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_status_pub.publish(status);
@@ -2313,6 +2361,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 	}
 }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	const unsigned last_generation = _magnetometer_sub.get_last_generation();
@@ -2356,6 +2405,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
@@ -2556,6 +2606,7 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 			  bias_valid, learning_valid);
 }
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
 	const Vector3f mag_bias = _ekf.getMagBias();
@@ -2585,6 +2636,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 		}
 	}
 }
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 int EKF2::custom_command(int argc, char *argv[])
 {
@@ -2624,6 +2676,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 			imu_instances = imu_instances_limited;
 		}
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 		int32_t sens_mag_mode = 1;
 		const param_t param_sens_mag_mode = param_find("SENS_MAG_MODE");
 		param_get(param_sens_mag_mode, &sens_mag_mode);
@@ -2654,6 +2707,8 @@ int EKF2::task_spawn(int argc, char *argv[])
 		} else {
 			mag_instances = 1;
 		}
+
+#endif // CONFIG_EKF2_MAGNETOMETER
 	}
 
 	if (multi_mode && !replay_mode) {

src/modules/ekf2/EKF2.hpp

@@ -85,7 +85,6 @@
 #include <uORB/topics/vehicle_imu.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
@@ -100,6 +99,10 @@
 # include <uORB/topics/landing_target_pose.h>
 #endif // CONFIG_EKF2_AUXVEL
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+# include <uORB/topics/vehicle_magnetometer.h>
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 # include <uORB/topics/vehicle_optical_flow.h>
 # include <uORB/topics/vehicle_optical_flow_vel.h>
@@ -212,7 +215,9 @@ private:
 	bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
+#endif // CONFIG_EKF2_MAGNETOMETER
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -230,7 +235,9 @@ private:
 			       const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid);
 	void UpdateAccelCalibration(const hrt_abstime &timestamp);
 	void UpdateGyroCalibration(const hrt_abstime &timestamp);
+#if defined(CONFIG_EKF2_MAGNETOMETER)
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
+#endif // CONFIG_EKF2_MAGNETOMETER
 
 	// publish helper for estimator_aid_source topics
 	template <typename T>
@@ -271,15 +278,10 @@ private:
 	perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
 	perf_counter_t _msg_missed_air_data_perf{nullptr};
 	perf_counter_t _msg_missed_gps_perf{nullptr};
-	perf_counter_t _msg_missed_magnetometer_perf{nullptr};
 	perf_counter_t _msg_missed_odometry_perf{nullptr};
 
-	// Used to control saving of mag declination to be used on next startup
-	bool _mag_decl_saved = false;	///< true when the magnetic declination has been saved
-
 	InFlightCalibration _accel_cal{};
 	InFlightCalibration _gyro_cal{};
-	InFlightCalibration _mag_cal{};
 
 	uint64_t _gps_time_usec{0};
 	int32_t _gps_alttitude_ellipsoid{0};			///< altitude in 1E-3 meters (millimeters) above ellipsoid
@@ -289,16 +291,13 @@ private:
 	uint8_t _accel_calibration_count{0};
 	uint8_t _baro_calibration_count{0};
 	uint8_t _gyro_calibration_count{0};
-	uint8_t _mag_calibration_count{0};
 
 	uint32_t _device_id_accel{0};
 	uint32_t _device_id_baro{0};
 	uint32_t _device_id_gyro{0};
-	uint32_t _device_id_mag{0};
 
 	Vector3f _last_accel_bias_published{};
 	Vector3f _last_gyro_bias_published{};
-	Vector3f _last_mag_bias_published{};
 
 	hrt_abstime _last_sensor_bias_published{0};
 	hrt_abstime _last_gps_status_published{0};
@@ -309,6 +308,27 @@ private:
 	hrt_abstime _status_fake_hgt_pub_last{0};
 	hrt_abstime _status_fake_pos_pub_last{0};
 
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+	uint32_t _device_id_mag {0};
+
+	perf_counter_t _msg_missed_magnetometer_perf {nullptr};
+
+	// Used to control saving of mag declination to be used on next startup
+	bool _mag_decl_saved = false;	///< true when the magnetic declination has been saved
+
+	InFlightCalibration _mag_cal{};
+	uint8_t _mag_calibration_count{0};
+	Vector3f _last_mag_bias_published{};
+
+	hrt_abstime _status_mag_pub_last{0};
+	hrt_abstime _status_mag_heading_pub_last{0};
+
+	uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
+
+	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)};
+#endif // CONFIG_EKF2_MAGNETOMETER
+
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	uORB::PublicationMulti<estimator_aid_source1d_s> _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)};
 	uORB::PublicationMulti<estimator_aid_source2d_s> _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)};
@@ -333,8 +353,6 @@ private:
 	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};
 
 	hrt_abstime _status_gravity_pub_last{0};
 
@@ -386,7 +404,6 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
-	uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
@@ -448,9 +465,6 @@ private:
 	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)};
-
 	uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
 
 	// publications with topic dependent on multi-mode
@@ -471,8 +485,6 @@ private:
 		(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
 		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
 
-		(ParamExtFloat<px4::params::EKF2_MAG_DELAY>)
-		_param_ekf2_mag_delay,	///< magnetometer measurement delay relative to the IMU (mSec)
 		(ParamExtFloat<px4::params::EKF2_BARO_DELAY>)
 		_param_ekf2_baro_delay,	///< barometer height measurement delay relative to the IMU (mSec)
 		(ParamExtFloat<px4::params::EKF2_GPS_DELAY>)
@@ -493,10 +505,6 @@ private:
 		_param_ekf2_gyr_b_noise,	///< process noise for IMU rate gyro bias prediction (rad/sec**2)
 		(ParamExtFloat<px4::params::EKF2_ACC_B_NOISE>)
 		_param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3)
-		(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>)
-		_param_ekf2_mag_e_noise,	///< process noise for earth magnetic field prediction (Gauss/sec)
-		(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
-		_param_ekf2_mag_b_noise,	///< process noise for body magnetic field prediction (Gauss/sec)
 		(ParamExtFloat<px4::params::EKF2_WIND_NSD>)
 		_param_ekf2_wind_nsd,	///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
 
@@ -541,25 +549,24 @@ private:
 		_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
 #endif // CONFIG_EKF2_SIDESLIP
 
-		// control of magnetometer fusion
+#if defined(CONFIG_EKF2_MAGNETOMETER)
-		(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
+		(ParamExtFloat<px4::params::EKF2_MAG_DELAY>) _param_ekf2_mag_delay,
-		_param_ekf2_head_noise,	///< measurement noise used for simple heading fusion (rad)
+		(ParamExtFloat<px4::params::EKF2_MAG_E_NOISE>) _param_ekf2_mag_e_noise,
-		(ParamExtFloat<px4::params::EKF2_MAG_NOISE>)
+		(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>) _param_ekf2_mag_b_noise,
-		_param_ekf2_mag_noise,		///< measurement noise used for 3-axis magnetoemeter fusion (Gauss)
+		(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>) _param_ekf2_head_noise,
-
+		(ParamExtFloat<px4::params::EKF2_MAG_NOISE>) _param_ekf2_mag_noise,
-		(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,///< magnetic declination (degrees)
+		(ParamExtFloat<px4::params::EKF2_MAG_DECL>) _param_ekf2_mag_decl,
-		(ParamExtFloat<px4::params::EKF2_HDG_GATE>)
+		(ParamExtFloat<px4::params::EKF2_HDG_GATE>) _param_ekf2_hdg_gate,
-		_param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD)
+		(ParamExtFloat<px4::params::EKF2_MAG_GATE>) _param_ekf2_mag_gate,
-		(ParamExtFloat<px4::params::EKF2_MAG_GATE>)
+		(ParamExtInt<px4::params::EKF2_DECL_TYPE>) _param_ekf2_decl_type,
-		_param_ekf2_mag_gate,	///< magnetometer fusion innovation consistency gate size (STD)
+		(ParamExtInt<px4::params::EKF2_MAG_TYPE>) _param_ekf2_mag_type,
-		(ParamExtInt<px4::params::EKF2_DECL_TYPE>)
+		(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>) _param_ekf2_mag_acclim,
-		_param_ekf2_decl_type,	///< bitmask used to control the handling of declination data
+		(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>) _param_ekf2_mag_yawlim,
-		(ParamExtInt<px4::params::EKF2_MAG_TYPE>)
+		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check,
-		_param_ekf2_mag_type,	///< integer used to specify the type of magnetometer fusion used
+		(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
-		(ParamExtFloat<px4::params::EKF2_MAG_ACCLIM>)
+		(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
-		_param_ekf2_mag_acclim,	///< integer used to specify the type of magnetometer fusion used
+		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>) _param_ekf2_synthetic_mag_z,
-		(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>)
+#endif // CONFIG_EKF2_MAGNETOMETER
-		_param_ekf2_mag_yawlim,	///< yaw rate threshold used by mode select logic (rad/sec)
 
 		(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
 		_param_ekf2_gps_check,	///< bitmask used to control which GPS quality checks are used
@@ -736,11 +743,6 @@ private:
 #endif // CONFIG_EKF2_BARO_COMPENSATION
 
 		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
-		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
-		(ParamExtFloat<px4::params::EKF2_MAG_CHK_STR>) _param_ekf2_mag_chk_str,
-		(ParamExtFloat<px4::params::EKF2_MAG_CHK_INC>) _param_ekf2_mag_chk_inc,
-		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>)
-		_param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone.
 
 		// Used by EKF-GSF experimental yaw estimator
 		(ParamExtFloat<px4::params::EKF2_GSF_TAS>)

src/modules/ekf2/Kconfig

@@ -56,6 +56,13 @@ depends on MODULES_EKF2
 	---help---
 		EKF2 GNSS yaw fusion support.
 
+menuconfig EKF2_MAGNETOMETER
+depends on MODULES_EKF2
+	bool "magnetometer support"
+	default y
+	---help---
+		EKF2 magnetometer support.
+
 menuconfig EKF2_OPTICAL_FLOW
 depends on MODULES_EKF2
         bool "optical flow fusion support"