Diff: MPU9250/MPU9250.cpp

Revision:

8:930aa9d0f5ae

Child:

9:350a1d643ef1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250/MPU9250.cpp	Mon May 14 20:18:20 2018 +0000
@@ -0,0 +1,178 @@
+#include "MPU9250.h"
+
+/** Class constructor */
+MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl)
+{
+}
+
+/** Try to initialize sensor (return true if succeed and false if failed) */
+bool MPU9250::init()
+{
+    // Setup I2C bus
+    setup_i2c();
+
+    // Test I2C bus
+    if (test_i2c()) {
+        // Setup accelerometer and gyroscope configurations
+        setup_accel();
+        setup_gyro();
+        // Setup auxiliary I2C bus pins
+        setup_aux_i2c();
+        return true;
+    } else {
+        return false;
+    }
+}
+
+/** Read sensor data */
+void MPU9250::read()
+{
+    // Read accelerometer and gyroscope data
+    read_accel();
+    read_gyro();
+}
+
+/** Setup I2C bus */
+void MPU9250::setup_i2c()
+{
+    // Setup I2C bus frequency to 100kHz
+    i2c.frequency(400000);
+}
+
+/** Test I2C bus */
+bool MPU9250::test_i2c()
+{
+    // Read device identity
+    uint8_t device_id = read_reg(WHO_AM_I);
+
+    // Check if device identity is 0x71
+    if (device_id == 0x71) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+/** Setup auxiliary I2C bus pins */
+void MPU9250::setup_aux_i2c()
+{
+    // Write 
+    write_reg(INT_PIN_CFG, 0b00000010);
+}
+
+/** Setup accelerometer configurations (full-scale range) */
+void MPU9250::setup_accel(accel_scale a_scale)
+{
+    // Write configuration data
+    write_reg(ACCEL_CONFIG_1, (0b000 << 5) | (a_scale << 3) | 0b000);
+
+    // Adjust resolution [m/s^2 / bit] accordingly to choose scale (g)
+    switch (a_scale) {
+        case ACCEL_SCALE_2G:
+            a_res = (2.0f*GRAVITY)/32768.0f;
+            break;
+        case ACCEL_SCALE_4G:
+            a_res = (4.0f*GRAVITY)/32768.0f;
+            break;
+        case ACCEL_SCALE_8G:
+            a_res = (8.0f*GRAVITY)/32768.0f;
+            break;
+        case ACCEL_SCALE_16G:
+            a_res = (16.0f*GRAVITY)/32768.0f;
+            break;
+    }
+}
+
+/** Setup gyroscope configurations (full-scale range) */
+void MPU9250::setup_gyro(gyro_scale g_scale)
+{
+    // Write configuration data
+    write_reg(GYRO_CONFIG, (0b000 << 5) | (g_scale << 3) | 0b000);
+
+    // Adjust resolution [rad/s / bit] accordingly to choose scale
+    switch (g_scale) {
+        case GYRO_SCALE_250DPS:
+            g_res = (250.0f*(PI/180.0f))/32768.0f;
+            break;
+        case GYRO_SCALE_500DPS:
+            g_res = (500.0f*(PI/180.0f))/32768.0f;
+            break;
+        case GYRO_SCALE_1000DPS:
+            g_res = (1000.0f*(PI/180.0f))/32768.0f;
+            break;
+        case GYRO_SCALE_2000DPS:
+            g_res = (2000.0f*(PI/180.0f))/32768.0f;
+            break;
+    }
+}
+
+/** 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);
+    uint8_t ay_raw_h = read_reg(ACCEL_YOUT_H);
+    uint8_t ay_raw_l = read_reg(ACCEL_YOUT_L);
+    uint8_t az_raw_h = read_reg(ACCEL_ZOUT_H);
+    uint8_t az_raw_l = read_reg(ACCEL_ZOUT_L);
+
+    // Reassemble raw data (one 16 bit data for each axis)
+    int16_t ax_raw = (ax_raw_h << 8) | ax_raw_l;
+    int16_t ay_raw = (ay_raw_h << 8) | ay_raw_l;
+    int16_t az_raw = (az_raw_h << 8) | az_raw_l;
+
+    // Convert raw data to SI units [m/s^2]
+    ax = ax_raw * a_res;
+    ay = ay_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);
+    uint8_t gy_raw_h = read_reg(GYRO_YOUT_H);
+    uint8_t gy_raw_l = read_reg(GYRO_YOUT_L);
+    uint8_t gz_raw_h = read_reg(GYRO_ZOUT_H);
+    uint8_t gz_raw_l = read_reg(GYRO_ZOUT_L);
+
+    // Reassemble raw data (one 16 bit data for each axis)
+    int16_t gx_raw = (gx_raw_h << 8 ) | gx_raw_l;
+    int16_t gy_raw = (gy_raw_h << 8 ) | gy_raw_l;
+    int16_t gz_raw = (gz_raw_h << 8 ) | gz_raw_l;
+
+    // Convert raw data to SI units [rad/s]
+    gx = gx_raw * g_res;
+    gy = gy_raw * g_res;
+    gz = gz_raw * g_res;
+}
+
+/** Write data into register on MPU9250 I2C bus address */
+void MPU9250::write_reg(uint8_t reg, uint8_t data)
+{
+    // Register address and data that will be writed
+    char i2c_reg_data[2] = {reg, data};
+
+    // Point to register address and write data
+    i2c.write(MPU9250_ADDRESS, i2c_reg_data, 2);
+}
+
+/** Read data from register on MPU9250 I2C bus address */
+char MPU9250::read_reg(uint8_t reg)
+{
+    // Register address
+    char i2c_reg[1] = {reg};
+    // Data that will be readed
+    char i2c_data[1];
+
+    // Point to register address
+    i2c.write(MPU9250_ADDRESS, i2c_reg, 1);
+    // Read data
+    i2c.read(MPU9250_ADDRESS, i2c_data, 1);
+
+    // Return readed data
+    return i2c_data[0];
+}
\ No newline at end of file