Diff: main.cpp

Revision:

3:88737ad5c803

Parent:

2:497faa1563ea

Child:

4:33fef1998fc8

--- a/main.cpp	Thu Jul 09 12:28:26 2015 +0000
+++ b/main.cpp	Mon Jul 13 13:19:08 2015 +0000
@@ -40,9 +40,15 @@
 Serial ftdi(p13,p14);    // default baud rate: 9600
 MPU6050 mpu6050;         // class: MPU6050, object: mpu6050 
 Ticker toggler1;
+Ticker filter;           
 
 void toggle_led1();
 void toggle_led2();
+void complementaryFilter(float* pitch, float* roll);
+void compFilter();
+
+float pitchAngle = 0;
+float rollAngle = 0;
 
 int main() 
 {
@@ -60,28 +66,72 @@
     
     while(1) 
     {
-        /* Get actual acc value */
-        mpu6050.readAccelData(accelData);
-        mpu6050.getAres();
-        ax = accelData[0]*aRes - accelBias[0];
-        ay = accelData[1]*aRes - accelBias[1];
-        az = accelData[2]*aRes - accelBias[2]; 
+//        /* Get actual acc value */
+//        mpu6050.readAccelData(accelData);
+//        mpu6050.getAres();
+//        ax = accelData[0]*aRes - accelBias[0];
+//        ay = accelData[1]*aRes - accelBias[1];
+//        az = accelData[2]*aRes - accelBias[2]; 
+//        
+//        /* Get actual gyro value */
+//        mpu6050.readGyroData(gyroData);
+//        mpu6050.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]; 
+//        
+//        ftdi.printf(" _____________________________________________________________  \r\n");
+//        ftdi.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f                \r\n",ax,ay,az);
+//        ftdi.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f                \r\n",gx,gy,gz);
+//        ftdi.printf("|_____________________________________________________________  \r\n\r\n");
+//        
+//        wait(2.5);
+                
+        filter.attach(&compFilter, 0.005);    // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
         
-        /* Get actual gyro value */
-        mpu6050.readGyroData(gyroData);
-        mpu6050.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]; 
+        ftdi.printf(" _______________\r\n");
+        ftdi.printf("| Pitch: %.3f   \r\n",pitchAngle);
+        ftdi.printf("| Roll:  %.3f   \r\n",rollAngle);
+        ftdi.printf("|_______________\r\n\r\n");
         
-        ftdi.printf(" _____________________________________________________________  \r\n");
-        ftdi.printf("| Accelerometer(g) | ax=%.3f | ay=%.3f | az=%.3f                \r\n",ax,ay,az);
-        ftdi.printf("| Gyroscope(deg/s) | gx=%.3f | gy=%.3f | gz=%.3f                \r\n",gx,gy,gz);
-        ftdi.printf("|_____________________________________________________________  \r\n\r\n");
-        
-        wait(2.5);
+        wait(1);
     }
 }
 
 void toggle_led1() {ledToggle(1);}
 void toggle_led2() {ledToggle(2);}
+
+/* This function is created to avoid address error that caused from Ticker.attach func */ 
+void compFilter() {complementaryFilter(&pitchAngle, &rollAngle);}
+
+void complementaryFilter(float* pitch, float* roll)
+{
+    /* Get actual acc value */
+    mpu6050.readAccelData(accelData);
+    mpu6050.getAres();
+    ax = accelData[0]*aRes - accelBias[0];
+    ay = accelData[1]*aRes - accelBias[1];
+    az = accelData[2]*aRes - accelBias[2]; 
+
+    /* Get actual gyro value */
+    mpu6050.readGyroData(gyroData);
+    mpu6050.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((float)accelData[1], (float)accelData[2])*180/PI;
+    rollAcc  = atan2f((float)accelData[0], (float)accelData[2])*180/PI;
+    
+    /* Apply Complementary Filter */
+    *pitch = *pitch * 0.98 + pitchAcc * 0.02;
+    *roll  = *roll  * 0.98 + rollAcc  * 0.02;   
+}
\ No newline at end of file