5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 15:38:29 -04:00

AP_AHRS: attitudes_consistent: no euler angles

This commit is contained in:
Mark Whitehorn 2019-04-03 09:40:37 -06:00 committed by Randy Mackay
parent b23d1431da
commit dd8bfb0ad4
2 changed files with 5 additions and 2 deletions

View File

@ -1317,7 +1317,7 @@ 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);
get_quat_body_to_ned(primary_quat);
// 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_euler(_dcm_attitude.x, _dcm_attitude.y, _dcm_attitude.z);
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) {

View File

@ -276,8 +276,11 @@ private:
bool _ekf2_started;
bool _ekf3_started;
bool _force_ekf;
// rotation from vehicle body to NED frame
Matrix3f _dcm_matrix;
Vector3f _dcm_attitude;
Vector3f _gyro_drift;
Vector3f _gyro_estimate;
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];