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: fix attitudes_consistent bug

Browse Source
This commit is contained in:
Mark Whitehorn 2019-03-28 16:03:22 -06:00 committed by Randy Mackay
parent 49554bf8d9
commit b32ccbfb33
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1325,7 +1325,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
for (uint8_t i = 0; i < EKF2.activeCores(); i++) { for (uint8_t i = 0; i < EKF2.activeCores(); i++) {
Quaternion ekf2_quat; Quaternion ekf2_quat;
Vector3f angle_diff; Vector3f angle_diff;
EKF2.getQuaternion(i, ekf2_quat); EKF2.getQuaternionBodyToNED(i, ekf2_quat);
primary_quat.angular_difference(ekf2_quat).to_axis_angle(angle_diff); primary_quat.angular_difference(ekf2_quat).to_axis_angle(angle_diff);
float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)); float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) { if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {
@ -1343,7 +1343,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
for (uint8_t i = 0; i < EKF3.activeCores(); i++) { for (uint8_t i = 0; i < EKF3.activeCores(); i++) {
Quaternion ekf3_quat; Quaternion ekf3_quat;
Vector3f angle_diff; Vector3f angle_diff;
EKF3.getQuaternion(i, ekf3_quat); EKF3.getQuaternionBodyToNED(i, ekf3_quat);
primary_quat.angular_difference(ekf3_quat).to_axis_angle(angle_diff); primary_quat.angular_difference(ekf3_quat).to_axis_angle(angle_diff);
float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y)); float diff = safe_sqrt(sq(angle_diff.x)+sq(angle_diff.y));
if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) { if (diff > ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD) {