GurvIMU.cpp

00001 #include "GurvIMU.h"
00002 #include "MPU6050.h"
00003 #include "mbed.h"
00004 
00005 #define  M_PI 3.1415926535897932384626433832795
00006 
00007 #define twoKpDef    (2.0f * 2.0f)   // 2 * proportional gain
00008 #define twoKiDef    (2.0f * 0.5f)   // 2 * integral gain
00009 
00010 
00011 GurvIMU::GurvIMU()
00012 {
00013     //MPU
00014     mpu = MPU6050(0x69); //0x69 = MPU6050 I2C ADDRESS
00015 
00016     // Variable definitions
00017     q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
00018     twoKp = twoKpDef;                                            // 2 * proportional gain (Kp)
00019     twoKi = twoKiDef;                                            // 2 * integral gain (Ki)
00020     integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
00021     cycle_nb = 0;
00022     timer_us.start();    
00023 }
00024 
00025 //Function definitions
00026 
00027 void GurvIMU::getValues(float * values)
00028 {
00029     int16_t accgyroval[6];
00030     mpu.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
00031     for(int i = 0; i<3; i++) values[i] = (float) accgyroval[i];
00032     for(int i = 3; i<6; i++) values[i] = (accgyroval[i]-offset[i]) * (M_PI / 180) / 16.4f;
00033 }
00034 
00035 void GurvIMU::getVerticalAcceleration(float av)
00036 {
00037     float values[6];
00038     float q[4]; // quaternion
00039     float g_x, g_y, g_z; // estimated gravity direction
00040     getQ(q);
00041   
00042     g_x = 2 * (q[1]*q[3] - q[0]*q[2]);
00043     g_y = 2 * (q[0]*q[1] + q[2]*q[3]);
00044     g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];   
00045     
00046     getValues(values);
00047     av = g_x*values[0]+g_y*values[1]+g_z*values[2]-offset[2];
00048 }
00049      
00050 
00051 void GurvIMU::getOffset(void)
00052 {
00053     int sample_nb = 50;
00054     float values[6];
00055     for(int i=0; i<6 ; i++) offset[i] = 0;
00056     for(int i=0; i<sample_nb; i++) {
00057         getValues(values);
00058         for(int j=0; j<6; j++) offset[j]+=values[j];        
00059     }     
00060     for(int j=0; j<6; j++) offset[j]/=sample_nb;
00061 }
00062 
00063 
00064 void GurvIMU::AHRS_update(float gx, float gy, float gz, float ax, float ay, float az)
00065 {
00066     float recipNorm;
00067     float halfvx, halfvy, halfvz;
00068     float halfex, halfey, halfez;
00069     float qa, qb, qc;
00070     
00071     dt_us=timer_us.read_us();
00072     sample_freq = 1.0 / ((dt_us) / 1000000.0);
00073     timer_us.reset();
00074     
00075     // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
00076     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
00077 
00078         // Normalise accelerometer measurement
00079         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
00080         ax *= recipNorm;
00081         ay *= recipNorm;
00082         az *= recipNorm;        
00083 
00084         // Estimated direction of gravity
00085         halfvx = q1 * q3 - q0 * q2;
00086         halfvy = q0 * q1 + q2 * q3;
00087         halfvz = q0 * q0 - 0.5f + q3 * q3;
00088     
00089         // Error is sum of cross product between estimated and measured direction of gravity
00090         halfex = (ay * halfvz - az * halfvy);
00091         halfey = (az * halfvx - ax * halfvz);
00092         halfez = (ax * halfvy - ay * halfvx);
00093 
00094         // Compute and apply integral feedback if enabled
00095         if(twoKi > 0.0f) {
00096             integralFBx += twoKi * halfex * (1.0f / sample_freq);    // integral error scaled by Ki
00097             integralFBy += twoKi * halfey * (1.0f / sample_freq);
00098             integralFBz += twoKi * halfez * (1.0f / sample_freq);
00099             gx += integralFBx;  // apply integral feedback
00100             gy += integralFBy;
00101             gz += integralFBz;
00102         }
00103         else {
00104             integralFBx = 0.0f; // prevent integral windup
00105             integralFBy = 0.0f;
00106             integralFBz = 0.0f;
00107         }
00108 
00109         // Apply proportional feedback
00110         gx += twoKp * halfex;
00111         gy += twoKp * halfey;
00112         gz += twoKp * halfez;
00113     }
00114     
00115     // Integrate rate of change of quaternion
00116     gx *= (0.5f * (1.0f / sample_freq));     // pre-multiply common factors
00117     gy *= (0.5f * (1.0f / sample_freq));
00118     gz *= (0.5f * (1.0f / sample_freq));
00119     qa = q0;
00120     qb = q1;
00121     qc = q2;
00122     q0 += (-qb * gx - qc * gy - q3 * gz);
00123     q1 += (qa * gx + qc * gz - q3 * gy);
00124     q2 += (qa * gy - qb * gz + q3 * gx);
00125     q3 += (qa * gz + qb * gy - qc * gx); 
00126     
00127     // Normalise quaternion
00128     recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
00129     q0 *= recipNorm;
00130     q1 *= recipNorm;
00131     q2 *= recipNorm;
00132     q3 *= recipNorm;
00133 }
00134 
00135 void GurvIMU::getQ(float * q) {
00136   float val[6];
00137   getValues(val);
00138   //while(cycle_nb < 1000){
00139     AHRS_update(val[3], val[4], val[5], val[0], val[1], val[2]);
00140   //cycle_nb++;}
00141 
00142   q[0] = q0;
00143   q[1] = q1;
00144   q[2] = q2;
00145   q[3] = q3;
00146   
00147 }
00148 
00149 void GurvIMU::getYawPitchRollRad(float * ypr) {
00150   float q[4]; // quaternion
00151   float g_x, g_y, g_z; // estimated gravity direction
00152   getQ(q);
00153   
00154   g_x = 2 * (q[1]*q[3] - q[0]*q[2]);
00155   g_y = 2 * (q[0]*q[1] + q[2]*q[3]);
00156   g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
00157   
00158   ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
00159   ypr[1] = atan(g_x * invSqrt(g_y*g_y + g_z*g_z));
00160   ypr[2] = atan(g_y * invSqrt(g_x*g_x + g_z*g_z));
00161 }
00162 
00163 void GurvIMU::init()
00164 {
00165     mpu.initialize();
00166     mpu.setI2CMasterModeEnabled(0);
00167     mpu.setI2CBypassEnabled(1);
00168     mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00169     getOffset();
00170     wait(0.005);
00171 }
00172 
00173 
00174 float invSqrt(float number)
00175 {
00176     volatile long i;
00177     volatile float x, y;
00178     volatile const float f = 1.5F;
00179 
00180     x = number * 0.5F;
00181     y = number;
00182     i = * ( long * ) &y;
00183     i = 0x5f375a86 - ( i >> 1 );
00184     y = * ( float * ) &i;
00185     y = y * ( f - ( x * y * y ) );
00186     return y;
00187 }