//============================================================================================= // MahonyAHRS.c //============================================================================================= // // Madgwick's implementation of Mayhony's AHRS algorithm. // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ // // From the x-io website "Open-source resources available on this website are // provided under the GNU General Public Licence unless an alternative licence // is provided in source." // // Date Author Notes // 29/09/2011 SOH Madgwick Initial release // 02/10/2011 SOH Madgwick Optimised for reduced CPU load // // Algorithm paper: // http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934 // //============================================================================================= //------------------------------------------------------------------------------------------- // Header files #include "MahonyAHRS.h" #include //------------------------------------------------------------------------------------------- // Definitions #define sampleFreq 100 #define twoKpDef (10.0f * 0.5f) // 2 * proportional gain #define twoKiDef (2.0f * 0.0f) // 2 * integral gain float twoKi = twoKiDef; // 2 * integral gain (Ki) float twoKp = twoKpDef; // 2 * integral gain (Ki) float invSampleFreq = 1.0f/sampleFreq; float q0=1.0f, q1=0.0f, q2=0.0f, q3=0.0f; // quaternion of sensor frame relative to auxiliary frame float integralFBx=0, integralFBy=0, integralFBz=0; // integral error terms scaled by Ki float roll, pitch, yaw; char anglesComputed; float Mahony_invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i>>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); y = y * (1.5f - (halfx * y * y)); return y; } float invSqrt(float x) { float halfx = 0.5f * x; float y = x; long i = *(long*)&y; i = 0x5f3759df - (i>>1); y = *(float*)&i; y = y * (1.5f - (halfx * y * y)); return y; } #define samplePeriod 0.01f //float twoKp=(5.0f * 0.5f); //float twoKi=(2.0f * 0.5f); //float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki //¹²éîËÄÔªÊý void quatconj(const float *Quat,float *out) { out[0]= Quat[0]; out[1]=-Quat[1]; out[2]=-Quat[2]; out[3]=-Quat[3]; } float quatdianc(const float* Q,const float* P) { float result; result=P[0]*Q[0] + P[1]*Q[1] + P[2]*Q[2] + P[3]*Q[3]; return result; } void quatmultiply(const float * Q,const float * P,float *QP) { QP[0]=P[0]*Q[0] - P[1]*Q[1] - P[2]*Q[2] - P[3]*Q[3]; QP[1]=P[0]*Q[1] + P[1]*Q[0] + P[2]*Q[3] - P[3]*Q[2]; QP[2]=P[0]*Q[2] + P[2]*Q[0] + P[3]*Q[1] - P[1]*Q[3]; QP[3]=P[0]*Q[3] + P[3]*Q[0] + P[1]*Q[2] - P[2]*Q[1]; } void quatinv(float* Q,float *quatinvQ) { float mod; float temp[4]; quatconj(Q,temp); mod=quatdianc(temp,temp); quatinvQ[0]=temp[0]/mod; quatinvQ[1]=temp[1]/mod; quatinvQ[2]=temp[2]/mod; quatinvQ[3]=temp[3]/mod; } void quatrotate(float* sour_pion,float* Q,float *out_poin) { float Quaternion_p[4]; float temp[4]; float temp1[4]; float temp2[4]; Quaternion_p[0]=0; Quaternion_p[1]=sour_pion[0]; Quaternion_p[2]=sour_pion[1]; Quaternion_p[3]=sour_pion[2]; quatmultiply(Q,Quaternion_p,temp); quatinv(Q,temp2); quatmultiply(temp,temp2,temp1); out_poin[0]=temp1[1]; out_poin[1]=temp1[2]; out_poin[2]=temp1[3]; } void accel_BConvertToN(float accel_s[3], float accel_d[3], float q[4]) { float temp[4]; quatconj(q,temp); quatrotate(accel_s,temp,accel_d); // accel_d[2] -= 1; } void updateIMU(float gx, float gy, float gz, float ax, float ay, float az) { 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))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity and vector perpendicular to magnetic flux halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // Error is sum of cross product between estimated and measured direction of gravity halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); halfez = (ax * halfvy - ay * halfvx); // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { integralFBx += twoKi * halfex * samplePeriod; // integral error scaled by Ki integralFBy += twoKi * halfey * samplePeriod; integralFBz += twoKi * halfez * samplePeriod; gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += twoKp * halfex; gy += twoKp * halfey; gz += twoKp * halfez; } // Integrate rate of change of quaternion gx *= (0.5f * samplePeriod); // pre-multiply common factors gy *= (0.5f * samplePeriod); gz *= (0.5f * samplePeriod); 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); // Normalise quaternion recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; } void Mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float recipNorm; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float hx, hy, bx, bz; float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; float halfex, halfey, halfez; float qa, qb, qc; // Convert gyroscope degrees/sec to radians/sec // gx *= 0.0174533f; // gy *= 0.0174533f; // gz *= 0.0174533f; // Compute feedback only if accelerometer measurement valid // (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = Mahony_invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Normalise magnetometer measurement recipNorm = Mahony_invSqrt(mx * mx + my * my + mz * mz); mx *= recipNorm; my *= recipNorm; mz *= recipNorm; // Auxiliary variables to avoid repeated arithmetic q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; // Reference direction of Earth's magnetic field hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); bx = sqrtf(hx * hx + hy * hy); bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); // Estimated direction of gravity and magnetic field halfvx = q1q3 - q0q2; halfvy = q0q1 + q2q3; halfvz = q0q0 - 0.5f + q3q3; halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); // Error is sum of cross product between estimated direction // and measured direction of field vectors halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy); halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz); halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx); // Compute and apply integral feedback if enabled if(twoKi > 0.0f) { // integral error scaled by Ki integralFBx += twoKi * halfex * invSampleFreq; integralFBy += twoKi * halfey * invSampleFreq; integralFBz += twoKi * halfez * invSampleFreq; gx += integralFBx; // apply integral feedback gy += integralFBy; gz += integralFBz; } else { integralFBx = 0.0f; // prevent integral windup integralFBy = 0.0f; integralFBz = 0.0f; } // Apply proportional feedback gx += twoKpDef * halfex; gy += twoKpDef * halfey; gz += twoKpDef * halfez; } // Integrate rate of change of quaternion gx *= (0.5f * invSampleFreq); // pre-multiply common factors gy *= (0.5f * invSampleFreq); gz *= (0.5f * invSampleFreq); 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); // Normalise quaternion recipNorm = Mahony_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= recipNorm; q1 *= recipNorm; q2 *= recipNorm; q3 *= recipNorm; anglesComputed = 0; } float roll, pitch, yaw; char anglesComputed; void Mahony_computeAngles() { roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2); pitch = asinf(-2.0f * (q1*q3 - q0*q2)); yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); anglesComputed = 1; } float getRoll() { if (!anglesComputed) Mahony_computeAngles(); return roll * 57.29578f; } float getPitch() { if (!anglesComputed) Mahony_computeAngles(); return pitch * 57.29578f; } float getYaw() { if (!anglesComputed) Mahony_computeAngles(); return yaw * 57.29578f + 180.0f; } //============================================================================================ // END OF CODE //============================================================================================