File content as of revision 2:3d5f4b1c7bdb:

#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;
}