Lib_MPU9250.cpp

00001 #include "Lib_MPU9250.h"
00002 
00003 // constructor (use SPI)
00004 MPU9250::MPU9250( PinName cs, PinName mosi, PinName miso, PinName sck ){    
00005     // initialize instance
00006     _cs         =   new DigitalOut( cs );
00007     _spi        =   new SPI( mosi, miso, sck );
00008     _i2c        =   0;
00009     _i2c_addr   =   0x0;
00010     
00011     // spi setting
00012     (*_cs) = 1;
00013     _spi->format( 8, 0 );
00014     _spi->frequency( 20000000 );
00015     
00016     _accscale   =   MPU9250A_2G;
00017     _gyroscale  =   MPU9250G_250DPS;
00018     
00019     _imu_if     =   MPU9250_SPI;
00020     
00021     return;
00022 }
00023 
00024 // constructor ( use I2C )
00025 MPU9250::MPU9250( PinName sda, PinName scl, uint8_t addr ){
00026     // initialize instance
00027     _cs         =   0;
00028     _spi        =   0;
00029     _i2c        =   new I2C( sda, scl );
00030     _i2c_addr   =   addr;
00031     
00032     // I2C setting
00033     _i2c->frequency( 100000 );
00034     
00035     _accscale   =   MPU9250A_2G;
00036     _gyroscale  =   MPU9250G_250DPS;
00037     
00038     _imu_if     =   MPU9250_I2C;
00039     
00040     return;
00041 }
00042 
00043 
00044 MPU9250::~MPU9250(){
00045     return;
00046 }
00047 
00048 // スタート
00049 void MPU9250::begin(){
00050     if( _imu_if == MPU9250_SPI ){
00051         // initialize state
00052         _writeRegister( PWR_MGMT_1,    0x01  );
00053         _writeRegister( PWR_MGMT_2,    0x00  );
00054         _writeRegister( USER_CTRL,     0x30  );
00055         _writeRegister( I2C_MST_CTRL,  0x0D  );      // I2C clock speed : 400kHz
00056         
00057         // reset AK8963
00058         _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
00059         _writeRegister( I2C_SLV0_REG,  CNTL2 );
00060         _writeRegister( I2C_SLV0_DO,   0x01  );
00061         _writeRegister( I2C_SLV0_CTRL, 0x81  );
00062     
00063         // AK8963 mode : 100Hz sampling mode
00064         _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
00065         _writeRegister( I2C_SLV0_REG,  CNTL1 );
00066         _writeRegister( I2C_SLV0_DO,   0x16  );
00067         _writeRegister( I2C_SLV0_CTRL, 0x81  );
00068     
00069     }else if( _imu_if == MPU9250_I2C ){
00070         // initialize state
00071         _writeRegister( PWR_MGMT_1,    0x01  );
00072         _writeRegister( PWR_MGMT_2,    0x00  );
00073         _writeRegister( USER_CTRL,     0x00  );
00074         _writeRegister( I2C_MST_CTRL,  0x0D  );      // I2C clock speed : 400kHz
00075         // reset AK8963
00076         _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
00077         _writeRegister( I2C_SLV0_REG,  CNTL2 );
00078         _writeRegister( I2C_SLV0_DO,   0x01  );
00079         _writeRegister( I2C_SLV0_CTRL, 0x81  );
00080     
00081         // AK8963 mode : 100Hz sampling mode
00082         _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
00083         _writeRegister( I2C_SLV0_REG,  CNTL1 );
00084         _writeRegister( I2C_SLV0_DO,   0x16  );
00085         _writeRegister( I2C_SLV0_CTRL, 0x81  );
00086 
00087     }
00088     return;
00089 }
00090 
00091 // リセット
00092 void MPU9250::reset(){
00093     _writeRegister( PWR_MGMT_1, BIT_H_RESET );
00094     return;
00095 }
00096 
00097 // WhoAmI
00098 uint8_t MPU9250::getWhoAmI(){
00099     uint8_t ret = 0x00;
00100     ret = _readRegister( WHO_AM_I );
00101     return ret;    
00102 }
00103 
00104 // 加速度センサのレンジ設定
00105 void MPU9250::setAccelRange( MPU9250BIT range ){
00106     switch( range ){
00107         case BITS_FS_2G:
00108             _accscale = MPU9250A_2G;
00109             break;
00110         case BITS_FS_4G:
00111             _accscale = MPU9250A_4G;
00112             break;
00113         case BITS_FS_8G:
00114             _accscale = MPU9250A_8G;
00115             break;
00116         case BITS_FS_16G:
00117             _accscale = MPU9250A_16G;
00118             break;
00119         default:
00120             return;
00121     }
00122     _writeRegister( ACCEL_CONFIG, range );
00123     return;
00124 }
00125 
00126 // ジャイロセンサのレンジ設定
00127 void MPU9250::setGyroRange( MPU9250BIT range ){
00128     switch( range ){
00129         case BITS_FS_250DPS:
00130             _gyroscale      = MPU9250G_250DPS;
00131 //          _gyroscalerad   = MPU9250G_250DPS_RAD;
00132             break;
00133         case BITS_FS_500DPS:
00134             _gyroscale      = MPU9250G_500DPS;
00135 //          _gyroscalerad   = MPU9250G_500DPS_RAD;
00136             break;
00137         case BITS_FS_1000DPS:
00138             _gyroscale      = MPU9250G_1000DPS;
00139 //          _gyroscalerad   = MPU9250G_1000DPS_RAD;
00140             break;
00141         case BITS_FS_2000DPS:
00142             _gyroscale      = MPU9250G_2000DPS;
00143 //          _gyroscalerad   = MPU9250G_2000DPS_RAD;
00144             break;
00145         default:
00146             return;
00147     }
00148     _writeRegister( GYRO_CONFIG, range );
00149     return;
00150 }
00151 
00152 // ローパスフィルタ設定
00153 void MPU9250::setDLPF( MPU9250BIT range ){
00154     return;
00155 }
00156 
00157 // 6軸分の生データを取得
00158 void MPU9250::read6AxisRaw( int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz ){
00159     uint8_t buf[14];
00160     _readBuffer( ACCEL_XOUT_H, 14, buf );
00161     *ax = (int16_t)( ( buf[0]  << 8 ) | buf[1]  );
00162     *ay = (int16_t)( ( buf[2]  << 8 ) | buf[3]  );
00163     *az = (int16_t)( ( buf[4]  << 8 ) | buf[5]  );
00164     *gx = (int16_t)( ( buf[8]  << 8 ) | buf[9]  );
00165     *gy = (int16_t)( ( buf[10] << 8 ) | buf[11] );
00166     *gz = (int16_t)( ( buf[12] << 8 ) | buf[13] );
00167     return;
00168 }
00169 
00170 // 9軸分の生データを取得
00171 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 ){
00172     uint8_t buf[21];
00173     _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR | 0x80 );     // Set the I2C slave addres of AK8963 and set for read.
00174     _writeRegister( I2C_SLV0_REG,  HXL  );                   // I2C slave 0 register address from where to begin data transfer
00175     _writeRegister( I2C_SLV0_CTRL, 0x87 );                   // Read 7 bytes from the magnetomete    
00176     _readBuffer( ACCEL_XOUT_H, 21, buf );
00177     *ax = (int16_t)( ( buf[0]  << 8 ) | buf[1]  );
00178     *ay = (int16_t)( ( buf[2]  << 8 ) | buf[3]  );
00179     *az = (int16_t)( ( buf[4]  << 8 ) | buf[5]  );
00180     *gx = (int16_t)( ( buf[8]  << 8 ) | buf[9]  );
00181     *gy = (int16_t)( ( buf[10] << 8 ) | buf[11] );
00182     *gz = (int16_t)( ( buf[12] << 8 ) | buf[13] );
00183     *mx = (int16_t)( ( buf[15] << 8 ) | buf[14] );
00184     *my = (int16_t)( ( buf[17] << 8 ) | buf[16] );
00185     *mz = (int16_t)( ( buf[19] << 8 ) | buf[18] );
00186     return;
00187 }
00188 
00189 // 加速度生データの取得
00190 void MPU9250::readAccelRaw( int16_t* ax, int16_t* ay, int16_t* az ){
00191     uint8_t buf[6];
00192     _readBuffer( ACCEL_XOUT_H, 6, buf );
00193     *ax = (int16_t)( ( buf[0] << 8 ) | buf[1] );
00194     *ay = (int16_t)( ( buf[2] << 8 ) | buf[3] );
00195     *az = (int16_t)( ( buf[4] << 8 ) | buf[5] );
00196     return;
00197 }
00198 
00199 // ジャイロ生データの取得
00200 void MPU9250::readGyroRaw( int16_t* gx, int16_t* gy, int16_t* gz ){
00201     uint8_t buf[6]; 
00202     _readBuffer( GYRO_XOUT_H, 6, buf );
00203     *gx = (int16_t)( ( buf[0] << 8 ) | buf[1] );
00204     *gy = (int16_t)( ( buf[2] << 8 ) | buf[3] );
00205     *gz = (int16_t)( ( buf[4] << 8 ) | buf[5] );
00206     return;
00207 }
00208 
00209 // 地磁気生データの取得
00210 void MPU9250::readMagRaw( int16_t* mx, int16_t* my, int16_t* mz ){
00211     uint8_t buf[7];
00212     _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR | 0x80 );     // Set the I2C slave addres of AK8963 and set for read.
00213     _writeRegister( I2C_SLV0_REG,  HXL  );                   // I2C slave 0 register address from where to begin data transfer
00214     _writeRegister( I2C_SLV0_CTRL, 0x87 );                   // Read 7 bytes from the magnetomete
00215     _readBuffer( EXT_SENS_DATA_00, 7, buf );
00216     *mx = (int16_t)( ( buf[1] << 8 ) | buf[0] );
00217     *my = (int16_t)( ( buf[3] << 8 ) | buf[2] );
00218     *mz = (int16_t)( ( buf[5] << 8 ) | buf[4] );
00219     return;
00220 }
00221 
00222 // 温度生データの取得
00223 int16_t MPU9250::readTempRaw(){
00224     uint8_t buf[2];
00225     _readBuffer( TEMP_OUT_H, 2, buf );
00226     return (int16_t)( ( buf[0] << 8 ) | buf[1] );
00227 }
00228 
00229 // 6軸データの取得
00230 void MPU9250::read6Axis( float* ax, float* ay, float* az, float* gx, float* gy, float* gz ){
00231     int16_t a_x, a_y, a_z, g_x, g_y, g_z;
00232     read6AxisRaw( &a_x, &a_y, &a_z, &g_x, &g_y, &g_z );
00233     *ax =   (float)a_x * _accscale;
00234     *ay =   (float)a_y * _accscale;
00235     *az =   (float)a_z * _accscale;
00236     *gx =   (float)g_x * _gyroscale;
00237     *gy =   (float)g_y * _gyroscale;
00238     *gz =   (float)g_z * _gyroscale;    
00239     return;
00240 }
00241 
00242 // 9軸データの取得
00243 void MPU9250::read9Axis( float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* mx, float* my, float* mz ){
00244     int16_t a_x, a_y, a_z, g_x, g_y, g_z, m_x, m_y, m_z;
00245     read9AxisRaw( &a_x, &a_y, &a_z, &g_x, &g_y, &g_z, &m_x, &m_y, &m_z );
00246     *ax =   (float)a_x * _accscale;
00247     *ay =   (float)a_y * _accscale;
00248     *az =   (float)a_z * _accscale;
00249     *gx =   (float)g_x * _gyroscale;
00250     *gy =   (float)g_y * _gyroscale;
00251     *gz =   (float)g_z * _gyroscale;
00252     *mx =   (float)m_x * MPU9250M_4800uT;
00253     *my =   (float)m_y * MPU9250M_4800uT;
00254     *mz =   (float)m_z * MPU9250M_4800uT;
00255     return;    
00256 }
00257 
00258 // 加速度データの取得
00259 void MPU9250::readAccel( float* ax, float* ay, float* az ){
00260     int16_t x, y ,z;
00261     readAccelRaw( &x, &y, &z );
00262     *ax = (float)x * _accscale;
00263     *ay = (float)y * _accscale;
00264     *az = (float)z * _accscale;
00265     return;
00266 }
00267 
00268 // ジャイロデータの取得
00269 void MPU9250::readGyro( float* gx, float* gy, float* gz ){
00270     int16_t x, y ,z;
00271     readGyroRaw( &x, &y, &z );
00272     *gx = (float)x * _gyroscale;
00273     *gy = (float)y * _gyroscale;
00274     *gz = (float)z * _gyroscale;
00275     return;
00276 }
00277 
00278 // 地磁気データの取得
00279 void MPU9250::readMag( float* mx, float* my, float* mz ){
00280     int16_t x, y ,z;
00281     readMagRaw( &x, &y, &z );
00282     *mx = (float)x * MPU9250M_4800uT;
00283     *my = (float)y * MPU9250M_4800uT;
00284     *mz = (float)z * MPU9250M_4800uT;
00285     return;
00286 }
00287 
00288 // get temperature
00289 float MPU9250::readTemp(){
00290     int16_t tmp;
00291     tmp = readTempRaw();
00292     return (float)tmp * MPU9250T_85degC;
00293 }
00294 
00295 // read register
00296 uint8_t MPU9250::_readRegister( MPU9250REG addr ){
00297     uint8_t ret;
00298     if( _imu_if == MPU9250_SPI ){
00299         (*_cs) =   0;
00300         ret = _spi->write( addr | 0x80 );        // send address
00301         ret = _spi->write( 0x00 );
00302         (*_cs) =   1;
00303     }else{
00304         _i2c->write( _i2c_addr * 2, (char*)(&addr), 1, 1 );
00305         _i2c->read(  _i2c_addr * 2, (char*)(&ret),  1 );
00306     }
00307     return ret;
00308 }
00309 
00310 // write register
00311 uint8_t MPU9250::_writeRegister( MPU9250REG addr, uint8_t data ){
00312     if( _imu_if == MPU9250_SPI ){
00313         (*_cs) =   0;
00314         _spi->write( addr );
00315         _spi->write( data );    
00316         (*_cs) =   1;
00317     }else{
00318         uint8_t buf[2] = { addr, data };
00319         _i2c->write( _i2c_addr * 2, (char*)buf, 2 );
00320     }
00321     return 0;
00322 }
00323 
00324 // レジスタの読み込み(バッファ)
00325 uint8_t MPU9250::_readBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
00326     if( _imu_if == MPU9250_SPI ){    
00327         (*_cs) =   0;
00328         _spi->write( addr | 0x80 );                  // send address
00329         while( len-- ){
00330             *(buf++) = _spi->write( 0x00 );          // read data
00331         }
00332         (*_cs) =   1;
00333     }else{
00334         _i2c->write( _i2c_addr * 2, (char*)(&addr),   1 );
00335         _i2c->read(  _i2c_addr * 2, (char*)buf,     len );
00336     }
00337     return 0;
00338 }
00339 
00340 // レジスタの書き込み(バッファ)
00341 uint8_t MPU9250::_writeBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
00342     if( _imu_if == MPU9250_SPI ){
00343         (*_cs) =   0;
00344         _spi->write( addr );                         // send address
00345         while( len-- ){
00346             _spi->write( *(buf++) );                 // send data
00347         }
00348         (*_cs) =   1;
00349     }else{
00350         _i2c->write( _i2c_addr * 2, (char*)(&addr),   1 );
00351         _i2c->write( _i2c_addr * 2, (char*)buf,     len );
00352     }
00353     return 0;
00354 }