Diff: MPU6050.cpp

Revision:

7:f01d2b922a5f

Parent:

6:5b90f2b5e6d9

--- a/MPU6050.cpp	Wed Aug 05 13:15:07 2015 +0000
+++ b/MPU6050.cpp	Mon Apr 16 07:13:44 2018 +0000
@@ -34,7 +34,7 @@
 //I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
 
 /* For NUCLEO-F411RE board */
-static I2C i2c(D14,D15);         // setup i2c (SDA,SCL)
+static I2C i2c(p9,p10);         // setup i2c (SDA,SCL)
 
 /* Set initial input parameters */
 
@@ -42,18 +42,18 @@
 enum Ascale
 {
     AFS_2G=0,  
-    AFS_4G,
-    AFS_8G,
-    AFS_16G
+    AFS_4G=1,
+    AFS_8G=2,
+    AFS_16G=3
 };
 
 // Gyro Full Scale Range +-250 500 1000 2000 Degrees per second
 enum Gscale
 {
     GFS_250DPS=0,   
-    GFS_500DPS,
-    GFS_1000DPS,
-    GFS_2000DPS
+    GFS_500DPS=1,
+    GFS_1000DPS=2,
+    GFS_2000DPS=3
 };
 
 // Sensor datas
@@ -142,17 +142,15 @@
 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");  
     }
     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");  
     }  
 }
 
@@ -166,7 +164,7 @@
   
     /* Wake up the device */
     writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00);  // wake up the device by clearing the sleep bit (bit6) 
-    wait_ms(100); // wait 100 ms to stabilize  
+    wait_ms(500); // wait 100 ms to stabilize  
     
     /* Get stable time source */
     // PLL with X axis gyroscope reference is used to improve stability
@@ -179,7 +177,8 @@
     
     /* Set sample rate = gyroscope output rate/(1+SMPLRT_DIV) */
     // SMPLRT_DIV=4 and sample rate=200 Hz (compatible with config above)
-    writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04);
+//    writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04);
+    writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x01);   // 500 Hz ///
     
     /* Accelerometer configuration */
     uint8_t temp = readByte(MPU6050_ADDRESS, ACCEL_CONFIG);
@@ -192,6 +191,10 @@
     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0xE0);      // Clear self-test bits [7:5]
     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp & ~0x18);      // Clear FS bits [4:3]
     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, temp | Gscale<<3);  // Set full scale range 
+
+    ////YS Add////
+    writeByte(MPU6050_ADDRESS, 0x37, 2);
+    writeByte(MPU6050_ADDRESS, 0x6B, 0);
 }
 
 // Resets the device
@@ -268,11 +271,22 @@
     wait(0.015);   
     
     /* Configure accel and gyro for bias calculation */
-    writeByte(MPU6050_ADDRESS, CONFIG, 0x01);       // Set low-pass filter to 188 Hz
+    writeByte(MPU6050_ADDRESS, CONFIG, 0x03);       // Set low-pass filter to 10 Hz
     writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00);   // Set sample rate to 1 kHz
     writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
     writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
-     
+/*
+ *          |   ACCELEROMETER    |           GYROSCOPE
+ * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
+ * ---------+-----------+--------+-----------+--------+-------------
+ * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
+ * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
+ * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
+ * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
+ * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
+ * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
+ * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
+ */     
     /* Configure FIFO to capture accelerometer and gyro data for bias calculation */
     writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
     writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78);     // Enable accelerometer and gyro for FIFO  (max size 1024 bytes in MPU-6050)
@@ -284,7 +298,6 @@
     readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]);  // Read FIFO sample count
     fifo_count = ((uint16_t)data[0] << 8) | data[1];
     packet_count = fifo_count/12;                          // The number of sets of full acc and gyro data for averaging. packet_count = 80 in this case
-    
     for(count=0; count<packet_count; count++)
     {
         int16_t accel_temp[3]={0,0,0};