aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: disable DCM yaw consistency check when using external yaw

Browse Source 
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
This commit is contained in:
Andrew Tridgell 2020-04-05 09:54:44 +10:00
parent 3ae0b0d446
commit 83ad1c17a8
1 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -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;