Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Quat: test patch for michael

This commit is contained in:
Andrew Tridgell 2012-03-05 18:50:09 +11:00
parent a954f68f27
commit d11aab610d
1 changed files with 20 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_Quaternion/AP_Quaternion.cpp
View File

@ -86,7 +86,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2; SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;
// Normalise the gradient // Normalise the gradient
norm = 1.0/sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); norm = 1.0/safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
if (isinf(norm)) { if (isinf(norm)) {
// we can't do an update - discard this data point and // we can't do an update - discard this data point and
// hope the next one is better // hope the next one is better
@ -111,7 +111,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat; SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
// Normalise quaternion // Normalise quaternion
norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
if (isinf(norm)) { if (isinf(norm)) {
// our quaternion is bad! Reset based on roll/pitch/yaw // our quaternion is bad! Reset based on roll/pitch/yaw
// and hope for the best ... // and hope for the best ...
@ -219,7 +219,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6; SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
// normalise the gradient to estimate direction of the gyroscope error // normalise the gradient to estimate direction of the gyroscope error
norm = 1.0 / sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
if (isinf(norm)) { if (isinf(norm)) {
// discard this data point // discard this data point
renorm_range_count++; renorm_range_count++;
@ -268,7 +268,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat; SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
// normalise quaternion // normalise quaternion
norm = 1.0/sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
if (isinf(norm)) { if (isinf(norm)) {
// our quaternion is bad! Reset based on roll/pitch/yaw // our quaternion is bad! Reset based on roll/pitch/yaw
// and hope for the best ... // and hope for the best ...
@ -307,6 +307,7 @@ void AP_Quaternion::update(void)
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
Quaternion q_saved = q; Quaternion q_saved = q;
Vector3f bias_saved = gyro_bias;
#endif #endif
_imu->update(); _imu->update();
@ -363,6 +364,21 @@ void AP_Quaternion::update(void)
update_MARG(deltat, gyro, accel, mag); update_MARG(deltat, gyro, accel, mag);
} }
#ifdef DESKTOP_BUILD
if (q.is_nan()) {
SITL_debug("QUAT NAN: deltat=%f roll=%f pitch=%f yaw=%f q0=[%f %f %f %f] q=[%f %f %f %f] a=[%f %f %f] g=(%f %f %f)\n",
deltat, roll, pitch, yaw,
q_saved.q1, q_saved.q2, q_saved.q3, q_saved.q4,
q.q1, q.q2, q.q3, q.q4,
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
q = q_saved;
gyro_bias = bias_saved;
update_IMU(deltat, gyro, accel);
}
#endif
// keep the corrected gyro for reporting // keep the corrected gyro for reporting
_gyro_corrected = gyro; _gyro_corrected = gyro;
@ -401,23 +417,6 @@ void AP_Quaternion::update(void)
yaw_sensor += 36000; yaw_sensor += 36000;
} }
#ifdef DESKTOP_BUILD
if (q.is_nan() ||
isnan(roll) ||
isnan(pitch) ||
isnan(yaw) ||
isnan(b_x) ||
isnan(b_z) ||
gyro_bias.is_nan()) {
SITL_debug("QUAT NAN: deltat=%f roll=%f pitch=%f yaw=%f q0=[%f %f %f %f] q=[%f %f %f %f] a=[%f %f %f] g=(%f %f %f)\n",
deltat, roll, pitch, yaw,
q_saved.q1, q_saved.q2, q_saved.q3, q_saved.q4,
q.q1, q.q2, q.q3, q.q4,
accel.x, accel.y, accel.z,
gyro.x, gyro.y, gyro.z);
}
#endif
} }
// average error in roll/pitch since last call // average error in roll/pitch since last call