Diff: Lib_MPU9250.cpp

Revision:

1:5334e111af53

Child:

2:c142d5a352c5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Lib_MPU9250.cpp	Fri Apr 20 18:13:45 2018 +0000
@@ -0,0 +1,320 @@
+#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;
+    
+    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( 400000 );
+    
+    _accscale   =   MPU9250A_2G;
+    _gyroscale  =   MPU9250G_250DPS;
+    return;
+}
+
+
+MPU9250::~MPU9250(){
+    return;    
+}
+
+// スタート
+void MPU9250::begin(){
+    // 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  );
+    return;
+}
+
+// リセット
+void MPU9250::reset(){
+    _writeRegister( PWR_MGMT_1, BIT_H_RESET );
+    return;
+}
+
+// 加速度センサのレンジ設定
+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;
+    (*_cs) =   0;
+    ret = _spi->write( addr | 0x80 );        // send address
+    ret = _spi->write( 0x00 );
+    (*_cs) =   1;
+    /*
+    uint8_t buf[2] = { addr, data };    
+    _i2c->write( ( _i2c_addr << 1 ) | I2C_WRITE_FLAG, &buf, 2, false );
+    _i2c->read(  ( _i2c_addr << 1 ) | I2C_READ_FLAG,  &ret, 1, true  );
+    */
+    return ret;
+}
+
+// write register
+uint8_t MPU9250::_writeRegister( MPU9250REG addr, uint8_t data ){
+    (*_cs) =   0;
+    _spi->write( addr );
+    _spi->write( data );    
+    (*_cs) =   1;
+    /*
+    uint8_t buf[2] = { addr, data };
+    _i2c->write( ( _i2c_addr << 1 ) | I2C_WRITE_FLAG, &buf, 2, true );
+    */
+    return 0;
+}
+
+// レジスタの読み込み(バッファ)
+uint8_t MPU9250::_readBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
+    (*_cs) =   0;
+    _spi->write( addr | 0x80 );                  // send address
+    while( len-- ){
+        *(buf++) = _spi->write( 0x00 );          // read data
+    }
+    (*_cs) =   1;
+    /*
+    uint8_t buf[2] = { addr, data };
+    _i2c->write( ( _i2c_addr << 1 ) | I2C_WRITE_FLAG, &buf, 2,   false );
+    _i2c->read(  ( _i2c_addr << 1 ) | I2C_READ_FLAG,  &buf, len, true  );
+    */
+    return 0;
+}
+
+// レジスタの書き込み(バッファ)
+uint8_t MPU9250::_writeBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
+    (*_cs) =   0;
+    _spi->write( addr );                         // send address
+    while( len-- ){
+        _spi->write( *(buf++) );                 // send data
+    }
+    (*_cs) =   1;
+    /*
+    uint8_t buf[2] = { addr, data };
+    _i2c->write( ( _i2c_addr << 1 ) | I2C_WRITE_FLAG, &buf, 2,   false );
+    _i2c->write( ( _i2c_addr << 1 ) | I2C_WRITE_FLAG, &buf, len, true  );
+    */    
+    return 0;
+}