ekf2: merge mag yaw angle observability into heading consistency

- the additional hyteresis logic for "yaw angle observable" was
   needed when it controlled time dependent mag_3d

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -683,7 +683,6 @@
 *(.text._ZN18MavlinkStreamDebug8get_sizeEv)
 *(.text._ZN12GPSDriverUBX7receiveEj)
 *(.text._ZN13BatteryStatus21parameter_update_pollEb)
-*(.text._ZN3Ekf26checkYawAngleObservabilityEv)
 *(.text._ZN3RTL18updateDatamanCacheEv)
 *(.text.__ieee754_sqrtf)
 *(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv)

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -52,8 +52,6 @@ void Ekf::controlMagFusion()
 		_control_status.flags.mag_aligned_in_flight = false;
 	}
 
-	checkYawAngleObservability();
-
 	if (_params.mag_fusion_type == MagFuseType::NONE) {
 		stopMagFusion();
 		return;
@@ -391,23 +389,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(const magSample &mag_sample)
 {
 	// use mag bias if variance good
@@ -437,7 +418,10 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample)
 	}
 
 	if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) {
-		if (_yaw_angle_observable) {
+		// Check if there has been enough change in horizontal velocity to make yaw observable
+		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
+
+		if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) {
 			// yaw angle must be observable to consider consistency
 			_control_status.flags.mag_heading_consistent = true;
 		}

src/modules/ekf2/EKF/ekf.h

@@ -703,7 +703,6 @@ private:
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// 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};
 	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
 
@@ -1028,7 +1027,6 @@ private:
 	void resetMagStates(const Vector3f &mag, bool reset_heading = true);
 	bool haglYawResetReq();
 
-	void checkYawAngleObservability();
 	void checkMagHeadingConsistency(const magSample &mag_sample);
 
 	bool checkMagField(const Vector3f &mag);