Diff: MPU9250/MPU9250.cpp

Revision:

3:2f2b8e863991

Parent:

2:3d5f4b1c7bdb

Child:

5:1ef8b91a0318

diff -r 3d5f4b1c7bdb -r 2f2b8e863991 MPU9250/MPU9250.cpp
--- a/MPU9250/MPU9250.cpp	Tue Apr 17 23:35:21 2018 +0000
+++ b/MPU9250/MPU9250.cpp	Thu Apr 19 19:41:02 2018 +0000
@@ -1,84 +1,96 @@
 #include "MPU9250.h"
 
-// Class constructor
+/** Class constructor */
 MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl)
 {
-    //
 }
     
-// Setup accelerometer, gyroscope and magnetometer
-void MPU9250::setup(accel_scale a_scale, gyro_scale g_scale)
+/** Initialize accelerometer and gyroscope */
+void MPU9250::init()
 {
-    setup_accel(a_scale);
-    setup_gyro(g_scale);
+    setup_aux_i2c();
+    setup_accel();
+    setup_gyro();
+}
+  
+/** Read accelerometer and gyroscope */
+void MPU9250::read()
+{
+    read_accel();
+    read_gyro();
 }
 
-// Setup accelerometer
+/** Setup auxiliary I2C */
+void MPU9250::setup_aux_i2c()
+{
+    // MPU9250 I2C address
+    char address = MPU9250_ADDRESS;
+    // Register address and data that we're going to write
+    char reg_data[2] = {INT_PIN_CFG, 0b00000010};
+    
+    // Point to register address and write data into this address
+    i2c.write(address, reg_data, 2);
+}
+
+/** Setup accelerometer with given output data rate and full-scale range*/
 void MPU9250::setup_accel(accel_scale a_scale)
 {   
     // MPU9250 I2C address
     char address = MPU9250_ADDRESS;
     // Register address and data that we're going to write
-    char reg_data[2] = {ACCEL_CONFIG_1, 0b000 | a_scale << 3 | 0b000};
+    char reg_data[2] = {ACCEL_CONFIG_1, 0b000 << 5 | a_scale << 3 | 0b000};
     
     // Point to register address and write data into this address
     i2c.write(address, reg_data, 2);
     
-    // Adjust resolution (m/s^2) accordingly to chosed scale (g)
+    // Adjust resolution [m/s^2 / bit] accordingly to chosed scale (g)
     switch (a_scale)
     {
         case ACCEL_SCALE_2G:
-            a.res = (2.0/32768.0)*GRAVITY;
+            a_res = (2.0*GRAVITY)/32768.0;
             break;
         case ACCEL_SCALE_4G:
-            a.res = (4.0/32768.0)*GRAVITY;
+            a_res = (4.0*GRAVITY)/32768.0;
             break;
         case ACCEL_SCALE_8G:
-            a.res = (8.0/32768.0)*GRAVITY;
+            a_res = (8.0*GRAVITY)/32768.0;
             break;
         case ACCEL_SCALE_16G:
-            a.res = (16.0/32768.0)*GRAVITY;
+            a_res = (16.0*GRAVITY)/32768.0;
             break;
     }
 }
 
-// Setup gyroscope
+/** Setup gyroscope with given output data rate and full-scale range*/
 void MPU9250::setup_gyro(gyro_scale g_scale)
 {   
     // MPU9250 I2C address
     char address = MPU9250_ADDRESS;
     // Register address and data that we're going to write
-    char reg_data[2] = {GYRO_CONFIG, 0b000 | g_scale << 3 | 0b000};
+    char reg_data[2] = {GYRO_CONFIG, 0b000 << 5 | g_scale << 3 | 0b000};
     
     // Point to register address and write data into this address
     i2c.write(address, reg_data, 2);
     
-    // Adjust resolution (rad/s) accordingly to chosed scale (º/s)
+    // Adjust resolution [rad/s / bit] accordingly to chosed scale
     switch (g_scale)
     {
         case GYRO_SCALE_250DPS:
-            g.res = (250.0/32768.0)*(PI/180);
+            g_res = (250.0*(PI/180.0))/32768.0;
             break;
         case GYRO_SCALE_500DPS:
-            g.res = (500.0/32768.0)*(PI/180);
+            g_res = (500.0*(PI/180.0))/32768.0;
             break;
         case GYRO_SCALE_1000DPS:
-            g.res = (1000.0/32768.0)*(PI/180);
+            g_res = (1000.0*(PI/180))/32768.0;
             break;
         case GYRO_SCALE_2000DPS:
-            g.res = (2000.0/32768.0)*(PI/180);
+            g_res = (2000.0*(PI/180.0))/32768.0;
             break;
     }
 }
-  
-// Read accelerometer, gyroscope and magnetometer output data
-void MPU9250::read()
-{
-    read_accel();
-    read_gyro();
-}
     
-// Read accelerometer output data
+/** Read accelerometer output data */
 void MPU9250::read_accel()
 {
     // MPU9250 I2C address
@@ -94,16 +106,16 @@
     i2c.read(address, data, 6);
 
     // Reassemble the data (two 8 bit data into one 16 bit data)
-    a.x_raw = (data[0] << 8 ) | data[1];
-    a.y_raw = (data[2] << 8 ) | data[3];
-    a.z_raw = (data[4] << 8 ) | data[5];
-    // Convert to SI units
-    a.x = a.x_raw * a.res;
-    a.y = a.y_raw * a.res;
-    a.z = a.z_raw * a.res;
+    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 = ax_raw * a_res;
+    ay = ay_raw * a_res;
+    az = az_raw * a_res;
 }
     
-// Read gyroscope output data
+/** Read accelerometer output data */
 void MPU9250::read_gyro()
 {
     // MPU9250 I2C address
@@ -119,11 +131,11 @@
     i2c.read(address, data, 6);
 
     // Reassemble the data (two 8 bit data into one 16 bit data)
-    g.x_raw = (data[0] << 8 ) | data[1];
-    g.y_raw = (data[2] << 8 ) | data[3];
-    g.z_raw = (data[4] << 8 ) | data[5];
-    // Convert to SI units
-    g.x = g.x_raw * g.res;
-    g.y = g.y_raw * g.res;
-    g.z = g.z_raw * g.res;
+    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 = gx_raw * g_res;
+    gy = gy_raw * g_res;
+    gz = gz_raw * g_res;
 }
\ No newline at end of file