ekf2: replace yaw failure detection criteria

The existing logic using the angle between velocity vectors failed to
determine a yaw failure in practice because the state velocity is often
too small compared to its uncertainty to be used. In all the failures
reported, the yaw emergency estimator converged properly and yaw reset
would have fixed the issue.
A much simpler check using the yaw difference between the main EKF
and the emergency estimator is now used to tell if the vel/pos update
failure is most likely caused by a wrong heading.

src/modules/ekf2/EKF/gps_control.cpp

@@ -71,14 +71,13 @@ void Ekf::controlGpsFusion()
 					fuseGpsVelPos();
 
 					if (shouldResetGpsFusion()){
-						const bool is_yaw_failure = !isVelStateAlignedWithObs();
 						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
 
 						/* A reset is not performed when getting GPS back after a significant period of no data
 						 * because the timeout could have been caused by bad GPS.
 						 * The total number of resets allowed per boot cycle is limited.
 						 */
-						if (is_yaw_failure
+						if (isYawFailure()
 						    && _control_status.flags.in_air
 						    && !was_gps_signal_lost
 						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
@@ -188,6 +187,20 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
 }
 
+bool Ekf::isYawFailure() const
+{
+	if (!isYawEmergencyEstimateAvailable()) {
+		return false;
+	}
+
+	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
+				? getEuler321Yaw(_R_to_earth)
+				: getEuler312Yaw(_R_to_earth);
+	const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
+
+	return fabsf(yaw_error) > math::radians(25.f);
+}
+
 void Ekf::processYawEstimatorResetRequest()
 {
 	/* The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.