Aloïs Wolff / MPU6050_tmp

MPU6050 arduino port by Szymon Gaertig (http://mbed.org/users/garfieldsg/code/MPU6050/) 1 memory overflow error corrected.

Dependents:   MbedFreeIMU gurvanAHRS

Fork of MPU6050 by Simon Garfieldsg

GurvIMU.cpp@6:40ac13ef7290, 2013-06-22 (annotated)

Committer:
pommzorz
Date:
Sat Jun 22 11:23:45 2013 +0000
Revision:
6:40ac13ef7290
Commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pommzorz 6:40ac13ef7290 1 #include "GurvIMU.h"
pommzorz 6:40ac13ef7290 2 #include "MPU6050.h"
pommzorz 6:40ac13ef7290 3 #include "mbed.h"
pommzorz 6:40ac13ef7290 4
pommzorz 6:40ac13ef7290 5 #define M_PI 3.1415926535897932384626433832795
pommzorz 6:40ac13ef7290 6
pommzorz 6:40ac13ef7290 7 #define twoKpDef (2.0f * 2.0f) // 2 * proportional gain
pommzorz 6:40ac13ef7290 8 #define twoKiDef (2.0f * 0.5f) // 2 * integral gain
pommzorz 6:40ac13ef7290 9
pommzorz 6:40ac13ef7290 10
pommzorz 6:40ac13ef7290 11 GurvIMU::GurvIMU()
pommzorz 6:40ac13ef7290 12 {
pommzorz 6:40ac13ef7290 13 //MPU
pommzorz 6:40ac13ef7290 14 mpu = MPU6050(0x69); //0x69 = MPU6050 I2C ADDRESS
pommzorz 6:40ac13ef7290 15
pommzorz 6:40ac13ef7290 16 // Variable definitions
pommzorz 6:40ac13ef7290 17 q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
pommzorz 6:40ac13ef7290 18 twoKp = twoKpDef; // 2 * proportional gain (Kp)
pommzorz 6:40ac13ef7290 19 twoKi = twoKiDef; // 2 * integral gain (Ki)
pommzorz 6:40ac13ef7290 20 integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
pommzorz 6:40ac13ef7290 21 cycle_nb = 0;
pommzorz 6:40ac13ef7290 22 timer_us.start();
pommzorz 6:40ac13ef7290 23 }
pommzorz 6:40ac13ef7290 24
pommzorz 6:40ac13ef7290 25 //Function definitions
pommzorz 6:40ac13ef7290 26
pommzorz 6:40ac13ef7290 27 void GurvIMU::getValues(float * values)
pommzorz 6:40ac13ef7290 28 {
pommzorz 6:40ac13ef7290 29 int16_t accgyroval[6];
pommzorz 6:40ac13ef7290 30 mpu.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
pommzorz 6:40ac13ef7290 31 for(int i = 0; i<3; i++) values[i] = (float) accgyroval[i];
pommzorz 6:40ac13ef7290 32 for(int i = 3; i<6; i++) values[i] = (accgyroval[i]-offset[i]) * (M_PI / 180) / 16.4f;
pommzorz 6:40ac13ef7290 33 }
pommzorz 6:40ac13ef7290 34
pommzorz 6:40ac13ef7290 35 void GurvIMU::getVerticalAcceleration(float av)
pommzorz 6:40ac13ef7290 36 {
pommzorz 6:40ac13ef7290 37 float values[6];
pommzorz 6:40ac13ef7290 38 float q[4]; // quaternion
pommzorz 6:40ac13ef7290 39 float g_x, g_y, g_z; // estimated gravity direction
pommzorz 6:40ac13ef7290 40 getQ(q);
pommzorz 6:40ac13ef7290 41
pommzorz 6:40ac13ef7290 42 g_x = 2 * (q[1]*q[3] - q[0]*q[2]);
pommzorz 6:40ac13ef7290 43 g_y = 2 * (q[0]*q[1] + q[2]*q[3]);
pommzorz 6:40ac13ef7290 44 g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
pommzorz 6:40ac13ef7290 45
pommzorz 6:40ac13ef7290 46 getValues(values);
pommzorz 6:40ac13ef7290 47 av = g_x*values[0]+g_y*values[1]+g_z*values[2]-offset[2];
pommzorz 6:40ac13ef7290 48 }
pommzorz 6:40ac13ef7290 49
pommzorz 6:40ac13ef7290 50
pommzorz 6:40ac13ef7290 51 void GurvIMU::getOffset(void)
pommzorz 6:40ac13ef7290 52 {
pommzorz 6:40ac13ef7290 53 int sample_nb = 50;
pommzorz 6:40ac13ef7290 54 float values[6];
pommzorz 6:40ac13ef7290 55 for(int i=0; i<6 ; i++) offset[i] = 0;
pommzorz 6:40ac13ef7290 56 for(int i=0; i<sample_nb; i++) {
pommzorz 6:40ac13ef7290 57 getValues(values);
pommzorz 6:40ac13ef7290 58 for(int j=0; j<6; j++) offset[j]+=values[j];
pommzorz 6:40ac13ef7290 59 }
pommzorz 6:40ac13ef7290 60 for(int j=0; j<6; j++) offset[j]/=sample_nb;
pommzorz 6:40ac13ef7290 61 }
pommzorz 6:40ac13ef7290 62
pommzorz 6:40ac13ef7290 63
pommzorz 6:40ac13ef7290 64 void GurvIMU::AHRS_update(float gx, float gy, float gz, float ax, float ay, float az)
pommzorz 6:40ac13ef7290 65 {
pommzorz 6:40ac13ef7290 66 float recipNorm;
pommzorz 6:40ac13ef7290 67 float halfvx, halfvy, halfvz;
pommzorz 6:40ac13ef7290 68 float halfex, halfey, halfez;
pommzorz 6:40ac13ef7290 69 float qa, qb, qc;
pommzorz 6:40ac13ef7290 70
pommzorz 6:40ac13ef7290 71 dt_us=timer_us.read_us();
pommzorz 6:40ac13ef7290 72 sample_freq = 1.0 / ((dt_us) / 1000000.0);
pommzorz 6:40ac13ef7290 73 timer_us.reset();
pommzorz 6:40ac13ef7290 74
pommzorz 6:40ac13ef7290 75 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
pommzorz 6:40ac13ef7290 76 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
pommzorz 6:40ac13ef7290 77
pommzorz 6:40ac13ef7290 78 // Normalise accelerometer measurement
pommzorz 6:40ac13ef7290 79 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
pommzorz 6:40ac13ef7290 80 ax *= recipNorm;
pommzorz 6:40ac13ef7290 81 ay *= recipNorm;
pommzorz 6:40ac13ef7290 82 az *= recipNorm;
pommzorz 6:40ac13ef7290 83
pommzorz 6:40ac13ef7290 84 // Estimated direction of gravity
pommzorz 6:40ac13ef7290 85 halfvx = q1 * q3 - q0 * q2;
pommzorz 6:40ac13ef7290 86 halfvy = q0 * q1 + q2 * q3;
pommzorz 6:40ac13ef7290 87 halfvz = q0 * q0 - 0.5f + q3 * q3;
pommzorz 6:40ac13ef7290 88
pommzorz 6:40ac13ef7290 89 // Error is sum of cross product between estimated and measured direction of gravity
pommzorz 6:40ac13ef7290 90 halfex = (ay * halfvz - az * halfvy);
pommzorz 6:40ac13ef7290 91 halfey = (az * halfvx - ax * halfvz);
pommzorz 6:40ac13ef7290 92 halfez = (ax * halfvy - ay * halfvx);
pommzorz 6:40ac13ef7290 93
pommzorz 6:40ac13ef7290 94 // Compute and apply integral feedback if enabled
pommzorz 6:40ac13ef7290 95 if(twoKi > 0.0f) {
pommzorz 6:40ac13ef7290 96 integralFBx += twoKi * halfex * (1.0f / sample_freq); // integral error scaled by Ki
pommzorz 6:40ac13ef7290 97 integralFBy += twoKi * halfey * (1.0f / sample_freq);
pommzorz 6:40ac13ef7290 98 integralFBz += twoKi * halfez * (1.0f / sample_freq);
pommzorz 6:40ac13ef7290 99 gx += integralFBx; // apply integral feedback
pommzorz 6:40ac13ef7290 100 gy += integralFBy;
pommzorz 6:40ac13ef7290 101 gz += integralFBz;
pommzorz 6:40ac13ef7290 102 }
pommzorz 6:40ac13ef7290 103 else {
pommzorz 6:40ac13ef7290 104 integralFBx = 0.0f; // prevent integral windup
pommzorz 6:40ac13ef7290 105 integralFBy = 0.0f;
pommzorz 6:40ac13ef7290 106 integralFBz = 0.0f;
pommzorz 6:40ac13ef7290 107 }
pommzorz 6:40ac13ef7290 108
pommzorz 6:40ac13ef7290 109 // Apply proportional feedback
pommzorz 6:40ac13ef7290 110 gx += twoKp * halfex;
pommzorz 6:40ac13ef7290 111 gy += twoKp * halfey;
pommzorz 6:40ac13ef7290 112 gz += twoKp * halfez;
pommzorz 6:40ac13ef7290 113 }
pommzorz 6:40ac13ef7290 114
pommzorz 6:40ac13ef7290 115 // Integrate rate of change of quaternion
pommzorz 6:40ac13ef7290 116 gx *= (0.5f * (1.0f / sample_freq)); // pre-multiply common factors
pommzorz 6:40ac13ef7290 117 gy *= (0.5f * (1.0f / sample_freq));
pommzorz 6:40ac13ef7290 118 gz *= (0.5f * (1.0f / sample_freq));
pommzorz 6:40ac13ef7290 119 qa = q0;
pommzorz 6:40ac13ef7290 120 qb = q1;
pommzorz 6:40ac13ef7290 121 qc = q2;
pommzorz 6:40ac13ef7290 122 q0 += (-qb * gx - qc * gy - q3 * gz);
pommzorz 6:40ac13ef7290 123 q1 += (qa * gx + qc * gz - q3 * gy);
pommzorz 6:40ac13ef7290 124 q2 += (qa * gy - qb * gz + q3 * gx);
pommzorz 6:40ac13ef7290 125 q3 += (qa * gz + qb * gy - qc * gx);
pommzorz 6:40ac13ef7290 126
pommzorz 6:40ac13ef7290 127 // Normalise quaternion
pommzorz 6:40ac13ef7290 128 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
pommzorz 6:40ac13ef7290 129 q0 *= recipNorm;
pommzorz 6:40ac13ef7290 130 q1 *= recipNorm;
pommzorz 6:40ac13ef7290 131 q2 *= recipNorm;
pommzorz 6:40ac13ef7290 132 q3 *= recipNorm;
pommzorz 6:40ac13ef7290 133 }
pommzorz 6:40ac13ef7290 134
pommzorz 6:40ac13ef7290 135 void GurvIMU::getQ(float * q) {
pommzorz 6:40ac13ef7290 136 float val[6];
pommzorz 6:40ac13ef7290 137 getValues(val);
pommzorz 6:40ac13ef7290 138 //while(cycle_nb < 1000){
pommzorz 6:40ac13ef7290 139 AHRS_update(val[3], val[4], val[5], val[0], val[1], val[2]);
pommzorz 6:40ac13ef7290 140 //cycle_nb++;}
pommzorz 6:40ac13ef7290 141
pommzorz 6:40ac13ef7290 142 q[0] = q0;
pommzorz 6:40ac13ef7290 143 q[1] = q1;
pommzorz 6:40ac13ef7290 144 q[2] = q2;
pommzorz 6:40ac13ef7290 145 q[3] = q3;
pommzorz 6:40ac13ef7290 146
pommzorz 6:40ac13ef7290 147 }
pommzorz 6:40ac13ef7290 148
pommzorz 6:40ac13ef7290 149 void GurvIMU::getYawPitchRollRad(float * ypr) {
pommzorz 6:40ac13ef7290 150 float q[4]; // quaternion
pommzorz 6:40ac13ef7290 151 float g_x, g_y, g_z; // estimated gravity direction
pommzorz 6:40ac13ef7290 152 getQ(q);
pommzorz 6:40ac13ef7290 153
pommzorz 6:40ac13ef7290 154 g_x = 2 * (q[1]*q[3] - q[0]*q[2]);
pommzorz 6:40ac13ef7290 155 g_y = 2 * (q[0]*q[1] + q[2]*q[3]);
pommzorz 6:40ac13ef7290 156 g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
pommzorz 6:40ac13ef7290 157
pommzorz 6:40ac13ef7290 158 ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
pommzorz 6:40ac13ef7290 159 ypr[1] = atan(g_x * invSqrt(g_y*g_y + g_z*g_z));
pommzorz 6:40ac13ef7290 160 ypr[2] = atan(g_y * invSqrt(g_x*g_x + g_z*g_z));
pommzorz 6:40ac13ef7290 161 }
pommzorz 6:40ac13ef7290 162
pommzorz 6:40ac13ef7290 163 void GurvIMU::init()
pommzorz 6:40ac13ef7290 164 {
pommzorz 6:40ac13ef7290 165 mpu.initialize();
pommzorz 6:40ac13ef7290 166 mpu.setI2CMasterModeEnabled(0);
pommzorz 6:40ac13ef7290 167 mpu.setI2CBypassEnabled(1);
pommzorz 6:40ac13ef7290 168 mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
pommzorz 6:40ac13ef7290 169 getOffset();
pommzorz 6:40ac13ef7290 170 wait(0.005);
pommzorz 6:40ac13ef7290 171 }
pommzorz 6:40ac13ef7290 172
pommzorz 6:40ac13ef7290 173
pommzorz 6:40ac13ef7290 174 float invSqrt(float number)
pommzorz 6:40ac13ef7290 175 {
pommzorz 6:40ac13ef7290 176 volatile long i;
pommzorz 6:40ac13ef7290 177 volatile float x, y;
pommzorz 6:40ac13ef7290 178 volatile const float f = 1.5F;
pommzorz 6:40ac13ef7290 179
pommzorz 6:40ac13ef7290 180 x = number * 0.5F;
pommzorz 6:40ac13ef7290 181 y = number;
pommzorz 6:40ac13ef7290 182 i = * ( long * ) &y;
pommzorz 6:40ac13ef7290 183 i = 0x5f375a86 - ( i >> 1 );
pommzorz 6:40ac13ef7290 184 y = * ( float * ) &i;
pommzorz 6:40ac13ef7290 185 y = y * ( f - ( x * y * y ) );
pommzorz 6:40ac13ef7290 186 return y;
pommzorz 6:40ac13ef7290 187 }