File content as of revision 3:2f2b8e863991:

#include "MPU9250.h"

/** Class constructor */
MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl)
{
}
    
/** Initialize accelerometer and gyroscope */
void MPU9250::init()
{
    setup_aux_i2c();
    setup_accel();
    setup_gyro();
}
  
/** Read accelerometer and gyroscope */
void MPU9250::read()
{
    read_accel();
    read_gyro();
}

/** 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 << 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 / bit] accordingly to chosed scale (g)
    switch (a_scale)
    {
        case ACCEL_SCALE_2G:
            a_res = (2.0*GRAVITY)/32768.0;
            break;
        case ACCEL_SCALE_4G:
            a_res = (4.0*GRAVITY)/32768.0;
            break;
        case ACCEL_SCALE_8G:
            a_res = (8.0*GRAVITY)/32768.0;
            break;
        case ACCEL_SCALE_16G:
            a_res = (16.0*GRAVITY)/32768.0;
            break;
    }
}

/** 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 << 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 / bit] accordingly to chosed scale
    switch (g_scale)
    {
        case GYRO_SCALE_250DPS:
            g_res = (250.0*(PI/180.0))/32768.0;
            break;
        case GYRO_SCALE_500DPS:
            g_res = (500.0*(PI/180.0))/32768.0;
            break;
        case GYRO_SCALE_1000DPS:
            g_res = (1000.0*(PI/180))/32768.0;
            break;
        case GYRO_SCALE_2000DPS:
            g_res = (2000.0*(PI/180.0))/32768.0;
            break;
    }
}
    
/** 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)
    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 accelerometer 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)
    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;
}