ekf2: integrate mag heading into mag 3D

msg/EstimatorAidSource1d.msg

@@ -20,5 +20,5 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
-# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
+# TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
 # TOPICS estimator_aid_src_terrain_range_finder

src/modules/ekf2/CMakeLists.txt

@@ -189,7 +189,6 @@ if(CONFIG_EKF2_MAGNETOMETER)
 		EKF/mag_3d_control.cpp
 		EKF/mag_control.cpp
 		EKF/mag_fusion.cpp
-		EKF/mag_heading_control.cpp
 	)
 endif()
 

src/modules/ekf2/EKF/CMakeLists.txt

@@ -107,7 +107,6 @@ if(CONFIG_EKF2_MAGNETOMETER)
 		mag_3d_control.cpp
 		mag_control.cpp
 		mag_fusion.cpp
-		mag_heading_control.cpp
 	)
 endif()
 

src/modules/ekf2/EKF/ekf.cpp

@@ -155,7 +155,6 @@ void Ekf::reset()
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	resetEstimatorAidStatus(_aid_src_mag_heading);
 	resetEstimatorAidStatus(_aid_src_mag);
 #endif // CONFIG_EKF2_MAGNETOMETER
 

src/modules/ekf2/EKF/ekf.h

@@ -149,11 +149,7 @@ public:
 	float getHeadingInnov() const
 	{
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-		if (_control_status.flags.mag_hdg) {
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-			return _aid_src_mag_heading.innovation;
-		}
-
-		if (_control_status.flags.mag_3D) {
 			return Vector3f(_aid_src_mag.innovation).max();
 		}
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -176,11 +172,7 @@ public:
 	float getHeadingInnovVar() const
 	{
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-		if (_control_status.flags.mag_hdg) {
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-			return _aid_src_mag_heading.innovation_variance;
-		}
-
-		if (_control_status.flags.mag_3D) {
 			return Vector3f(_aid_src_mag.innovation_variance).max();
 		}
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -203,11 +195,7 @@ public:
 	float getHeadingInnovRatio() const
 	{
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-		if (_control_status.flags.mag_hdg) {
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-			return _aid_src_mag_heading.test_ratio;
-		}
-
-		if (_control_status.flags.mag_3D) {
 			return Vector3f(_aid_src_mag.test_ratio).max();
 		}
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -460,7 +448,6 @@ public:
 #endif // CONFIG_EKF2_GNSS
 
 #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
 
@@ -715,19 +702,13 @@ private:
 #endif // CONFIG_EKF2_BAROMETER
 
 #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 _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
 	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)
@@ -777,7 +758,7 @@ private:
 
 #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);
+	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true, bool update_tilt = true);
 
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
@@ -1048,7 +1029,6 @@ private:
 #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);
 	void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src);
 
 	bool checkHaglYawResetReq() const;
@@ -1058,12 +1038,11 @@ private:
 	bool haglYawResetReq();
 
 	void checkYawAngleObservability();
-	void checkMagHeadingConsistency();
+	void checkMagHeadingConsistency(const magSample &mag_sample);
 
 	bool checkMagField(const Vector3f &mag);
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
 
-	void stopMagHdgFusion();
 	void stopMagFusion();
 
 	// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -381,11 +381,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	mag = 0.f;
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	if (_control_status.flags.mag_hdg) {
+	if (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
-		mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
-	}
-
-	if (_control_status.flags.mag_3D) {
 		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -523,12 +519,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 (_control_status.flags.mag_hdg ||_control_status.flags.mag_3D) {
-		if (_aid_src_mag_heading.test_ratio < 1.f) {
-			mag_innov_good = false;
-		}
-
-	} else if (_control_status.flags.mag_3D) {
 		if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) {
 			mag_innov_good = false;
 		}

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -67,6 +67,18 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 				       && !_control_status.flags.ev_yaw
 				       && !_control_status.flags.gps_yaw;
 
+	const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
+
+	_control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING)
+					      || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
+					     && _control_status.flags.tilt_align
+					     && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
+					         || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && !_control_status.flags.mag_fault
+					     && !_control_status.flags.mag_field_disturbed
+					     && !_control_status.flags.ev_yaw
+					     && !_control_status.flags.gps_yaw;
+
 	// TODO: allow clearing mag_fault if mag_3d is good?
 
 	if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
@@ -108,8 +120,9 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 					// The normal sequence is to fuse the magnetometer data first before fusing
 					// declination angle at a higher uncertainty to allow some learning of
 					// declination angle over time.
-					const bool update_all_states = _control_status.flags.mag_3D;
+					const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
-					fuseMag(mag_sample.mag, aid_src, update_all_states);
+					const bool update_tilt = _control_status.flags.mag_3D;
+					fuseMag(mag_sample.mag, aid_src, update_all_states, update_tilt);
 
 					if (_control_status.flags.mag_dec) {
 						fuseDeclination(0.5f);
@@ -199,6 +212,12 @@ void Ekf::stopMagFusion()
 			_control_status.flags.mag_3D = false;
 		}
 
+		if (_control_status.flags.mag_hdg) {
+			ECL_INFO("stopping mag heading fusion");
+			_control_status.flags.mag_hdg = false;
+			_fault_status.flags.bad_hdg = false;
+		}
+
 		_fault_status.flags.bad_mag_x = false;
 		_fault_status.flags.bad_mag_y = false;
 		_fault_status.flags.bad_mag_z = false;

src/modules/ekf2/EKF/mag_control.cpp

@@ -48,11 +48,9 @@ void Ekf::controlMagFusion()
 	}
 
 	checkYawAngleObservability();
-	checkMagHeadingConsistency();
 
 	if (_params.mag_fusion_type == MagFuseType::NONE) {
 		stopMagFusion();
-		stopMagHdgFusion();
 		return;
 	}
 
@@ -105,12 +103,11 @@ void Ekf::controlMagFusion()
 			_control_status.flags.synthetic_mag_z = false;
 		}
 
+		checkMagHeadingConsistency(mag_sample);
 		controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
-		controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading);
 
 	} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
-		stopMagHdgFusion();
 		stopMagFusion();
 	}
 }
@@ -239,8 +236,34 @@ void Ekf::checkYawAngleObservability()
 	}
 }
 
-void Ekf::checkMagHeadingConsistency()
+void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 {
+	// use mag bias if variance good
+	Vector3f mag_bias{0.f, 0.f, 0.f};
+	const Vector3f mag_bias_var = getMagBiasVariance();
+
+	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
+		mag_bias = _state.mag_B;
+	}
+
+	// calculate mag heading
+	// Rotate the measurements into earth frame using the zero yaw angle
+	const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
+
+	// the angle of the projection onto the horizontal gives the yaw angle
+	// calculate the yaw innovation and wrap to the interval between +-pi
+	const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias);
+	const float declination = getMagDeclination();
+	const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
+
+	if (_control_status.flags.yaw_align) {
+		const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg);
+		_mag_heading_innov_lpf.update(innovation);
+
+	} else {
+		_mag_heading_innov_lpf.reset(0.f);
+	}
+
 	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
 		if (_yaw_angle_observable) {
 			// yaw angle must be observable to consider consistency
@@ -349,7 +372,6 @@ void Ekf::resetMagHeading(const Vector3f &mag)
 
 	// update quaternion states and corresponding covarainces
 	resetQuatStateYaw(yaw_new, yaw_new_variance);
-	_mag_heading_last_declination = declination;
 
 	_time_last_heading_fuse = _time_delayed_us;
 

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -51,7 +51,7 @@
 
 #include <mathlib/mathlib.h>
 
-bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states)
+bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool update_tilt)
 {
 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
 	const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
@@ -191,6 +191,11 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 			Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 		}
 
+		if (!update_tilt) {
+			Kfusion(State::quat_nominal.idx) = 0.f;
+			Kfusion(State::quat_nominal.idx + 1) = 0.f;
+		}
+
 		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
 			fused[index] = true;
 			limitDeclination();

src/modules/ekf2/EKF/mag_heading_control.cpp

@@ -1,204 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-/**
- * @file mag_heading_control.cpp
- * Control functions for ekf mag heading fusion
- */
-
-#include "ekf.h"
-
-void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
-				  estimator_aid_source1d_s &aid_src)
-{
-	static constexpr const char *AID_SRC_NAME = "mag heading";
-
-	// use mag bias if variance good
-	Vector3f mag_bias{0.f, 0.f, 0.f};
-	const Vector3f mag_bias_var = getMagBiasVariance();
-
-	if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) {
-		mag_bias = _state.mag_B;
-	}
-
-	// calculate mag heading
-	// Rotate the measurements into earth frame using the zero yaw angle
-	const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
-
-	// the angle of the projection onto the horizontal gives the yaw angle
-	// calculate the yaw innovation and wrap to the interval between +-pi
-	const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias);
-	const float declination = getMagDeclination();
-	const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination;
-
-	resetEstimatorAidStatus(aid_src);
-	aid_src.observation = measured_hdg;
-	aid_src.observation_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f));
-
-	if (_control_status.flags.yaw_align) {
-		// mag heading
-		aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
-		_mag_heading_innov_lpf.update(aid_src.innovation);
-
-	} else {
-		// mag heading delta (logging only)
-		aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev)
-					     - wrap_pi(measured_hdg - _mag_heading_prev));
-		_mag_heading_innov_lpf.reset(0.f);
-	}
-
-	// determine if we should use mag heading aiding
-	const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
-
-	bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::HEADING)
-					      || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
-					     && _control_status.flags.tilt_align
-					     && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
-					         || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-					     && !_control_status.flags.mag_fault
-					     && !_control_status.flags.mag_field_disturbed
-					     && !_control_status.flags.ev_yaw
-					     && !_control_status.flags.gps_yaw
-					     && PX4_ISFINITE(aid_src.observation)
-					     && PX4_ISFINITE(aid_src.observation_variance);
-
-	const bool starting_conditions_passing = common_starting_conditions_passing
-			&& continuing_conditions_passing
-			&& isTimedOut(aid_src.time_last_fuse, 3e6);
-
-	if (_control_status.flags.mag_hdg) {
-		aid_src.timestamp_sample = mag_sample.time_us;
-
-		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
-
-			const bool declination_changed = _control_status_prev.flags.mag_hdg
-							 && (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f));
-			bool skip_timeout_check = false;
-
-			if (mag_sample.reset || declination_changed) {
-				if (declination_changed) {
-					ECL_INFO("reset to %s, declination changed %.5f->%.5f",
-						 AID_SRC_NAME, (double)_mag_heading_last_declination, (double)declination);
-
-				} else {
-					ECL_INFO("reset to %s", AID_SRC_NAME);
-				}
-
-				resetMagHeading(_mag_lpf.getState());
-				aid_src.time_last_fuse = _time_delayed_us;
-
-			} else {
-				VectorState H_YAW;
-				computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW);
-
-				if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) {
-					// Only fuse mag to constrain the yaw variance to a safe value
-					fuseYaw(aid_src, H_YAW);
-
-				} else {
-					// Yaw variance is low enough, stay in mag heading mode but don't fuse the data and skip the fusion timeout check
-					skip_timeout_check = true;
-					aid_src.test_ratio = 0.f; // required for preflight checks to pass
-				}
-			}
-
-			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max) && !skip_timeout_check;
-
-			if (is_fusion_failing) {
-				if (_nb_mag_heading_reset_available > 0) {
-					// Data seems good, attempt a reset
-					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-					resetMagHeading(_mag_lpf.getState());
-					aid_src.time_last_fuse = _time_delayed_us;
-
-					if (_control_status.flags.in_air) {
-						_nb_mag_heading_reset_available--;
-					}
-
-				} else if (starting_conditions_passing) {
-					// Data seems good, but previous reset did not fix the issue
-					// something else must be wrong, declare the sensor faulty and stop the fusion
-					_control_status.flags.mag_fault = true;
-					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
-					stopMagHdgFusion();
-
-				} else {
-					// A reset did not fix the issue but all the starting checks are not passing
-					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
-					stopMagHdgFusion();
-				}
-			}
-
-		} else {
-			// Stop fusion but do not declare it faulty
-			ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME);
-			stopMagHdgFusion();
-		}
-
-	} else {
-		if (starting_conditions_passing) {
-			// activate fusion
-			ECL_INFO("starting %s fusion", AID_SRC_NAME);
-
-			if (!_control_status.flags.yaw_align) {
-				// reset heading
-				resetMagHeading(_mag_lpf.getState());
-				_control_status.flags.yaw_align = true;
-			}
-
-			_control_status.flags.mag_hdg = true;
-			aid_src.time_last_fuse = _time_delayed_us;
-
-			_nb_mag_heading_reset_available = 1;
-		}
-	}
-
-	// record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only)
-	_mag_heading_prev = measured_hdg;
-	_mag_heading_pred_prev = getEulerYaw(_state.quat_nominal);
-
-	_mag_heading_last_declination = getMagDeclination();
-}
-
-void Ekf::stopMagHdgFusion()
-{
-	if (_control_status.flags.mag_hdg) {
-		ECL_INFO("stopping mag heading fusion");
-		resetEstimatorAidStatus(_aid_src_mag_heading);
-
-		_control_status.flags.mag_hdg = false;
-
-		_fault_status.flags.bad_hdg = false;
-	}
-}

src/modules/ekf2/EKF2.cpp

@@ -335,7 +335,6 @@ bool EKF2::multi_init(int imu, int mag)
 
 	// mag advertise
 	if (_param_ekf2_mag_type.get() != MagFuseType::NONE) {
-		_estimator_aid_src_mag_heading_pub.advertise();
 		_estimator_aid_src_mag_pub.advertise();
 	}
 
@@ -874,9 +873,6 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)
 #endif // CONFIG_EKF2_GNSS
 
 #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

src/modules/ekf2/EKF2.hpp

@@ -307,11 +307,9 @@ private:
 	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
 

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -259,7 +259,6 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData)
 	// THEN: the flow due to the yaw rotation and the offsets is canceled
 	// and the velocity estimate stays 0
 	// FIXME: the estimate isn't perfect 0 mainly because the mag simulated measurement isn't rotating
-	// and this creates an incorrect Z gyro bias which is used to compensate for the apparent flow
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
 	EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f)
 			<< "estimated vel = " << estimated_horz_velocity(0);
@@ -298,10 +297,11 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData)
 
 	// THEN: the flow due to the yaw rotation and the offsets is canceled
 	// and the velocity estimate stays 0
+	// FIXME: the estimate isn't perfect 0 mainly because the mag simulated measurement isn't rotating
 	const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity());
-	EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f)
+	EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f)
 			<< "estimated vel = " << estimated_horz_velocity(0);
-	EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f)
+	EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.02f)
 			<< "estimated vel = " << estimated_horz_velocity(1);
 	_ekf->state().vector().print();
 	_ekf->covariances().print();

src/modules/logger/logged_topics.cpp

@@ -204,7 +204,6 @@ void LoggedTopics::add_default_topics()
 	// add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES);
-	// add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
 	// add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES);
@@ -297,7 +296,6 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES);
-	add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);
 	add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES);