Diff: MPU6050.cpp

Revision:

2:c68be621f2f1

Parent:

1:cf3a9ec7205e

Child:

3:48773a5c8d48

diff -r cf3a9ec7205e -r c68be621f2f1 MPU6050.cpp
--- a/MPU6050.cpp	Thu Jan 05 05:04:56 2017 +0000
+++ b/MPU6050.cpp	Sun May 14 11:28:03 2017 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "MPU6050.h"
 
-#define ADDRESS 0xD1
+#define ADDRESS 0xD1 //68 shifted left: 7-bit to 8-bit address conversion
 
 MPU6050::MPU6050(PinName sda, PinName scl):
     _MPU6050(sda, scl)
@@ -12,9 +12,10 @@
 void MPU6050::start(void)
 {
     write_reg(ADDRESS,MPU6050_PWR_MGMT_1,0x00);
-    write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x10);//gyro +-1000deg/s
-    write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x08);//accel +-4G
-    write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x10);//sample rate 500Hz
+    write_reg(ADDRESS,MPU6050_GYRO_CONFIG,0x08);//gyro +-500deg/s //0x10);//gyro +-1000deg/s
+    write_reg(ADDRESS,MPU6050_ACCEL_CONFIG,0x00);//accel +-2G //0x08);//accel +-4G
+    write_reg(ADDRESS,MPU6050_CONFIG,0x05);//bandwidth 10Hz, delay 14 mS
+    write_reg(ADDRESS,MPU6050_SMPLRT_DIV,0x19);//sample rate 40Hz (1000/25)
 }
 
 char MPU6050::getID(void)
@@ -30,12 +31,12 @@
     char data2[6];
     if (read_data(ADDRESS, MPU6050_GYRO_XOUT_H, data, 6)) {
         read_data(ADDRESS, MPU6050_ACCEL_XOUT_H, data2, 6);
-        *gx = float(short(data[0] << 8 | data[1]))*0.0305f;
-        *gy =  float(short(data[2] << 8 | data[3]))*0.0305f;
-        *gz =  float(short(data[4] << 8 | data[5]))*0.0305f;
-        *ax = float(short(data2[0] << 8 | data2[1]))*0.000122f;
-        *ay =  float(short(data2[2] << 8 | data2[3]))*0.000122f;
-        *az =  float(short(data2[4] << 8 | data2[5]))*0.000122f;
+        *gx = float(short(data[0] << 8 | data[1]))*1.526e-2; //deg/s
+        *gy =  float(short(data[2] << 8 | data[3]))*1.526e-2;
+        *gz =  float(short(data[4] << 8 | data[5]))*1.526e-2;
+        *ax = float(short(data2[0] << 8 | data2[1]))*5.9875e-4; //cm^2/s
+        *ay =  float(short(data2[2] << 8 | data2[3]))*5.9875e-4;
+        *az =  float(short(data2[4] << 8 | data2[5]))*5.9875e-4;
         return true;
     }
     return false;
@@ -68,12 +69,12 @@
 {
     char data = addr_reg;
     bool result = false;
-    __disable_irq();
+//    __disable_irq();
     if ((_MPU6050.write(addr_i2c, &data, 1) == 0) && (_MPU6050.read(addr_i2c, &data, 1) == 0)) {
         *v = data;
         result = true;
     }
-    __enable_irq();
+//    __enable_irq();
     return result;
 }