From 32bace0824ca37c424cc98ecca6ced86cfe10149 Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Tue, 21 May 2013 17:52:12 +1000 Subject: I do not know why roll angle is not correct. But system looks okay --- .../attitude_estimator_so3_comp_main.cpp | 72 ++++++++++++---------- 1 file changed, 38 insertions(+), 34 deletions(-) diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 81a5e5b07..a8561a078 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -44,8 +44,8 @@ extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]) static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ -volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame -volatile float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki +static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame +static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki /** * Mainloop of attitude_estimator_so3_comp. @@ -135,7 +135,6 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; - float qa, qb, qc; // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { @@ -181,13 +180,10 @@ void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float gx *= (0.5f * dt); // pre-multiply common factors gy *= (0.5f * dt); gz *= (0.5f * dt); - qa = q0; - qb = q1; - qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz); - q1 += (qa * gx + qc * gz - q3 * gy); - q2 += (qa * gy - qb * gz + q3 * gx); - q3 += (qa * gz + qb * gy - qc * gx); + q0 += (-q1 * gx - q2 * gy - q3 * gz); + q1 += (q0 * gx + q2 * gz - q3 * gy); + q2 += (q0 * gy - q1 * gz + q3 * gx); + q3 += (q0 * gz + q1 * gy - q2 * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); @@ -203,7 +199,6 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az float hx, hy, bx, bz; float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; float halfex, halfey, halfez; - float qa, qb, qc; // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation) if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { @@ -282,13 +277,10 @@ void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az gx *= (0.5f * dt); // pre-multiply common factors gy *= (0.5f * dt); gz *= (0.5f * dt); - qa = q0; - qb = q1; - qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz); - q1 += (qa * gx + qc * gz - q3 * gy); - q2 += (qa * gy - qb * gz + q3 * gx); - q3 += (qa * gz + qb * gy - qc * gx); + q0 += (-q1 * gx - q2 * gy - q3 * gz); + q1 += (q0 * gx + q2 * gz - q3 * gy); + q2 += (q0 * gy - q1 * gz + q3 * gx); + q3 += (q0 * gz + q1 * gy - q2 * gx); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); @@ -532,19 +524,19 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float c = q2; float d = q3; - Rot_matrix[0] = aSq + bSq - cSq - dSq; - Rot_matrix[1] = 2.0 * (b * c - a * d); - Rot_matrix[2] = 2.0 * (a * c + b * d); - Rot_matrix[3] = 2.0 * (b * c + a * d); - Rot_matrix[4] = aSq - bSq + cSq - dSq; - Rot_matrix[5] = 2.0 * (c * d - a * b); - Rot_matrix[6] = 2.0 * (b * d - a * c); - Rot_matrix[7] = 2.0 * (a * b + c * d); - Rot_matrix[8] = aSq - bSq - cSq + dSq; + Rot_matrix[0] = aSq + bSq - cSq - dSq; // 11 + Rot_matrix[1] = 2.0 * (b * c - a * d); // 12 + Rot_matrix[2] = 2.0 * (a * c + b * d); // 13 + Rot_matrix[3] = 2.0 * (b * c + a * d); // 21 + Rot_matrix[4] = aSq - bSq + cSq - dSq; // 22 + Rot_matrix[5] = 2.0 * (c * d - a * b); // 23 + Rot_matrix[6] = 2.0 * (b * d - a * c); // 31 + Rot_matrix[7] = 2.0 * (a * b + c * d); // 32 + Rot_matrix[8] = aSq - bSq - cSq + dSq; // 33 - /* Compute Euler angle */ - float theta = asinf(-Rot_matrix[6]); - euler[1] = theta; + /* FIXME : Work around this later... + float theta = asinf(-Rot_matrix[6]); // -r_{31} + euler[1] = theta; // pitch angle if(fabsf(theta - M_PI_2_F) < 1.0e-3f){ euler[0] = 0.0f; @@ -556,6 +548,16 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds euler[0] = atan2f(Rot_matrix[7], Rot_matrix[8]); euler[2] = atan2f(Rot_matrix[3], Rot_matrix[0]); } + */ + + float q1q1 = q1*q1; + float q2q2 = q2*q2; + float q3q3 = q3*q3; + + euler[0] = atan2f(2*(q0*q1 + q2*q3),1-2*(q1q1+q2q2)); // roll + euler[1] = asinf(2*(q0*q2 - q3*q1)); // pitch + euler[2] = atan2f(2*(q0*q3 + q1*q2),1-2*(q2q2 + q3q3)); // yaw + /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { @@ -577,10 +579,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitch = euler[1] - so3_comp_params.pitch_off; att.yaw = euler[2] - so3_comp_params.yaw_off; - /* FIXME : This can be a problem for rate controller. Rate in body or inertial? - att.rollspeed = x_aposteriori[0]; - att.pitchspeed = x_aposteriori[1]; - att.yawspeed = x_aposteriori[2]; + /* FIXME : This can be a problem for rate controller. Rate in body or inertial? */ + att.rollspeed = q1; + att.pitchspeed = q2; + att.yawspeed = q3; + + /* att.rollacc = x_aposteriori[3]; att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; -- cgit v1.2.3