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 fix for roll-pitch difference calc

Browse Source 
previously yaw difference would appear as roll, pitch differrences
This commit is contained in:
Randy Mackay 2021-06-02 14:39:56 +09:00
parent b263a1d751
commit ab5d4da776
1 changed files with 34 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -2026,17 +2026,21 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// check primary vs ekf2
for (uint8_t i = 0; i < EKF2.activeCores(); i++) {
Quaternion ekf2_quat;
Vector3f angle_diff;
EKF2.getQuaternionBodyToNED(i, ekf2_quat);
primary_quat.angular_difference(ekf2_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, "EKF2 Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
// check roll and pitch difference
const float rp_diff_rad = primary_quat.roll_pitch_difference(ekf2_quat);
if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Roll/Pitch inconsistent by %d deg", (int)degrees(rp_diff_rad));
return false;
}
diff = fabsf(angle_diff.z);
if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Yaw inconsistent by %d deg", (int)degrees(diff));
// check yaw difference
Vector3f angle_diff;
primary_quat.angular_difference(ekf2_quat).to_axis_angle(angle_diff);
const float yaw_diff = fabsf(angle_diff.z);
if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 Yaw inconsistent by %d deg", (int)degrees(yaw_diff));
return false;
}
}
@ -2047,17 +2051,21 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// check primary vs ekf3
for (uint8_t i = 0; i < EKF3.activeCores(); i++) {
Quaternion ekf3_quat;
Vector3f angle_diff;
EKF3.getQuaternionBodyToNED(i, ekf3_quat);
primary_quat.angular_difference(ekf3_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, "EKF3 Roll/Pitch inconsistent by %d deg", (int)degrees(diff));
// check roll and pitch difference
const float rp_diff_rad = primary_quat.roll_pitch_difference(ekf3_quat);
if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Roll/Pitch inconsistent by %d deg", (int)degrees(rp_diff_rad));
return false;
}
diff = fabsf(angle_diff.z);
if (check_yaw && (diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Yaw inconsistent by %d deg", (int)degrees(diff));
// check yaw difference
Vector3f angle_diff;
primary_quat.angular_difference(ekf3_quat).to_axis_angle(angle_diff);
const float yaw_diff = fabsf(angle_diff.z);
if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 Yaw inconsistent by %d deg", (int)degrees(yaw_diff));
return false;
}
}
@ -2067,12 +2075,12 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// check primary vs dcm
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));
// check roll and pitch difference
const float rp_diff_rad = primary_quat.roll_pitch_difference(dcm_quat);
if (rp_diff_rad > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
hal.util->snprintf(failure_msg, failure_msg_len, "DCM Roll/Pitch inconsistent by %d deg", (int)degrees(rp_diff_rad));
return false;
}
@ -2083,9 +2091,11 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
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));
Vector3f angle_diff;
primary_quat.angular_difference(dcm_quat).to_axis_angle(angle_diff);
const float yaw_diff = fabsf(angle_diff.z);
if (check_yaw && (yaw_diff > ATTITUDE_CHECK_THRESH_YAW_RAD)) {
hal.util->snprintf(failure_msg, failure_msg_len, "DCM Yaw inconsistent by %d deg", (int)degrees(yaw_diff));
return false;
}
}