Browse Source

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
zr-v5.1
Andrew Tridgell 5 years ago
parent
commit
83ad1c17a8
  1. 15
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

15
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -1642,10 +1642,17 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu @@ -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;

Loading…
Cancel
Save