Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: attitudes_consistent obeys always_use_EKF

This commit is contained in:
Randy Mackay 2020-05-11 09:33:00 +09:00 committed by Andrew Tridgell
parent d1170db4bf
commit 839e03f3e4
1 changed files with 25 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1640,6 +1640,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
get_quat_body_to_ned(primary_quat);
// only check yaw if compasses are being used
bool check_yaw = _compass && _compass->use_for_yaw();
uint8_t total_ekf_cores = 0;
#if HAL_NAVEKF2_AVAILABLE
// check primary vs ekf2
@ -1659,6 +1660,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
return false;
}
}
total_ekf_cores = EKF2.activeCores();
#endif
#if HAL_NAVEKF3_AVAILABLE
@ -1679,33 +1681,34 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
return false;
}
}
total_ekf_cores += EKF3.activeCores();
#endif
// check primary vs dcm
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));
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
bool using_external_yaw = false;
#if HAL_NAVEKF3_AVAILABLE
using_external_yaw = ekf_type() == EKFType::THREE && EKF3.using_external_yaw();
#endif
if (!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));
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));
return false;
}
// Check vs DCM yaw if this vehicle could use DCM in flight
// and if not using an external yaw source (DCM does not support external yaw sources)
bool using_external_yaw = false;
#if HAL_NAVEKF3_AVAILABLE
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));
return false;
}
}
}
return true;