Revision 2:3e0dfce73a58, committed 2015-07-16

Comitter:

BaserK

Date:

Thu Jul 16 13:56:09 2015 +0000

Parent:

1:a248e65a25cc

Child:

3:a173ad187e67

Commit message:

complementary filter is added to the library as a function

Changed in this revision

--- 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;  
+}

--- a/MPU6050.h	Mon Jul 13 13:18:34 2015 +0000
+++ b/MPU6050.h	Thu Jul 16 13:56:09 2015 +0000
@@ -7,7 +7,7 @@
 #include "math.h"
 #include "MPU6050RegDef.h"
 
-#define PI 3.14159265359
+#define PI 3.14159265359    // This value will be used when calculating angles
 #define dt 0.005            // 200 Hz sampling period
 
 extern I2C i2c;             // extern the i2c in order to able to use from other files
@@ -15,11 +15,11 @@
 
 /* whoAmI func uses this func, variables etc */
 extern Ticker toggler1;  
-extern Serial ftdi;   
+extern Serial pc;   
 extern DigitalOut led2;
 extern void toggle_led1();
 
-/* Sensor datas to be used in main.cpp */
+/* Sensor datas to be used in program */
 extern float ax,ay,az;
 extern float gx,gy,gz;
 extern int16_t accelData[3],gyroData[3],tempData;
@@ -42,6 +42,7 @@
     void readGyroData(int16_t* dest);
     int16_t readTempData();
     void calibrate(float* dest1, float* dest2);
+    void complementaryFilter(float* pitch, float* roll);
 };
 
 #endif
\ No newline at end of file