Revision 7:5fa6b4d52805, committed 2018-01-19

Comitter:

dgu

Date:

Fri Jan 19 09:51:24 2018 +0000

Parent:

6:5b90f2b5e6d9

Commit message:

na

Changed in this revision

diff -r 5b90f2b5e6d9 -r 5fa6b4d52805 MPU6050.cpp
--- a/MPU6050.cpp	Wed Aug 05 13:15:07 2015 +0000
+++ b/MPU6050.cpp	Fri Jan 19 09:51:24 2018 +0000
@@ -31,10 +31,10 @@
 #include "MPU6050.h"
 
 /* For LPC1768 board */
-//I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
+I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
 
 /* For NUCLEO-F411RE board */
-static I2C i2c(D14,D15);         // setup i2c (SDA,SCL)
+//static I2C i2c(D14,D15);         // setup i2c (SDA,SCL)
 
 /* Set initial input parameters */
 
@@ -347,7 +347,7 @@
     dest2[2] = gyro_bias[2]*gRes;
 }
 
-void MPU6050::complementaryFilter(float* pitch, float* roll)
+void MPU6050::complementaryFilter(float* pitch, float* roll, float* yaw)
 {
     /* Get actual acc value */
     readAccelData(accelData);
@@ -367,7 +367,8 @@
 
     /* 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
+    *roll  -= gy * dt;  // Angle around the Y-axis
+    *yaw   += gz * dt;  // Angle around the Z-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. */
@@ -376,5 +377,6 @@
   
     /* Apply Complementary Filter */
     *pitch = *pitch * 0.98 + pitchAcc * 0.02;
-    *roll  = *roll  * 0.98 + rollAcc  * 0.02;  
+    *roll  = *roll  * 0.98 + rollAcc  * 0.02; 
+    *yaw   = *yaw; 
 }

diff -r 5b90f2b5e6d9 -r 5fa6b4d52805 MPU6050.h
--- a/MPU6050.h	Wed Aug 05 13:15:07 2015 +0000
+++ b/MPU6050.h	Fri Jan 19 09:51:24 2018 +0000
@@ -64,7 +64,7 @@
     void readGyroData(int16_t* dest);
     int16_t readTempData();
     void calibrate(float* dest1, float* dest2);
-    void complementaryFilter(float* pitch, float* roll);
+    void complementaryFilter(float* pitch, float* roll, float* yaw);
 };
 
 #endif
\ No newline at end of file