Diff: MPU6050.cpp

Revision:

7:ba03ec2cc88c

Parent:

6:5b90f2b5e6d9

Child:

8:e84123602ff0

--- a/MPU6050.cpp	Wed Aug 05 13:15:07 2015 +0000
+++ b/MPU6050.cpp	Fri Mar 17 10:15:15 2017 +0000
@@ -1,40 +1,11 @@
-/*   MPU6050 Library   
-*    
-*    @author: Baser Kandehir 
-*    @date: July 16, 2015
-*    @license: MIT license
-*     
-*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
-*
-*   Permission is hereby granted, free of charge, to any person obtaining a copy
-*   of this software and associated documentation files (the "Software"), to deal
-*   in the Software without restriction, including without limitation the rights
-*   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-*   copies of the Software, and to permit persons to whom the Software is
-*   furnished to do so, subject to the following conditions:
-*
-*   The above copyright notice and this permission notice shall be included in
-*   all copies or substantial portions of the Software.
-*
-*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-*   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-*   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-*   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-*   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-*   THE SOFTWARE.
-*
-*/
-
 // Most of the code is adapted from Kris Winer's MPU6050 library
 
 #include "MPU6050.h"
 
-/* For LPC1768 board */
 //I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
 
 /* For NUCLEO-F411RE board */
-static I2C i2c(D14,D15);         // setup i2c (SDA,SCL)
+static I2C i2c(p20, p19);       // setup i2c (SDA,SCL)
 
 /* Set initial input parameters */
 
@@ -147,12 +118,13 @@
     if(whoAmI==0x68)
     {
         pc.printf("MPU6050 is online... \r\n");  
-        led2=1;
+        //led2=1;
+        
     }
     else
     {
         pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n");  
-        toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
+       // toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
     }  
 }
 
@@ -210,6 +182,10 @@
     dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // ACCEL_XOUT
     dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // ACCEL_YOUT
     dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // ACCEL_ZOUT
+    ax=dest[0];
+    ay=dest[1];
+    az=dest[2];
+  //  printf("");
 }
 
 void MPU6050::readGyroData(int16_t* dest)
@@ -346,35 +322,42 @@
     dest2[1] = gyro_bias[1]*gRes;
     dest2[2] = gyro_bias[2]*gRes;
 }
-
-void MPU6050::complementaryFilter(float* pitch, float* roll)
+void MPU6050::complementaryFilter()
 {
     /* 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]; 
+    ax=accelData[0];
+    ay=accelData[1];
+    az=accelData[2];
+  //  ax = accelData[0]*aRes - accelBias[0];
+   // ay = accelData[1]*aRes - accelBias[1];
+   // az = accelData[2]*aRes - accelBias[2]; 
+   // printf("%.1f,    %.1f,    %.1f   ",ax,ay,az); 
 
     /* 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;
+    getGres();
+    gx=  gyroData[0];
+    gy=  gyroData[1];
+    gz=  gyroData[2];   
+    //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]; 
+  //printf("              %.1f,     %.1f,     %.1f\r\n",gx,gy,gz); 
+   
+   // 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
+ //   *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;
+  //  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;  
+ //   *pitch = *pitch * 0.98 + pitchAcc * 0.02;
+  //  *roll  = *roll  * 0.98 + rollAcc  * 0.02;  
 }