Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: attitudes_consistent obeys always_use_EKF

Browse Source
This commit is contained in:
Randy Mackay 2020-05-11 09:33:00 +09:00
parent 685e412d14
commit 099067dbc9
1 changed files with 22 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1354,6 +1354,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
get_quat_body_to_ned(primary_quat); get_quat_body_to_ned(primary_quat);
// only check yaw if compasses are being used // only check yaw if compasses are being used
bool check_yaw = _compass && _compass->use_for_yaw(); bool check_yaw = _compass && _compass->use_for_yaw();
uint8_t total_ekf_cores = 0;
// check primary vs ekf2 // check primary vs ekf2
for (uint8_t i = 0; i < EKF2.activeCores(); i++) { for (uint8_t i = 0; i < EKF2.activeCores(); i++) {
@ -1372,6 +1373,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
return false; return false;
} }
} }
total_ekf_cores = EKF2.activeCores();
// check primary vs ekf3 // check primary vs ekf3
for (uint8_t i = 0; i < EKF3.activeCores(); i++) { for (uint8_t i = 0; i < EKF3.activeCores(); i++) {
@ -1390,21 +1392,27 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
return false; return false;
} }
} }
total_ekf_cores += EKF3.activeCores();
// check primary vs dcm // check primary vs dcm if this vehicle could use DCM in flight or only one core is enabled
Quaternion dcm_quat; if (!always_use_EKF() || (total_ekf_cores == 1)) {
Vector3f angle_diff; Quaternion dcm_quat;
dcm_quat.from_rotation_matrix(get_DCM_rotation_body_to_ned()); Vector3f angle_diff;
primary_quat.angular_difference(dcm_quat).to_axis_angle(angle_diff); dcm_quat.from_rotation_matrix(get_DCM_rotation_body_to_ned());
float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)); primary_quat.angular_difference(dcm_quat).to_axis_angle(angle_diff);
if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) { float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
hal.util->snprintf(failure_msg, failure_msg_len, "DCM Roll/Pitch inconsistent by %d deg", (int)degrees(diff)); if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
return false; 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)); if (!always_use_EKF()) {
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;
}
}
} }
return true; return true;