Diff: MPU9250/MPU9250.cpp

Revision:

10:237258f2d05e

Parent:

9:350a1d643ef1

--- a/MPU9250/MPU9250.cpp	Fri Jul 20 20:40:10 2018 +0000
+++ b/MPU9250/MPU9250.cpp	Sun Aug 26 17:17:46 2018 +0000
@@ -36,7 +36,7 @@
 void MPU9250::setup_i2c()
 {
     // Setup I2C bus frequency to 100kHz
-    i2c.frequency(400000);
+    i2c.frequency(100000);
 }
 
 /** Test I2C bus */
@@ -109,6 +109,7 @@
 /** Read accelerometer output data */
 void MPU9250::read_accel()
 {
+    /*
     // Read raw data (two 8 bit data for each axis)
     uint8_t ax_raw_h = read_reg(ACCEL_XOUT_H);
     uint8_t ax_raw_l = read_reg(ACCEL_XOUT_L);
@@ -126,11 +127,34 @@
     ax = ay_raw * a_res;
     ay = -ax_raw * a_res;
     az = -az_raw * a_res;
+    */
+    
+    // MPU9250 I2C bus address
+    char address = MPU9250_ADDRESS;
+    // Register address
+    char reg[1] = {ACCEL_XOUT_H};
+    // Data that we're going to read
+    char data[6];
+
+    // Point to register address
+    i2c.write(address, reg, 1);
+    // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
+    i2c.read(address, data, 6);
+
+    // Reassemble the data (two 8 bit data into one 16 bit data)
+    int16_t ax_raw = (data[0] << 8 ) | data[1];
+    int16_t ay_raw = (data[2] << 8 ) | data[3];
+    int16_t az_raw = (data[4] << 8 ) | data[5];
+    // Convert to SI units [m/s^2]
+    ax = ay_raw * a_res;
+    ay = -ax_raw * a_res;
+    az = -az_raw * a_res;
 }
 
 /** Read accelerometer data */
 void MPU9250::read_gyro()
 {
+    /*
     // Read raw data (two 8 bit data for each axis)
     uint8_t gx_raw_h = read_reg(GYRO_XOUT_H);
     uint8_t gx_raw_l = read_reg(GYRO_XOUT_L);
@@ -148,6 +172,27 @@
     gx = -gy_raw * g_res;
     gy = gx_raw * g_res;
     gz = gz_raw * g_res;
+    */
+    // MPU9250 I2C bus address
+    char address = MPU9250_ADDRESS;
+    // Register address
+    char reg[1] = {GYRO_XOUT_H};
+    // Data that we're going to read
+    char data[6];
+
+    // Point to register address
+    i2c.write(address, reg, 1);
+    // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
+    i2c.read(address, data, 6);
+
+    // Reassemble the data (two 8 bit data into one 16 bit data)
+    int16_t gx_raw = (data[0] << 8 ) | data[1];
+    int16_t gy_raw = (data[2] << 8 ) | data[3];
+    int16_t gz_raw = (data[4] << 8 ) | data[5];
+    // Convert to SI units [rad/s]
+    gx = -gy_raw * g_res;
+    gy = gx_raw * g_res;
+    gz = gz_raw * g_res;
 }
 
 /** Write data into register on MPU9250 I2C bus address */