mbed library for MPU9250 (SPI interface)
Dependents: InvertedPendulum2017
Lib_MPU9250.cpp
- Committer:
- bluefish
- Date:
- 2018-04-20
- Revision:
- 1:5334e111af53
- Child:
- 2:c142d5a352c5
File content as of revision 1:5334e111af53:
#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; }