Async MPU6050 library

Revision 8:43f4b192e893, committed 2013-11-05

Comitter:

tyftyftyf

Date:

Tue Nov 05 11:28:54 2013 +0000

Parent:

7:95e74f827c08

Child:

9:d879deb55ae1

Commit message:

Changed in this revision

--- a/GurvIMU.cpp	Sat Nov 02 17:23:43 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,171 +0,0 @@
-#include "GurvIMU.h"
-#include "MPU6050.h"
-#include "mbed.h"
-
-#define  M_PI 3.1415926535897932384626433832795
-
-#define twoKpDef    (2.0f * 2.0f)   // 2 * proportional gain
-#define twoKiDef    (2.0f * 0.5f)   // 2 * integral gain
-
-
-GurvIMU::GurvIMU()
-{
-    //MPU
-    mpu = MPU6050(0x68); //0x69 = MPU6050 I2C ADDRESS
-
-    // Variable definitions
-    q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
-    twoKp = twoKpDef;                                            // 2 * proportional gain (Kp)
-    twoKi = twoKiDef;                                            // 2 * integral gain (Ki)
-    integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
-    cycle_nb = 0;
-    timer_us.start();    
-}
-
-//Function definitions
-
-void GurvIMU::getValues(float * values)
-{
-    int16_t accgyroval[6];
-    mpu.getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
-    for(int i = 0; i<3; i++) values[i] = (float) accgyroval[i];
-    for(int i = 3; i<6; i++) values[i] = (accgyroval[i]-offset[i]) * (M_PI / 180) / 16.4f;
-}
-
-void GurvIMU::getVerticalAcceleration(float *av)
-{
-    float values[6];
-    float q[4]; // quaternion
-    float g_x, g_y, g_z; // estimated gravity direction
-    getQ(q);
-  
-    g_x = 2 * (q[1]*q[3] - q[0]*q[2]);
-    g_y = 2 * (q[0]*q[1] + q[2]*q[3]);
-    g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];   
-    
-    getValues(values);
-    *av = g_x*values[0]+g_y*values[1]+g_z*values[2]-offset[2];
-}
-     
-
-void GurvIMU::getOffset(void)
-{
-    int sample_nb = 50;
-    float values[6];
-    for(int i=0; i<6 ; i++) offset[i] = 0;
-    for(int i=0; i<sample_nb; i++) {
-        getValues(values);
-        for(int j=0; j<6; j++) offset[j]+=values[j];        
-    }     
-    for(int j=0; j<6; j++) offset[j]/=sample_nb;
-}
-
-
-void GurvIMU::AHRS_update(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;
-    
-    dt_us=timer_us.read_us();
-    sample_freq = 1.0 / ((dt_us) / 1000000.0);
-    timer_us.reset();
-    
-    // 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
-        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 * (1.0f / sample_freq);    // integral error scaled by Ki
-            integralFBy += twoKi * halfey * (1.0f / sample_freq);
-            integralFBz += twoKi * halfez * (1.0f / sample_freq);
-            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 * (1.0f / sample_freq));     // pre-multiply common factors
-    gy *= (0.5f * (1.0f / sample_freq));
-    gz *= (0.5f * (1.0f / sample_freq));
-    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 GurvIMU::getQ(float * q) {
-  float val[6];
-  getValues(val);
-  //while(cycle_nb < 1000){
-    AHRS_update(val[3], val[4], val[5], val[0], val[1], val[2]);
-  //cycle_nb++;}
-
-  q[0] = q0;
-  q[1] = q1;
-  q[2] = q2;
-  q[3] = q3;
-  
-}
-
-void GurvIMU::getYawPitchRollRad(float * ypr) {
-  float q[4]; // quaternion
-  float g_x, g_y, g_z; // estimated gravity direction
-  getQ(q);
-  
-  g_x = 2 * (q[1]*q[3] - q[0]*q[2]);
-  g_y = 2 * (q[0]*q[1] + q[2]*q[3]);
-  g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
-  
-  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
-  ypr[1] = atan(g_x * invSqrt(g_y*g_y + g_z*g_z));
-  ypr[2] = atan(g_y * invSqrt(g_x*g_x + g_z*g_z));
-}
-
-void GurvIMU::init()
-{
-    mpu.initialize();
-    mpu.setI2CMasterModeEnabled(0);
-    mpu.setI2CBypassEnabled(1);
-    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
-    getOffset();
-    wait(0.005);
-}

--- a/GurvIMU.h	Sat Nov 02 17:23:43 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,40 +0,0 @@
-#ifndef _GURVIMU_H_
-#define _GURVIMU_H_
-
-#include "mbed.h"
-#include "MPU6050.h"
-
-class GurvIMU {
-    private:
-        //Variables
-        MPU6050 mpu;
-        float twoKp;            // 2 * proportional gain (Kp)
-        float twoKi;            // 2 * integral gain (Ki)
-        float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki
-        float q0, q1, q2, q3;   // quaternion of sensor frame relative to auxiliary frame
-        float ax, ay, az, gx, gy, gz;
-        float sample_freq;
-        float offset[6];
-        Timer timer_us;
-        float dt_us;
-        float cycle_nb;
-        
-        
-        //Functions
-        void getOffset();
-        void getValues(float * values);
-        void AHRS_update(float gx, float gy, float gz, float ax, float ay, float az);
-        void getQ(float * q);
-
-    public:
-        GurvIMU();
-        void init();
-        void getYawPitchRollRad(float * ypr);
-        void getVerticalAcceleration(float * av);
-        
-};
-
-//Fast Inverse Square Root
-float invSqrt(float number);
-
-#endif /* _GURVIMU_H_ */
\ No newline at end of file