MahonyAHRS.cpp

00001 // Header files
00002 
00003 #include "mbed.h"
00004 #include "MahonyAHRS.h"
00005 #include <math.h>
00006 
00007 //---------------------------------------------------------------------------------------------------
00008 // Definitions
00009 
00010 //#define sampleFreq  512.0f      // sample frequency in Hz
00011 #define twoKpDef    (2.0f * 0.5f)   // 2 * proportional gain
00012 #define twoKiDef    (2.0f * 0.0f)   // 2 * integral gain
00013 #define PI     3.14159265359f
00014 
00015 //---------------------------------------------------------------------------------------------------
00016 
00017 MahonyAHRS::MahonyAHRS(float Freq){
00018 
00019 sampleFreq = Freq;
00020 
00021 }
00022 
00023 float twoKp = twoKpDef;                                            // 2 * proportional gain (Kp)
00024 float twoKi = twoKiDef;                                            // 2 * integral gain (Ki)
00025 float q4 = 1.0f, q5 = 0.0f, q6 = 0.0f, q7 = 0.0f;                  // quaternion of sensor frame relative to auxiliary frame
00026 float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
00027 
00028 
00029 float inv_Sqrt(float x);
00030 
00031 //====================================================================================================
00032 // Functions
00033 
00034 //---------------------------------------------------------------------------------------------------
00035 // AHRS algorithm update
00036 
00037 void MahonyAHRS::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
00038         float recipNorm;
00039     float q4q4, q4q5, q4q6, q4q7, q5q5, q5q6, q5q7, q6q6, q6q7, q7q7;  
00040     float hx, hy, bx, bz;
00041     float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
00042     float halfex, halfey, halfez;
00043     float qa, qb, qc;
00044 
00045     // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
00046     if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
00047         MahonyAHRS::updateIMU(gx, gy, gz, ax, ay, az);
00048         return;
00049     }
00050 
00051     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00052     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00053 
00054         // Normalise accelerometer measurement
00055         recipNorm = inv_Sqrt(ax * ax + ay * ay + az * az);
00056         ax *= recipNorm;
00057         ay *= recipNorm;
00058         az *= recipNorm;     
00059 
00060         // Normalise magnetometer measurement
00061         recipNorm = inv_Sqrt(mx * mx + my * my + mz * mz);
00062         mx *= recipNorm;
00063         my *= recipNorm;
00064         mz *= recipNorm;   
00065 
00066         // Auxiliary variables to avoid repeated arithmetic
00067         q4q4 = q4 * q4;
00068         q4q5 = q4 * q5;
00069         q4q6 = q4 * q6;
00070         q4q7 = q4 * q7;
00071         q5q5 = q5 * q5;
00072         q5q6 = q5 * q6;
00073         q5q7 = q5 * q7;
00074         q6q6 = q6 * q6;
00075         q6q7 = q6 * q7;
00076         q7q7 = q7 * q7;   
00077 
00078         // Reference direction of Earth's magnetic field
00079         hx = 2.0f * (mx * (0.5f - q6q6 - q7q7) + my * (q5q6 - q4q7) + mz * (q5q7 + q4q6));
00080         hy = 2.0f * (mx * (q5q6 + q4q7) + my * (0.5f - q5q5 - q7q7) + mz * (q6q7 - q4q5));
00081         bx = sqrt(hx * hx + hy * hy);
00082         bz = 2.0f * (mx * (q5q7 - q4q6) + my * (q6q7 + q4q5) + mz * (0.5f - q5q5 - q6q6));
00083 
00084         // Estimated direction of gravity and magnetic field
00085         halfvx = q5q7 - q4q6;
00086         halfvy = q4q5 + q6q7;
00087         halfvz = q4q4 - 0.5f + q7q7;
00088         halfwx = bx * (0.5f - q6q6 - q7q7) + bz * (q5q7 - q4q6);
00089         halfwy = bx * (q5q6 - q4q7) + bz * (q4q5 + q6q7);
00090         halfwz = bx * (q4q6 + q5q7) + bz * (0.5f - q5q5 - q6q6);  
00091     
00092         // Error is sum of cross product between estimated direction and measured direction of field vectors
00093         halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
00094         halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
00095         halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
00096 
00097         // Compute and apply integral feedback if enabled
00098         if(twoKi > 0.0f) {
00099             integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki
00100             integralFBy += twoKi * halfey * (1.0f / sampleFreq);
00101             integralFBz += twoKi * halfez * (1.0f / sampleFreq);
00102             gx += integralFBx;  // apply integral feedback
00103             gy += integralFBy;
00104             gz += integralFBz;
00105         }
00106         else {
00107             integralFBx = 0.0f; // prevent integral windup
00108             integralFBy = 0.0f;
00109             integralFBz = 0.0f;
00110         }
00111 
00112         // Apply proportional feedback
00113         gx += twoKp * halfex;
00114         gy += twoKp * halfey;
00115         gz += twoKp * halfez;
00116     }
00117     
00118     // Integrate rate of change of quaternion
00119     gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors
00120     gy *= (0.5f * (1.0f / sampleFreq));
00121     gz *= (0.5f * (1.0f / sampleFreq));
00122     qa = q4;
00123     qb = q5;
00124     qc = q6;
00125     q4 += (-qb * gx - qc * gy - q7 * gz);
00126     q5 += (qa * gx + qc * gz - q7 * gy);
00127     q6 += (qa * gy - qb * gz + q7 * gx);
00128     q7 += (qa * gz + qb * gy - qc * gx); 
00129     
00130     // Normalise quaternion
00131     recipNorm = inv_Sqrt(q4 * q4 + q5 * q5 + q6 * q6 + q7 * q7);
00132     q4 *= recipNorm;
00133     q5 *= recipNorm;
00134     q6 *= recipNorm;
00135     q7 *= recipNorm;
00136 }
00137 
00138 //---------------------------------------------------------------------------------------------------
00139 // IMU algorithm update
00140 
00141 void MahonyAHRS::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
00142     float recipNorm;
00143     float halfvx, halfvy, halfvz;
00144     float halfex, halfey, halfez;
00145     float qa, qb, qc;
00146 
00147     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00148     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00149 
00150         // Normalise accelerometer measurement
00151         recipNorm = inv_Sqrt(ax * ax + ay * ay + az * az);
00152         ax *= recipNorm;
00153         ay *= recipNorm;
00154         az *= recipNorm;        
00155 
00156         // Estimated direction of gravity and vector perpendicular to magnetic flux
00157         halfvx = q5 * q7 - q4 * q6;
00158         halfvy = q4 * q5 + q6 * q7;
00159         halfvz = q4 * q4 - 0.5f + q7 * q7;
00160     
00161         // Error is sum of cross product between estimated and measured direction of gravity
00162         halfex = (ay * halfvz - az * halfvy);
00163         halfey = (az * halfvx - ax * halfvz);
00164         halfez = (ax * halfvy - ay * halfvx);
00165 
00166         // Compute and apply integral feedback if enabled
00167         if(twoKi > 0.0f) {
00168             integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki
00169             integralFBy += twoKi * halfey * (1.0f / sampleFreq);
00170             integralFBz += twoKi * halfez * (1.0f / sampleFreq);
00171             gx += integralFBx;  // apply integral feedback
00172             gy += integralFBy;
00173             gz += integralFBz;
00174         }
00175         else {
00176             integralFBx = 0.0f; // prevent integral windup
00177             integralFBy = 0.0f;
00178             integralFBz = 0.0f;
00179         }
00180 
00181         // Apply proportional feedback
00182         gx += twoKp * halfex;
00183         gy += twoKp * halfey;
00184         gz += twoKp * halfez;
00185     }
00186     
00187     // Integrate rate of change of quaternion
00188     gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors
00189     gy *= (0.5f * (1.0f / sampleFreq));
00190     gz *= (0.5f * (1.0f / sampleFreq));
00191     qa = q4;
00192     qb = q5;
00193     qc = q6;
00194     q4 += (-qb * gx - qc * gy - q7 * gz);
00195     q5 += (qa * gx + qc * gz - q7 * gy);
00196     q6 += (qa * gy - qb * gz + q7 * gx);
00197     q7 += (qa * gz + qb * gy - qc * gx); 
00198     
00199     // Normalise quaternion
00200     recipNorm = inv_Sqrt(q4 * q4 + q5 * q5 + q6 * q6 + q7 * q7);
00201     q4 *= recipNorm;
00202     q5 *= recipNorm;
00203     q6 *= recipNorm;
00204     q7 *= recipNorm;
00205 }
00206 
00207 //---------------------------------------------------------------------------------------------------
00208 // Fast inverse square-root
00209 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
00210 
00211 float inv_Sqrt(float x) {
00212     float halfx = 0.5f * x;
00213     float y = x;
00214     long i = *(long*)&y;
00215     i = 0x5f3759df - (i>>1);
00216     y = *(float*)&i;
00217     y = y * (1.5f - (halfx * y * y));
00218     return y;
00219     }
00220 
00221 
00222 void MahonyAHRS::getEuler(){
00223     
00224     float gx = 2*(q5*q7 - q4*q6);
00225     float gy = 2 * (q4*q5 + q6*q7);
00226     float gz = q4*q4 - q5*q5 - q6*q6 + q7*q7;
00227     
00228     roll = atan(gy / sqrt(gx*gx + gz*gz));
00229     pitch = atan(gx / sqrt(gy*gy + gz*gz));
00230     yaw = atan2(2 * q5 * q6 - 2 * q4 * q7, 2 * q4*q4 + 2 * q5 * q5 - 1);
00231     
00232     roll = roll*180/PI;
00233     pitch = pitch*180/PI;
00234     yaw = yaw*180/PI;
00235     
00236     if (ceil(roll) - roll <= .5){
00237         roll = ceil(roll);
00238         }
00239     else{
00240          roll = floor(roll);
00241         }
00242         
00243     if (ceil(pitch) - pitch <= .5){
00244         pitch = ceil(pitch);
00245         }
00246     else{
00247          pitch = floor(pitch);
00248         }
00249     
00250     if (ceil(yaw) - yaw <= .5){
00251         yaw = ceil(yaw);
00252         }
00253     else{
00254          yaw = floor(yaw);
00255         }
00256     }
00257     
00258 int16_t MahonyAHRS::getRoll(){
00259     return (int16_t)roll;
00260     }
00261     
00262 int16_t MahonyAHRS::getPitch(){
00263     return (int16_t)pitch;
00264     }
00265     
00266 int16_t MahonyAHRS::getYaw(){
00267     return (int16_t)yaw;
00268     }
00269 
00270 //====================================================================================================
00271 // END OF CODE
00272 //====================================================================================================