Diff: MPU9250/MPU9250.cpp

Revision:

2:3d5f4b1c7bdb

Parent:

0:3d8a882699ef

Child:

3:2f2b8e863991

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250/MPU9250.cpp	Tue Apr 17 23:35:21 2018 +0000
@@ -0,0 +1,129 @@
+#include "MPU9250.h"
+
+// 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)
+{
+    setup_accel(a_scale);
+    setup_gyro(g_scale);
+}
+
+// Setup accelerometer
+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};
+    
+    // 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)
+    switch (a_scale)
+    {
+        case ACCEL_SCALE_2G:
+            a.res = (2.0/32768.0)*GRAVITY;
+            break;
+        case ACCEL_SCALE_4G:
+            a.res = (4.0/32768.0)*GRAVITY;
+            break;
+        case ACCEL_SCALE_8G:
+            a.res = (8.0/32768.0)*GRAVITY;
+            break;
+        case ACCEL_SCALE_16G:
+            a.res = (16.0/32768.0)*GRAVITY;
+            break;
+    }
+}
+
+// Setup gyroscope
+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};
+    
+    // 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)
+    switch (g_scale)
+    {
+        case GYRO_SCALE_250DPS:
+            g.res = (250.0/32768.0)*(PI/180);
+            break;
+        case GYRO_SCALE_500DPS:
+            g.res = (500.0/32768.0)*(PI/180);
+            break;
+        case GYRO_SCALE_1000DPS:
+            g.res = (1000.0/32768.0)*(PI/180);
+            break;
+        case GYRO_SCALE_2000DPS:
+            g.res = (2000.0/32768.0)*(PI/180);
+            break;
+    }
+}
+  
+// Read accelerometer, gyroscope and magnetometer output data
+void MPU9250::read()
+{
+    read_accel();
+    read_gyro();
+}
+    
+// Read accelerometer output data
+void MPU9250::read_accel()
+{
+    // MPU9250 I2C 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)
+    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;
+}
+    
+// Read gyroscope output data
+void MPU9250::read_gyro()
+{
+    // MPU9250 I2C 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)
+    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;
+}
\ No newline at end of file