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 -

Diff: MPU6050.cpp

Revision:
7:56e591a74939
Parent:
6:502448484f91
Child:
8:b1570b99df9e
--- 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-----------------------
 //--------------------------------------------------