From 83ad1c17a8d575b465e21e8a76b3126fb447a369 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 5 Apr 2020 09:54:44 +1000 Subject: [PATCH] AP_AHRS: disable DCM yaw consistency check when using external yaw when EKF3 is using an external (typically GPS) supplied yaw then we don't expect DCM to have the right yaw so should not do the DCM yaw consistency check --- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 0a7c2f7006..f942e2459d 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -1642,10 +1642,17 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu hal.util->snprintf(failure_msg, failure_msg_len, "DCM Roll/Pitch inconsistent by %d deg", (int)degrees(diff)); return false; } - 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)); - return false; + + // we only check yaw against DCM if we are not using external yaw + // for EKF3. DCM can't use external yaw, so we don't expect it's + // yaw to align with EKF3 when EKF3 is using an external yaw + // source + if (ekf_type() != EKFType::THREE || !EKF3.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)); + return false; + } } return true;