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

AP_AHRS: fix bug in attitudes_consistent()

Browse Source
This commit is contained in:
Mark Whitehorn 2019-03-18 11:55:16 -06:00 committed by Andrew Tridgell
parent 825055d736
commit 61a04bd898
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

@ -309,6 +309,7 @@ void AP_AHRS_NavEKF::update_SITL(void)
const AP_InertialSensor &_ins = AP::ins();
if (active_EKF_type() == EKF_TYPE_SITL) {
fdm.quaternion.rotation_matrix(_dcm_matrix);
_dcm_matrix = _dcm_matrix * get_rotation_vehicle_body_to_autopilot_body();
_dcm_matrix.to_euler(&roll, &pitch, &yaw);
@ -1317,7 +1318,6 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// get primary attitude source's attitude as quaternion
Quaternion primary_quat;
primary_quat.from_euler(roll, pitch, yaw);
// only check yaw if compasses are being used
bool check_yaw = _compass && _compass->use_for_yaw();
@ -1360,7 +1360,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu
// check primary vs dcm
Quaternion dcm_quat;
Vector3f angle_diff;
dcm_quat.from_axis_angle(_dcm_attitude);
dcm_quat.from_euler(_dcm_attitude.x, _dcm_attitude.y, _dcm_attitude.z);
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) {