HACK: create mag hdg fusion by modifying 3D fusion

src/modules/ekf2/EKF/ekf.h

@@ -762,7 +762,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 mag_heading = false);
 
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians

src/modules/ekf2/EKF/mag_3d_control.cpp

@@ -62,7 +62,11 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 	_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
 				       && _control_status.flags.mag
 				       && _control_status.flags.mag_aligned_in_flight
-				       && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
+				       && !_control_status.flags.mag_fault
+				       && !_control_status.flags.ev_yaw
+				       && !_control_status.flags.gps_yaw;
+	_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.mag
 				       && !_control_status.flags.mag_fault
 				       && !_control_status.flags.ev_yaw
 				       && !_control_status.flags.gps_yaw;
@@ -102,14 +106,14 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 					// states for the first few observations.
 					fuseDeclination(0.02f);
 					_mag_decl_cov_reset = true;
-					fuseMag(mag_sample.mag, aid_src, false);
+					fuseMag(mag_sample.mag, aid_src, false, false);
 
 				} else {
 					// 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;
-					fuseMag(mag_sample.mag, aid_src, update_all_states);
+					const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
+					fuseMag(mag_sample.mag, aid_src, update_all_states, _control_status.flags.mag_hdg);
 
 					if (_control_status.flags.mag_dec) {
 						fuseDeclination(0.5f);
@@ -176,7 +180,7 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
 
 			} else {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
-				fuseMag(mag_sample.mag, aid_src, false);
+				fuseMag(mag_sample.mag, aid_src, false, false);
 			}
 
 			aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/mag_control.cpp

@@ -47,9 +47,6 @@ void Ekf::controlMagFusion()
 		_control_status.flags.mag_aligned_in_flight = false;
 	}
 
-	checkYawAngleObservability();
-	checkMagHeadingConsistency();
-
 	if (_params.mag_fusion_type == MagFuseType::NONE) {
 		stopMagFusion();
 		stopMagHdgFusion();
@@ -106,7 +103,7 @@ void Ekf::controlMagFusion()
 		}
 
 		controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);
-		controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading);
+		// 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.
@@ -222,36 +219,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	}
 }
 
-void Ekf::checkYawAngleObservability()
-{
-	if (_control_status.flags.gps) {
-		// Check if there has been enough change in horizontal velocity to make yaw observable
-		// Apply hysteresis to check to avoid rapid toggling
-		if (_yaw_angle_observable) {
-			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
-
-		} else {
-			_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
-		}
-
-	} else {
-		_yaw_angle_observable = false;
-	}
-}
-
-void Ekf::checkMagHeadingConsistency()
-{
-	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
-		if (_yaw_angle_observable) {
-			// yaw angle must be observable to consider consistency
-			_control_status.flags.mag_heading_consistent = true;
-		}
-
-	} else {
-		_control_status.flags.mag_heading_consistent = false;
-	}
-}
-
 bool Ekf::checkMagField(const Vector3f &mag_sample)
 {
 	_control_status.flags.mag_field_disturbed = false;

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 mag_heading)
 {
 	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
 	const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
@@ -178,6 +178,17 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo
 
 		VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];
 
+		if (mag_heading) {
+			// Modify the Kalman gain to zero the delta quaternion changes perpendicular to the down NED axis
+			// To do this, transform K from body to NED, zero the xy components and transform back to body frame.
+			// Transformation from body to NED of a delta quaternion in the tangent space is done using the adjoint
+			// at the current attitude (i.e.: the current rotation matrix)
+			Vector3f K_rot = Kfusion.slice<3, 1>(State::quat_nominal.idx, State::quat_nominal.idx);
+			Vector3f row = _R_to_earth.row(2);
+			K_rot = row * row.dot(K_rot);
+			Kfusion.slice<3, 1>(State::quat_nominal.idx, State::quat_nominal.idx) = K_rot;
+		}
+
 		if (!update_all_states) {
 			// zero non-mag Kalman gains if not updating all states