File content as of revision 2:c142d5a352c5:

#include "Lib_MPU9250.h"

// constructor (use SPI)
MPU9250::MPU9250( PinName cs, PinName mosi, PinName miso, PinName sck ){    
    // initialize instance
    _cs         =   new DigitalOut( cs );
    _spi        =   new SPI( mosi, miso, sck );
    _i2c        =   0;
    _i2c_addr   =   0x0;
    
    // spi setting
    (*_cs) = 1;
    _spi->format( 8, 0 );
    _spi->frequency( 20000000 );
    
    _accscale   =   MPU9250A_2G;
    _gyroscale  =   MPU9250G_250DPS;
    
    _imu_if     =   MPU9250_SPI;
    
    return;
}

// constructor ( use I2C )
MPU9250::MPU9250( PinName sda, PinName scl, uint8_t addr ){
    // initialize instance
    _cs         =   0;
    _spi        =   0;
    _i2c        =   new I2C( sda, scl );
    _i2c_addr   =   addr;
    
    // I2C setting
    _i2c->frequency( 100000 );
    
    _accscale   =   MPU9250A_2G;
    _gyroscale  =   MPU9250G_250DPS;
    
    _imu_if     =   MPU9250_I2C;
    
    return;
}


MPU9250::~MPU9250(){
    return;
}

// スタート
void MPU9250::begin(){
    if( _imu_if == MPU9250_SPI ){
        // initialize state
        _writeRegister( PWR_MGMT_1,    0x01  );
        _writeRegister( PWR_MGMT_2,    0x00  );
        _writeRegister( USER_CTRL,     0x30  );
        _writeRegister( I2C_MST_CTRL,  0x0D  );      // I2C clock speed : 400kHz
        
        // reset AK8963
        _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
        _writeRegister( I2C_SLV0_REG,  CNTL2 );
        _writeRegister( I2C_SLV0_DO,   0x01  );
        _writeRegister( I2C_SLV0_CTRL, 0x81  );
    
        // AK8963 mode : 100Hz sampling mode
        _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
        _writeRegister( I2C_SLV0_REG,  CNTL1 );
        _writeRegister( I2C_SLV0_DO,   0x16  );
        _writeRegister( I2C_SLV0_CTRL, 0x81  );
    
    }else if( _imu_if == MPU9250_I2C ){
        // initialize state
        _writeRegister( PWR_MGMT_1,    0x01  );
        _writeRegister( PWR_MGMT_2,    0x00  );
        _writeRegister( USER_CTRL,     0x00  );
        _writeRegister( I2C_MST_CTRL,  0x0D  );      // I2C clock speed : 400kHz
        // reset AK8963
        _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
        _writeRegister( I2C_SLV0_REG,  CNTL2 );
        _writeRegister( I2C_SLV0_DO,   0x01  );
        _writeRegister( I2C_SLV0_CTRL, 0x81  );
    
        // AK8963 mode : 100Hz sampling mode
        _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
        _writeRegister( I2C_SLV0_REG,  CNTL1 );
        _writeRegister( I2C_SLV0_DO,   0x16  );
        _writeRegister( I2C_SLV0_CTRL, 0x81  );

    }
    return;
}

// リセット
void MPU9250::reset(){
    _writeRegister( PWR_MGMT_1, BIT_H_RESET );
    return;
}

// WhoAmI
uint8_t MPU9250::getWhoAmI(){
    uint8_t ret = 0x00;
    ret = _readRegister( WHO_AM_I );
    return ret;    
}

// 加速度センサのレンジ設定
void MPU9250::setAccelRange( MPU9250BIT range ){
    switch( range ){
        case BITS_FS_2G:
            _accscale = MPU9250A_2G;
            break;
        case BITS_FS_4G:
            _accscale = MPU9250A_4G;
            break;
        case BITS_FS_8G:
            _accscale = MPU9250A_8G;
            break;
        case BITS_FS_16G:
            _accscale = MPU9250A_16G;
            break;
        default:
            return;
    }
    _writeRegister( ACCEL_CONFIG, range );
    return;
}

// ジャイロセンサのレンジ設定
void MPU9250::setGyroRange( MPU9250BIT range ){
    switch( range ){
        case BITS_FS_250DPS:
            _gyroscale      = MPU9250G_250DPS;
//          _gyroscalerad   = MPU9250G_250DPS_RAD;
            break;
        case BITS_FS_500DPS:
            _gyroscale      = MPU9250G_500DPS;
//          _gyroscalerad   = MPU9250G_500DPS_RAD;
            break;
        case BITS_FS_1000DPS:
            _gyroscale      = MPU9250G_1000DPS;
//          _gyroscalerad   = MPU9250G_1000DPS_RAD;
            break;
        case BITS_FS_2000DPS:
            _gyroscale      = MPU9250G_2000DPS;
//          _gyroscalerad   = MPU9250G_2000DPS_RAD;
            break;
        default:
            return;
    }
    _writeRegister( GYRO_CONFIG, range );
    return;
}

// ローパスフィルタ設定
void MPU9250::setDLPF( MPU9250BIT range ){
    return;
}

// 6軸分の生データを取得
void MPU9250::read6AxisRaw( int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz ){
    uint8_t buf[14];
    _readBuffer( ACCEL_XOUT_H, 14, buf );
    *ax = (int16_t)( ( buf[0]  << 8 ) | buf[1]  );
    *ay = (int16_t)( ( buf[2]  << 8 ) | buf[3]  );
    *az = (int16_t)( ( buf[4]  << 8 ) | buf[5]  );
    *gx = (int16_t)( ( buf[8]  << 8 ) | buf[9]  );
    *gy = (int16_t)( ( buf[10] << 8 ) | buf[11] );
    *gz = (int16_t)( ( buf[12] << 8 ) | buf[13] );
    return;
}

// 9軸分の生データを取得
void MPU9250::read9AxisRaw( int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz ){
    uint8_t buf[21];
    _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR | 0x80 );     // Set the I2C slave addres of AK8963 and set for read.
    _writeRegister( I2C_SLV0_REG,  HXL  );                   // I2C slave 0 register address from where to begin data transfer
    _writeRegister( I2C_SLV0_CTRL, 0x87 );                   // Read 7 bytes from the magnetomete    
    _readBuffer( ACCEL_XOUT_H, 21, buf );
    *ax = (int16_t)( ( buf[0]  << 8 ) | buf[1]  );
    *ay = (int16_t)( ( buf[2]  << 8 ) | buf[3]  );
    *az = (int16_t)( ( buf[4]  << 8 ) | buf[5]  );
    *gx = (int16_t)( ( buf[8]  << 8 ) | buf[9]  );
    *gy = (int16_t)( ( buf[10] << 8 ) | buf[11] );
    *gz = (int16_t)( ( buf[12] << 8 ) | buf[13] );
    *mx = (int16_t)( ( buf[15] << 8 ) | buf[14] );
    *my = (int16_t)( ( buf[17] << 8 ) | buf[16] );
    *mz = (int16_t)( ( buf[19] << 8 ) | buf[18] );
    return;
}

// 加速度生データの取得
void MPU9250::readAccelRaw( int16_t* ax, int16_t* ay, int16_t* az ){
    uint8_t buf[6];
    _readBuffer( ACCEL_XOUT_H, 6, buf );
    *ax = (int16_t)( ( buf[0] << 8 ) | buf[1] );
    *ay = (int16_t)( ( buf[2] << 8 ) | buf[3] );
    *az = (int16_t)( ( buf[4] << 8 ) | buf[5] );
    return;
}

// ジャイロ生データの取得
void MPU9250::readGyroRaw( int16_t* gx, int16_t* gy, int16_t* gz ){
    uint8_t buf[6]; 
    _readBuffer( GYRO_XOUT_H, 6, buf );
    *gx = (int16_t)( ( buf[0] << 8 ) | buf[1] );
    *gy = (int16_t)( ( buf[2] << 8 ) | buf[3] );
    *gz = (int16_t)( ( buf[4] << 8 ) | buf[5] );
    return;
}

// 地磁気生データの取得
void MPU9250::readMagRaw( int16_t* mx, int16_t* my, int16_t* mz ){
    uint8_t buf[7];
    _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR | 0x80 );     // Set the I2C slave addres of AK8963 and set for read.
    _writeRegister( I2C_SLV0_REG,  HXL  );                   // I2C slave 0 register address from where to begin data transfer
    _writeRegister( I2C_SLV0_CTRL, 0x87 );                   // Read 7 bytes from the magnetomete
    _readBuffer( EXT_SENS_DATA_00, 7, buf );
    *mx = (int16_t)( ( buf[1] << 8 ) | buf[0] );
    *my = (int16_t)( ( buf[3] << 8 ) | buf[2] );
    *mz = (int16_t)( ( buf[5] << 8 ) | buf[4] );
    return;
}

// 温度生データの取得
int16_t MPU9250::readTempRaw(){
    uint8_t buf[2];
    _readBuffer( TEMP_OUT_H, 2, buf );
    return (int16_t)( ( buf[0] << 8 ) | buf[1] );
}

// 6軸データの取得
void MPU9250::read6Axis( float* ax, float* ay, float* az, float* gx, float* gy, float* gz ){
    int16_t a_x, a_y, a_z, g_x, g_y, g_z;
    read6AxisRaw( &a_x, &a_y, &a_z, &g_x, &g_y, &g_z );
    *ax =   (float)a_x * _accscale;
    *ay =   (float)a_y * _accscale;
    *az =   (float)a_z * _accscale;
    *gx =   (float)g_x * _gyroscale;
    *gy =   (float)g_y * _gyroscale;
    *gz =   (float)g_z * _gyroscale;    
    return;
}

// 9軸データの取得
void MPU9250::read9Axis( float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* mx, float* my, float* mz ){
    int16_t a_x, a_y, a_z, g_x, g_y, g_z, m_x, m_y, m_z;
    read9AxisRaw( &a_x, &a_y, &a_z, &g_x, &g_y, &g_z, &m_x, &m_y, &m_z );
    *ax =   (float)a_x * _accscale;
    *ay =   (float)a_y * _accscale;
    *az =   (float)a_z * _accscale;
    *gx =   (float)g_x * _gyroscale;
    *gy =   (float)g_y * _gyroscale;
    *gz =   (float)g_z * _gyroscale;
    *mx =   (float)m_x * MPU9250M_4800uT;
    *my =   (float)m_y * MPU9250M_4800uT;
    *mz =   (float)m_z * MPU9250M_4800uT;
    return;    
}

// 加速度データの取得
void MPU9250::readAccel( float* ax, float* ay, float* az ){
    int16_t x, y ,z;
    readAccelRaw( &x, &y, &z );
    *ax = (float)x * _accscale;
    *ay = (float)y * _accscale;
    *az = (float)z * _accscale;
    return;
}

// ジャイロデータの取得
void MPU9250::readGyro( float* gx, float* gy, float* gz ){
    int16_t x, y ,z;
    readGyroRaw( &x, &y, &z );
    *gx = (float)x * _gyroscale;
    *gy = (float)y * _gyroscale;
    *gz = (float)z * _gyroscale;
    return;
}

// 地磁気データの取得
void MPU9250::readMag( float* mx, float* my, float* mz ){
    int16_t x, y ,z;
    readMagRaw( &x, &y, &z );
    *mx = (float)x * MPU9250M_4800uT;
    *my = (float)y * MPU9250M_4800uT;
    *mz = (float)z * MPU9250M_4800uT;
    return;
}

// get temperature
float MPU9250::readTemp(){
    int16_t tmp;
    tmp = readTempRaw();
    return (float)tmp * MPU9250T_85degC;
}

// read register
uint8_t MPU9250::_readRegister( MPU9250REG addr ){
    uint8_t ret;
    if( _imu_if == MPU9250_SPI ){
        (*_cs) =   0;
        ret = _spi->write( addr | 0x80 );        // send address
        ret = _spi->write( 0x00 );
        (*_cs) =   1;
    }else{
        _i2c->write( _i2c_addr * 2, (char*)(&addr), 1, 1 );
        _i2c->read(  _i2c_addr * 2, (char*)(&ret),  1 );
    }
    return ret;
}

// write register
uint8_t MPU9250::_writeRegister( MPU9250REG addr, uint8_t data ){
    if( _imu_if == MPU9250_SPI ){
        (*_cs) =   0;
        _spi->write( addr );
        _spi->write( data );    
        (*_cs) =   1;
    }else{
        uint8_t buf[2] = { addr, data };
        _i2c->write( _i2c_addr * 2, (char*)buf, 2 );
    }
    return 0;
}

// レジスタの読み込み(バッファ)
uint8_t MPU9250::_readBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
    if( _imu_if == MPU9250_SPI ){    
        (*_cs) =   0;
        _spi->write( addr | 0x80 );                  // send address
        while( len-- ){
            *(buf++) = _spi->write( 0x00 );          // read data
        }
        (*_cs) =   1;
    }else{
        _i2c->write( _i2c_addr * 2, (char*)(&addr),   1 );
        _i2c->read(  _i2c_addr * 2, (char*)buf,     len );
    }
    return 0;
}

// レジスタの書き込み(バッファ)
uint8_t MPU9250::_writeBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
    if( _imu_if == MPU9250_SPI ){
        (*_cs) =   0;
        _spi->write( addr );                         // send address
        while( len-- ){
            _spi->write( *(buf++) );                 // send data
        }
        (*_cs) =   1;
    }else{
        _i2c->write( _i2c_addr * 2, (char*)(&addr),   1 );
        _i2c->write( _i2c_addr * 2, (char*)buf,     len );
    }
    return 0;
}