AP_AHRS: attitudes_consistent fix for roll-pitch difference calc

previously yaw difference would appear as roll, pitch differrences

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -2026,17 +2026,21 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
     // check primary vs ekf2
     for (uint8_t i = 0; i < EKF2.activeCores(); i++) {
         Quaternion ekf2_quat;
-        Vector3f angle_diff;
         EKF2.getQuaternionBodyToNED(i, ekf2_quat);
-        primary_quat.angular_difference(ekf2_quat).to_axis_angle(angle_diff);
-        float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
-        if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
-            hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
+
+        // check roll and pitch difference
+        const float rp_diff_rad = primary_quat.roll_pitch_difference(ekf2_quat);
+        if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Roll/Pitch inconsistent by %d deg", (int)degrees(rp_diff_rad));
             return false;
         }
-        diff = fabsf(angle_diff.z);
-        if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
-            hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Yaw inconsistent by %d deg", (int)degrees(diff));
+
+        // check yaw difference
+        Vector3f angle_diff;
+        primary_quat.angular_difference(ekf2_quat).to_axis_angle(angle_diff);
+        const float yaw_diff = fabsf(angle_diff.z);
+        if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Yaw inconsistent by %d deg", (int)degrees(yaw_diff));
             return false;
         }
     }
@@ -2047,17 +2051,21 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
     // check primary vs ekf3
     for (uint8_t i = 0; i < EKF3.activeCores(); i++) {
         Quaternion ekf3_quat;
-        Vector3f angle_diff;
         EKF3.getQuaternionBodyToNED(i, ekf3_quat);
-        primary_quat.angular_difference(ekf3_quat).to_axis_angle(angle_diff);
-        float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
-        if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
-            hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
+
+        // check roll and pitch difference
+        const float rp_diff_rad = primary_quat.roll_pitch_difference(ekf3_quat);
+        if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Roll/Pitch inconsistent by %d deg", (int)degrees(rp_diff_rad));
             return false;
         }
-        diff = fabsf(angle_diff.z);
-        if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
-            hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Yaw inconsistent by %d deg", (int)degrees(diff));
+
+        // check yaw difference
+        Vector3f angle_diff;
+        primary_quat.angular_difference(ekf3_quat).to_axis_angle(angle_diff);
+        const float yaw_diff = fabsf(angle_diff.z);
+        if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Yaw inconsistent by %d deg", (int)degrees(yaw_diff));
             return false;
         }
     }
@@ -2067,12 +2075,12 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
     // check primary vs dcm
     if (!always_use_EKF() || (total_ekf_cores == 1)) {
         Quaternion dcm_quat;
-        Vector3f angle_diff;
         dcm_quat.from_rotation_matrix(get_DCM_rotation_body_to_ned());
-        primary_quat.angular_difference(dcm_quat).to_axis_angle(angle_diff);
-        float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
-        if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
-            hal.util->snprintf(failure_msg, failure_msg_len, "DCM Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
+
+        // check roll and pitch difference
+        const float rp_diff_rad = primary_quat.roll_pitch_difference(dcm_quat);
+        if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "DCM Roll/Pitch inconsistent by %d deg", (int)degrees(rp_diff_rad));
             return false;
         }
 
@@ -2083,9 +2091,11 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
         using_external_yaw = ekf_type() == EKFType::THREE && EKF3.using_external_yaw();
 #endif
         if (!always_use_EKF() && !using_external_yaw) {
-            diff = fabsf(angle_diff.z);
-            if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
-                hal.util->snprintf(failure_msg, failure_msg_len, "DCM Yaw inconsistent by %d deg", (int)degrees(diff));
+            Vector3f angle_diff;
+            primary_quat.angular_difference(dcm_quat).to_axis_angle(angle_diff);
+            const float yaw_diff = fabsf(angle_diff.z);
+            if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
+                hal.util->snprintf(failure_msg, failure_msg_len, "DCM Yaw inconsistent by %d deg", (int)degrees(yaw_diff));
                 return false;
             }
         }