Diff: MPU6050.cpp

Revision:

7:4e799f8ec792

Parent:

6:5b90f2b5e6d9

diff -r 5b90f2b5e6d9 -r 4e799f8ec792 MPU6050.cpp
--- a/MPU6050.cpp	Wed Aug 05 13:15:07 2015 +0000
+++ b/MPU6050.cpp	Mon May 30 08:11:07 2016 +0000
@@ -30,12 +30,6 @@
 
 #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)
-
 /* Set initial input parameters */
 
 // Acc Full Scale Range  +-2G 4G 8G 16G  
@@ -64,12 +58,17 @@
 float gyroBias[3] = {0, 0, 0};   // Bias corrections for gyro 
 
 // Specify sensor full scale range
-int Ascale = AFS_2G;
-int Gscale = GFS_250DPS;
+int Ascale = AFS_16G;
+int Gscale = GFS_2000DPS;
 
 // Scale resolutions per LSB for the sensors
 float aRes, gRes; 
 
+MPU6050::MPU6050 (PinName p_sda, PinName p_scl ) : i2c(p_sda, p_scl) {
+    currentGyroRange = 0;
+    currentAcceleroRange=0;
+}
+
 // Calculates Acc resolution
 void MPU6050::getAres()
 {
@@ -142,17 +141,17 @@
 void MPU6050::whoAmI()
 {
     uint8_t whoAmI = readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);   // Should return 0x68
-    pc.printf("I AM 0x%x \r\n",whoAmI);
+    //pc.printf("I AM 0x%x \r\n",whoAmI);
     
     if(whoAmI==0x68)
     {
-        pc.printf("MPU6050 is online... \r\n");  
-        led2=1;
+    //    pc.printf("MPU6050 is online... \r\n");  
+    //    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
+    //    pc.printf("Could not connect to MPU6050 \r\nCheck the connections... \r\n");  
+    //    toggler1.attach(&toggle_led1,0.1);     // toggles led1 every 100 ms
     }  
 }
 
@@ -201,7 +200,7 @@
     wait_ms(100);    // wait 100 ms to stabilize        
 }
 
-void MPU6050::readAccelData(int16_t* dest)
+void MPU6050::getAccelRaw(int16_t* dest)
 {
     uint8_t rawData[6];  // x,y,z acc data            
     readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);   // read six raw data registers sequentially and write them into data array
@@ -212,7 +211,7 @@
     dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // ACCEL_ZOUT
 }
 
-void MPU6050::readGyroData(int16_t* dest)
+void MPU6050::getGyroRaw(int16_t* dest)
 {
     uint8_t rawData[6];  // x,y,z gyro data            
     readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);   // read the six raw data registers sequentially and write them into data array
@@ -222,6 +221,14 @@
     dest[1] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // GYRO_YOUT
     dest[2] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // GYRO_ZOUT    
 }
+
+void MPU6050::getGyro( float *data ) {
+    int16_t temp[3];
+    this->getGyroRaw(temp);
+    data[0]=(float)temp[0] * gRes;
+    data[1]=(float)temp[1] * gRes;
+    data[2]=(float)temp[2] * gRes;
+}
     
 int16_t MPU6050::readTempData()
 {
@@ -346,18 +353,18 @@
     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);
+    // Get actual acc value 
+    getAccelRaw(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);
+    // Get actual gyro value 
+    getGyroRaw(gyroData);
     getGres();     
     gx = gyroData[0]*gRes;  // - gyroBias[0];      // Results are better without extracting gyroBias[i]
     gy = gyroData[1]*gRes;  // - gyroBias[1]; 
@@ -365,16 +372,17 @@
 
     float pitchAcc, rollAcc;
 
-    /* Integrate the gyro data(deg/s) over time to get angle */
+    // 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. */
+    // 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 */
+    // Apply Complementary Filter 
     *pitch = *pitch * 0.98 + pitchAcc * 0.02;
     *roll  = *roll  * 0.98 + rollAcc  * 0.02;  
 }
+*/
\ No newline at end of file