Montvydas Klumbys / MPU6050

Thanks Erik Olieman for his beta library, that saved me a huge amount of time when getting Raw data from MPU6050 module! I was able to update this library by adding additional functions, which would allow a fast angle calculation by using complementary filter. I will probably be updating this library more soon to add additional functionality or make some changes that would look sensible.

Dependents:   QuadcopterProgram 3DTracking ControlYokutan2017 Gyro ... more

Fork of MPU6050 by Erik -

Files at this revision

API Documentation at this revision

Revision 7:56e591a74939, committed 2015-02-13

Comitter:
moklumbys
Date:
Fri Feb 13 01:04:17 2015 +0000
Parent:
6:502448484f91
Child:
8:b1570b99df9e
Commit message:
So, added the final function to the library, which would allow to compute angle, knowing the time difference between each calculation.

Changed in this revision

MPU6050.cpp Show annotated file Show diff for this revision Revisions of this file
MPU6050.h Show annotated file Show diff for this revision Revisions of this file 
--- a/MPU6050.cpp	Thu Feb 12 21:01:38 2015 +0000
+++ b/MPU6050.cpp	Fri Feb 13 01:04:17 2015 +0000
@@ -181,6 +181,27 @@
         wait (0.01);    //wait between each reading for accuracy (maybe will not need later on) ****))#$(#@)$@#)(#@(%)@#(%@)(#%#@$--------------
     } 
 }
+
+void MPU6050::computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime)
+{
+    float gyro[3];
+    float accAngle[3];
+    
+    this->getGyro(gyro);
+    this->getAcceleroAngle(accAngle);
+    
+    for (int i = 0; i < 3; i++)
+    {
+        gyro[i] -= gyroOffset[i];
+        accAngle[i] -= accOffset[i];
+    }
+    
+    angle[X_AXIS] = ALPHA * (angle[X_AXIS] + GYRO_SCALE*gyro[X_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[X_AXIS];  //apply filter on both reading to get all angles
+    angle[Y_AXIS] = ALPHA * (angle[Y_AXIS] + GYRO_SCALE*gyro[Y_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Y_AXIS];
+  //angle[Z_AXIS] = ALPHA * (angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime)) + (1-ALPHA)*accAngle[Z_AXIS];
+    angle[Z_AXIS] = angle[Z_AXIS] + GYRO_SCALE*gyro[Z_AXIS]*(*currTime-*prevTime);  //this is Yaw hopefully :D
+    //Y = atan2(rawY,rawZ) * 180 / PI;  //This spits out values between -180 to 180 (360 degrees)    
+}
 //--------------------------------------------------
 //------------------Gyroscope-----------------------
 //--------------------------------------------------

--- a/MPU6050.h	Thu Feb 12 21:01:38 2015 +0000
+++ b/MPU6050.h	Fri Feb 13 01:04:17 2015 +0000
@@ -80,6 +80,10 @@
 
 #define RADIANS_TO_DEGREES 180/3.1415926536
 
+#define ALPHA 0.97   //filter constant
+
+#define GYRO_SCALE 2.31 //scale the gyro
+
 /** MPU6050 IMU library.
   *
   * Example:
@@ -266,6 +270,7 @@
     //added aditional functions
      void getAcceleroAngle( float *data );
      void getOffset(float *accOffset, float *gyroOffset, int sampleSize);
+     void computeAngle (float *angle, float *accOffset, float *gyroOffset, float *currTime, float *prevTime);
         
      private: