Revision 7:4e799f8ec792, committed 2016-05-30

Comitter:

Decimus

Date:

Mon May 30 08:11:07 2016 +0000

Parent:

6:5b90f2b5e6d9

Commit message:

[+]

Changed in this revision

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

diff -r 5b90f2b5e6d9 -r 4e799f8ec792 MPU6050.h
--- a/MPU6050.h	Wed Aug 05 13:15:07 2015 +0000
+++ b/MPU6050.h	Mon May 30 08:11:07 2016 +0000
@@ -30,16 +30,16 @@
 #include "math.h"
 #include "MPU6050RegDef.h"
 
-#define PI 3.14159265359    // This value will be used when calculating angles
-#define dt 0.005            // 200 Hz sampling period
+//#define PI 3.14159265359    // This value will be used when calculating angles
+//#define dt 0.005            // 200 Hz sampling period
 
 extern float aRes, gRes; 
 
 /* whoAmI func uses this func, variables etc */
-extern Ticker toggler1;  
-extern Serial pc;   
-extern DigitalOut led2;
-extern void toggle_led1();
+//extern Ticker toggler1;  
+//extern Serial pc;   
+//extern DigitalOut led2;
+//extern void toggle_led1();
 
 /* Sensor datas to be used in program */
 extern float ax,ay,az;
@@ -48,10 +48,10 @@
 extern float accelBias[3], gyroBias[3];
  
 /* Function Prototypes */
-class MPU6050 
-{
-    protected:
+class MPU6050 {
     public: 
+    MPU6050(PinName p_sda, PinName p_scl);
+         
     void getAres();
     void getGres();
     void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
@@ -60,11 +60,17 @@
     void whoAmI();
     void init();
     void reset();
-    void readAccelData(int16_t* dest);
-    void readGyroData(int16_t* dest);
+    void getAccelRaw(int16_t* dest);
+    void getGyroRaw(int16_t* dest);
+    void getGyro(float* dest);
     int16_t readTempData();
     void calibrate(float* dest1, float* dest2);
-    void complementaryFilter(float* pitch, float* roll);
+    //void complementaryFilter(float* pitch, float* roll);
+
+    protected:
+    I2C i2c;
+    char currentAcceleroRange;
+    char currentGyroRange;    
 };
 
 #endif
\ No newline at end of file

diff -r 5b90f2b5e6d9 -r 4e799f8ec792 MPU6050RegDef.h
--- a/MPU6050RegDef.h	Wed Aug 05 13:15:07 2015 +0000
+++ b/MPU6050RegDef.h	Mon May 30 08:11:07 2016 +0000
@@ -28,6 +28,7 @@
 // See also MPU-6050 Register Map and Descriptions, Revision 4.0, RM-MPU-6050A-00, 9/12/2012 for registers not listed in 
 // above document; the MPU6050 and MPU 9150 are virtually identical but the latter has an on-board magnetic sensor
 //
+
 #define XGOFFS_TC        0x00 // Bit 7 PWR_MODE, bits 6:1 XG_OFFS_TC, bit 0 OTP_BNK_VLD                 
 #define YGOFFS_TC        0x01                                                                          
 #define ZGOFFS_TC        0x02