Diff: MPU6050.cpp

Revision:

2:3e0dfce73a58

Parent:

0:954f15bd95f1

Child:

3:a173ad187e67

diff -r a248e65a25cc -r 3e0dfce73a58 MPU6050.cpp
--- a/MPU6050.cpp	Mon Jul 13 13:18:34 2015 +0000
+++ b/MPU6050.cpp	Thu Jul 16 13:56:09 2015 +0000
@@ -1,5 +1,5 @@
 /*   @author: Baser Kandehir 
-*    @date: July 9, 2015
+*    @date: July 16, 2015
 *    @license: Use this code however you'd like
 */
 
@@ -115,16 +115,16 @@
 void MPU6050::whoAmI()
 {
     uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);   // Should return 0x68
-    ftdi.printf("I AM 0x%x \r\n",whoAmI);
+    pc.printf("I AM 0x%x \r\n",whoAmI);
     
     if(whoAmI==0x68)
     {
-        ftdi.printf("MPU6050 is online... \r\n");  
+        pc.printf("MPU6050 is online... \r\n");  
         led2=1;
     }
     else
     {
-        ftdi.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n");  
+        pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n");  
         toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
     }  
 }
@@ -317,3 +317,35 @@
     dest2[1] = gyro_bias[1]*gRes;
     dest2[2] = gyro_bias[2]*gRes;
 }
+
+void MPU6050::complementaryFilter(float* pitch, float* roll)
+{
+    /* Get actual acc value */
+    readAccelData(accelData);
+    getAres();
+    ax = accelData[0]*aRes - accelBias[0];
+    ay = accelData[1]*aRes - accelBias[1];
+    az = accelData[2]*aRes - accelBias[2]; 
+
+    /* Get actual gyro value */
+    readGyroData(gyroData);
+    getGres();     
+    gx = gyroData[0]*gRes;  // - gyroBias[0];      // Results are better without extracting gyroBias[i]
+    gy = gyroData[1]*gRes;  // - gyroBias[1]; 
+    gz = gyroData[2]*gRes;  // - gyroBias[2]; 
+
+    float pitchAcc, rollAcc;
+
+    /* Integrate the gyro data(deg/s) over time to get angle */
+    *pitch += gx * dt;  // Angle around the X-axis
+    *roll -=  gy * dt;  // Angle around the Y-axis
+    
+    /* Turning around the X-axis results in a vector on the Y-axis
+    whereas turning around the Y-axis results in a vector on the X-axis. */
+    pitchAcc = atan2f(accelData[1], accelData[2])*180/PI;
+    rollAcc  = atan2f(accelData[0], accelData[2])*180/PI;
+  
+    /* Apply Complementary Filter */
+    *pitch = *pitch * 0.98 + pitchAcc * 0.02;
+    *roll  = *roll  * 0.98 + rollAcc  * 0.02;  
+}