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:
parent
b23d1431da
commit
dd8bfb0ad4
libraries/AP_AHRS
@ -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) {
|
||||
|
@ -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];
|
||||
|
Loading…
Reference in New Issue
Block a user