Diff: MPU6050.cpp

Revision:

5:5873df1e58be

Parent:

4:268d3fcb92ba

Child:

6:502448484f91

diff -r 268d3fcb92ba -r 5873df1e58be MPU6050.cpp
--- a/MPU6050.cpp	Thu Feb 12 20:36:53 2015 +0000
+++ b/MPU6050.cpp	Thu Feb 12 20:56:04 2015 +0000
@@ -158,6 +158,29 @@
     data[Z_AXIS] = atan (sqrt(pow(temp[X_AXIS], 2) + pow(temp[Y_AXIS], 2))/temp[Z_AXIS]) * RADIANS_TO_DEGREES; //not sure what is this one :D
     
 }
+void MPU6050::getOffset(float *accOffset, float *gyroOffset, int sampleSize){
+    float gyro[3];      
+    float accAngle[3];
+    
+    for (int i = 0; i < 3; i++) //initialise to 0.0 offsets
+    {
+        accOffset[i] = 0.0;
+        gyroOffset[i] = 0.0;
+    }
+    
+    for (int i = 0; i < sampleSize; i++)
+    {
+        mpu.getGyro(gyro); //take real life measurements  
+        mpu.getAcceleroAngle (accAngle);
+        
+        for (int j = 0; j < 3; j++)
+        {
+            *(accOffset+j) += accAngle[j]/sampleSize;    //average measurements
+            *(gyroOffset+j) += gyro[j]/sampleSize;
+        }
+        wait (0.01);    //wait between each reading for accuracy (maybe will not need later on) ****))#$(#@)$@#)(#@(%)@#(%@)(#%#@$--------------
+    } 
+}
 //--------------------------------------------------
 //------------------Gyroscope-----------------------
 //--------------------------------------------------