Shimon AJISAKA / Lib_MPU9250_SPI

mbed library for MPU9250 (SPI interface)

Dependents:   InvertedPendulum2017

Lib_MPU9250.cpp@2:c142d5a352c5, 2018-05-02 (annotated)

Committer:
bluefish
Date:
Wed May 02 10:56:50 2018 +0000
Revision:
2:c142d5a352c5
Parent:
1:5334e111af53 
for Peshala project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bluefish 1:5334e111af53 1 #include "Lib_MPU9250.h"
bluefish 1:5334e111af53 2
bluefish 1:5334e111af53 3 // constructor (use SPI)
bluefish 1:5334e111af53 4 MPU9250::MPU9250( PinName cs, PinName mosi, PinName miso, PinName sck ){
bluefish 1:5334e111af53 5 // initialize instance
bluefish 1:5334e111af53 6 _cs = new DigitalOut( cs );
bluefish 1:5334e111af53 7 _spi = new SPI( mosi, miso, sck );
bluefish 1:5334e111af53 8 _i2c = 0;
bluefish 1:5334e111af53 9 _i2c_addr = 0x0;
bluefish 1:5334e111af53 10
bluefish 1:5334e111af53 11 // spi setting
bluefish 1:5334e111af53 12 (*_cs) = 1;
bluefish 1:5334e111af53 13 _spi->format( 8, 0 );
bluefish 1:5334e111af53 14 _spi->frequency( 20000000 );
bluefish 1:5334e111af53 15
bluefish 1:5334e111af53 16 _accscale = MPU9250A_2G;
bluefish 1:5334e111af53 17 _gyroscale = MPU9250G_250DPS;
bluefish 1:5334e111af53 18
bluefish 2:c142d5a352c5 19 _imu_if = MPU9250_SPI;
bluefish 2:c142d5a352c5 20
bluefish 1:5334e111af53 21 return;
bluefish 1:5334e111af53 22 }
bluefish 1:5334e111af53 23
bluefish 1:5334e111af53 24 // constructor ( use I2C )
bluefish 1:5334e111af53 25 MPU9250::MPU9250( PinName sda, PinName scl, uint8_t addr ){
bluefish 1:5334e111af53 26 // initialize instance
bluefish 1:5334e111af53 27 _cs = 0;
bluefish 1:5334e111af53 28 _spi = 0;
bluefish 1:5334e111af53 29 _i2c = new I2C( sda, scl );
bluefish 1:5334e111af53 30 _i2c_addr = addr;
bluefish 1:5334e111af53 31
bluefish 1:5334e111af53 32 // I2C setting
bluefish 2:c142d5a352c5 33 _i2c->frequency( 100000 );
bluefish 1:5334e111af53 34
bluefish 1:5334e111af53 35 _accscale = MPU9250A_2G;
bluefish 1:5334e111af53 36 _gyroscale = MPU9250G_250DPS;
bluefish 2:c142d5a352c5 37
bluefish 2:c142d5a352c5 38 _imu_if = MPU9250_I2C;
bluefish 2:c142d5a352c5 39
bluefish 1:5334e111af53 40 return;
bluefish 1:5334e111af53 41 }
bluefish 1:5334e111af53 42
bluefish 1:5334e111af53 43
bluefish 1:5334e111af53 44 MPU9250::~MPU9250(){
bluefish 2:c142d5a352c5 45 return;
bluefish 1:5334e111af53 46 }
bluefish 1:5334e111af53 47
bluefish 1:5334e111af53 48 // スタート
bluefish 1:5334e111af53 49 void MPU9250::begin(){
bluefish 2:c142d5a352c5 50 if( _imu_if == MPU9250_SPI ){
bluefish 2:c142d5a352c5 51 // initialize state
bluefish 2:c142d5a352c5 52 _writeRegister( PWR_MGMT_1, 0x01 );
bluefish 2:c142d5a352c5 53 _writeRegister( PWR_MGMT_2, 0x00 );
bluefish 2:c142d5a352c5 54 _writeRegister( USER_CTRL, 0x30 );
bluefish 2:c142d5a352c5 55 _writeRegister( I2C_MST_CTRL, 0x0D ); // I2C clock speed : 400kHz
bluefish 2:c142d5a352c5 56
bluefish 2:c142d5a352c5 57 // reset AK8963
bluefish 2:c142d5a352c5 58 _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR );
bluefish 2:c142d5a352c5 59 _writeRegister( I2C_SLV0_REG, CNTL2 );
bluefish 2:c142d5a352c5 60 _writeRegister( I2C_SLV0_DO, 0x01 );
bluefish 2:c142d5a352c5 61 _writeRegister( I2C_SLV0_CTRL, 0x81 );
bluefish 2:c142d5a352c5 62
bluefish 2:c142d5a352c5 63 // AK8963 mode : 100Hz sampling mode
bluefish 2:c142d5a352c5 64 _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR );
bluefish 2:c142d5a352c5 65 _writeRegister( I2C_SLV0_REG, CNTL1 );
bluefish 2:c142d5a352c5 66 _writeRegister( I2C_SLV0_DO, 0x16 );
bluefish 2:c142d5a352c5 67 _writeRegister( I2C_SLV0_CTRL, 0x81 );
bluefish 1:5334e111af53 68
bluefish 2:c142d5a352c5 69 }else if( _imu_if == MPU9250_I2C ){
bluefish 2:c142d5a352c5 70 // initialize state
bluefish 2:c142d5a352c5 71 _writeRegister( PWR_MGMT_1, 0x01 );
bluefish 2:c142d5a352c5 72 _writeRegister( PWR_MGMT_2, 0x00 );
bluefish 2:c142d5a352c5 73 _writeRegister( USER_CTRL, 0x00 );
bluefish 2:c142d5a352c5 74 _writeRegister( I2C_MST_CTRL, 0x0D ); // I2C clock speed : 400kHz
bluefish 2:c142d5a352c5 75 // reset AK8963
bluefish 2:c142d5a352c5 76 _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR );
bluefish 2:c142d5a352c5 77 _writeRegister( I2C_SLV0_REG, CNTL2 );
bluefish 2:c142d5a352c5 78 _writeRegister( I2C_SLV0_DO, 0x01 );
bluefish 2:c142d5a352c5 79 _writeRegister( I2C_SLV0_CTRL, 0x81 );
bluefish 2:c142d5a352c5 80
bluefish 2:c142d5a352c5 81 // AK8963 mode : 100Hz sampling mode
bluefish 2:c142d5a352c5 82 _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR );
bluefish 2:c142d5a352c5 83 _writeRegister( I2C_SLV0_REG, CNTL1 );
bluefish 2:c142d5a352c5 84 _writeRegister( I2C_SLV0_DO, 0x16 );
bluefish 2:c142d5a352c5 85 _writeRegister( I2C_SLV0_CTRL, 0x81 );
bluefish 1:5334e111af53 86
bluefish 2:c142d5a352c5 87 }
bluefish 1:5334e111af53 88 return;
bluefish 1:5334e111af53 89 }
bluefish 1:5334e111af53 90
bluefish 1:5334e111af53 91 // リセット
bluefish 1:5334e111af53 92 void MPU9250::reset(){
bluefish 1:5334e111af53 93 _writeRegister( PWR_MGMT_1, BIT_H_RESET );
bluefish 1:5334e111af53 94 return;
bluefish 1:5334e111af53 95 }
bluefish 1:5334e111af53 96
bluefish 2:c142d5a352c5 97 // WhoAmI
bluefish 2:c142d5a352c5 98 uint8_t MPU9250::getWhoAmI(){
bluefish 2:c142d5a352c5 99 uint8_t ret = 0x00;
bluefish 2:c142d5a352c5 100 ret = _readRegister( WHO_AM_I );
bluefish 2:c142d5a352c5 101 return ret;
bluefish 2:c142d5a352c5 102 }
bluefish 2:c142d5a352c5 103
bluefish 1:5334e111af53 104 // 加速度センサのレンジ設定
bluefish 1:5334e111af53 105 void MPU9250::setAccelRange( MPU9250BIT range ){
bluefish 1:5334e111af53 106 switch( range ){
bluefish 1:5334e111af53 107 case BITS_FS_2G:
bluefish 1:5334e111af53 108 _accscale = MPU9250A_2G;
bluefish 1:5334e111af53 109 break;
bluefish 1:5334e111af53 110 case BITS_FS_4G:
bluefish 1:5334e111af53 111 _accscale = MPU9250A_4G;
bluefish 1:5334e111af53 112 break;
bluefish 1:5334e111af53 113 case BITS_FS_8G:
bluefish 1:5334e111af53 114 _accscale = MPU9250A_8G;
bluefish 1:5334e111af53 115 break;
bluefish 1:5334e111af53 116 case BITS_FS_16G:
bluefish 1:5334e111af53 117 _accscale = MPU9250A_16G;
bluefish 1:5334e111af53 118 break;
bluefish 1:5334e111af53 119 default:
bluefish 1:5334e111af53 120 return;
bluefish 1:5334e111af53 121 }
bluefish 1:5334e111af53 122 _writeRegister( ACCEL_CONFIG, range );
bluefish 1:5334e111af53 123 return;
bluefish 1:5334e111af53 124 }
bluefish 1:5334e111af53 125
bluefish 1:5334e111af53 126 // ジャイロセンサのレンジ設定
bluefish 1:5334e111af53 127 void MPU9250::setGyroRange( MPU9250BIT range ){
bluefish 1:5334e111af53 128 switch( range ){
bluefish 1:5334e111af53 129 case BITS_FS_250DPS:
bluefish 1:5334e111af53 130 _gyroscale = MPU9250G_250DPS;
bluefish 1:5334e111af53 131 // _gyroscalerad = MPU9250G_250DPS_RAD;
bluefish 1:5334e111af53 132 break;
bluefish 1:5334e111af53 133 case BITS_FS_500DPS:
bluefish 1:5334e111af53 134 _gyroscale = MPU9250G_500DPS;
bluefish 1:5334e111af53 135 // _gyroscalerad = MPU9250G_500DPS_RAD;
bluefish 1:5334e111af53 136 break;
bluefish 1:5334e111af53 137 case BITS_FS_1000DPS:
bluefish 1:5334e111af53 138 _gyroscale = MPU9250G_1000DPS;
bluefish 1:5334e111af53 139 // _gyroscalerad = MPU9250G_1000DPS_RAD;
bluefish 1:5334e111af53 140 break;
bluefish 1:5334e111af53 141 case BITS_FS_2000DPS:
bluefish 1:5334e111af53 142 _gyroscale = MPU9250G_2000DPS;
bluefish 1:5334e111af53 143 // _gyroscalerad = MPU9250G_2000DPS_RAD;
bluefish 1:5334e111af53 144 break;
bluefish 1:5334e111af53 145 default:
bluefish 1:5334e111af53 146 return;
bluefish 1:5334e111af53 147 }
bluefish 1:5334e111af53 148 _writeRegister( GYRO_CONFIG, range );
bluefish 1:5334e111af53 149 return;
bluefish 1:5334e111af53 150 }
bluefish 1:5334e111af53 151
bluefish 1:5334e111af53 152 // ローパスフィルタ設定
bluefish 1:5334e111af53 153 void MPU9250::setDLPF( MPU9250BIT range ){
bluefish 1:5334e111af53 154 return;
bluefish 1:5334e111af53 155 }
bluefish 1:5334e111af53 156
bluefish 1:5334e111af53 157 // 6軸分の生データを取得
bluefish 1:5334e111af53 158 void MPU9250::read6AxisRaw( int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz ){
bluefish 1:5334e111af53 159 uint8_t buf[14];
bluefish 1:5334e111af53 160 _readBuffer( ACCEL_XOUT_H, 14, buf );
bluefish 1:5334e111af53 161 *ax = (int16_t)( ( buf[0] << 8 ) | buf[1] );
bluefish 1:5334e111af53 162 *ay = (int16_t)( ( buf[2] << 8 ) | buf[3] );
bluefish 1:5334e111af53 163 *az = (int16_t)( ( buf[4] << 8 ) | buf[5] );
bluefish 1:5334e111af53 164 *gx = (int16_t)( ( buf[8] << 8 ) | buf[9] );
bluefish 1:5334e111af53 165 *gy = (int16_t)( ( buf[10] << 8 ) | buf[11] );
bluefish 1:5334e111af53 166 *gz = (int16_t)( ( buf[12] << 8 ) | buf[13] );
bluefish 1:5334e111af53 167 return;
bluefish 1:5334e111af53 168 }
bluefish 1:5334e111af53 169
bluefish 1:5334e111af53 170 // 9軸分の生データを取得
bluefish 1:5334e111af53 171 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 ){
bluefish 1:5334e111af53 172 uint8_t buf[21];
bluefish 1:5334e111af53 173 _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR | 0x80 ); // Set the I2C slave addres of AK8963 and set for read.
bluefish 1:5334e111af53 174 _writeRegister( I2C_SLV0_REG, HXL ); // I2C slave 0 register address from where to begin data transfer
bluefish 1:5334e111af53 175 _writeRegister( I2C_SLV0_CTRL, 0x87 ); // Read 7 bytes from the magnetomete
bluefish 1:5334e111af53 176 _readBuffer( ACCEL_XOUT_H, 21, buf );
bluefish 1:5334e111af53 177 *ax = (int16_t)( ( buf[0] << 8 ) | buf[1] );
bluefish 1:5334e111af53 178 *ay = (int16_t)( ( buf[2] << 8 ) | buf[3] );
bluefish 1:5334e111af53 179 *az = (int16_t)( ( buf[4] << 8 ) | buf[5] );
bluefish 1:5334e111af53 180 *gx = (int16_t)( ( buf[8] << 8 ) | buf[9] );
bluefish 1:5334e111af53 181 *gy = (int16_t)( ( buf[10] << 8 ) | buf[11] );
bluefish 1:5334e111af53 182 *gz = (int16_t)( ( buf[12] << 8 ) | buf[13] );
bluefish 1:5334e111af53 183 *mx = (int16_t)( ( buf[15] << 8 ) | buf[14] );
bluefish 1:5334e111af53 184 *my = (int16_t)( ( buf[17] << 8 ) | buf[16] );
bluefish 1:5334e111af53 185 *mz = (int16_t)( ( buf[19] << 8 ) | buf[18] );
bluefish 1:5334e111af53 186 return;
bluefish 1:5334e111af53 187 }
bluefish 1:5334e111af53 188
bluefish 1:5334e111af53 189 // 加速度生データの取得
bluefish 1:5334e111af53 190 void MPU9250::readAccelRaw( int16_t* ax, int16_t* ay, int16_t* az ){
bluefish 1:5334e111af53 191 uint8_t buf[6];
bluefish 1:5334e111af53 192 _readBuffer( ACCEL_XOUT_H, 6, buf );
bluefish 1:5334e111af53 193 *ax = (int16_t)( ( buf[0] << 8 ) | buf[1] );
bluefish 1:5334e111af53 194 *ay = (int16_t)( ( buf[2] << 8 ) | buf[3] );
bluefish 1:5334e111af53 195 *az = (int16_t)( ( buf[4] << 8 ) | buf[5] );
bluefish 1:5334e111af53 196 return;
bluefish 1:5334e111af53 197 }
bluefish 1:5334e111af53 198
bluefish 1:5334e111af53 199 // ジャイロ生データの取得
bluefish 1:5334e111af53 200 void MPU9250::readGyroRaw( int16_t* gx, int16_t* gy, int16_t* gz ){
bluefish 1:5334e111af53 201 uint8_t buf[6];
bluefish 1:5334e111af53 202 _readBuffer( GYRO_XOUT_H, 6, buf );
bluefish 1:5334e111af53 203 *gx = (int16_t)( ( buf[0] << 8 ) | buf[1] );
bluefish 1:5334e111af53 204 *gy = (int16_t)( ( buf[2] << 8 ) | buf[3] );
bluefish 1:5334e111af53 205 *gz = (int16_t)( ( buf[4] << 8 ) | buf[5] );
bluefish 1:5334e111af53 206 return;
bluefish 1:5334e111af53 207 }
bluefish 1:5334e111af53 208
bluefish 1:5334e111af53 209 // 地磁気生データの取得
bluefish 1:5334e111af53 210 void MPU9250::readMagRaw( int16_t* mx, int16_t* my, int16_t* mz ){
bluefish 1:5334e111af53 211 uint8_t buf[7];
bluefish 1:5334e111af53 212 _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR | 0x80 ); // Set the I2C slave addres of AK8963 and set for read.
bluefish 1:5334e111af53 213 _writeRegister( I2C_SLV0_REG, HXL ); // I2C slave 0 register address from where to begin data transfer
bluefish 1:5334e111af53 214 _writeRegister( I2C_SLV0_CTRL, 0x87 ); // Read 7 bytes from the magnetomete
bluefish 1:5334e111af53 215 _readBuffer( EXT_SENS_DATA_00, 7, buf );
bluefish 1:5334e111af53 216 *mx = (int16_t)( ( buf[1] << 8 ) | buf[0] );
bluefish 1:5334e111af53 217 *my = (int16_t)( ( buf[3] << 8 ) | buf[2] );
bluefish 1:5334e111af53 218 *mz = (int16_t)( ( buf[5] << 8 ) | buf[4] );
bluefish 1:5334e111af53 219 return;
bluefish 1:5334e111af53 220 }
bluefish 1:5334e111af53 221
bluefish 1:5334e111af53 222 // 温度生データの取得
bluefish 1:5334e111af53 223 int16_t MPU9250::readTempRaw(){
bluefish 1:5334e111af53 224 uint8_t buf[2];
bluefish 1:5334e111af53 225 _readBuffer( TEMP_OUT_H, 2, buf );
bluefish 1:5334e111af53 226 return (int16_t)( ( buf[0] << 8 ) | buf[1] );
bluefish 1:5334e111af53 227 }
bluefish 1:5334e111af53 228
bluefish 1:5334e111af53 229 // 6軸データの取得
bluefish 1:5334e111af53 230 void MPU9250::read6Axis( float* ax, float* ay, float* az, float* gx, float* gy, float* gz ){
bluefish 1:5334e111af53 231 int16_t a_x, a_y, a_z, g_x, g_y, g_z;
bluefish 1:5334e111af53 232 read6AxisRaw( &a_x, &a_y, &a_z, &g_x, &g_y, &g_z );
bluefish 1:5334e111af53 233 *ax = (float)a_x * _accscale;
bluefish 1:5334e111af53 234 *ay = (float)a_y * _accscale;
bluefish 1:5334e111af53 235 *az = (float)a_z * _accscale;
bluefish 1:5334e111af53 236 *gx = (float)g_x * _gyroscale;
bluefish 1:5334e111af53 237 *gy = (float)g_y * _gyroscale;
bluefish 1:5334e111af53 238 *gz = (float)g_z * _gyroscale;
bluefish 1:5334e111af53 239 return;
bluefish 1:5334e111af53 240 }
bluefish 1:5334e111af53 241
bluefish 1:5334e111af53 242 // 9軸データの取得
bluefish 1:5334e111af53 243 void MPU9250::read9Axis( float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* mx, float* my, float* mz ){
bluefish 1:5334e111af53 244 int16_t a_x, a_y, a_z, g_x, g_y, g_z, m_x, m_y, m_z;
bluefish 1:5334e111af53 245 read9AxisRaw( &a_x, &a_y, &a_z, &g_x, &g_y, &g_z, &m_x, &m_y, &m_z );
bluefish 1:5334e111af53 246 *ax = (float)a_x * _accscale;
bluefish 1:5334e111af53 247 *ay = (float)a_y * _accscale;
bluefish 1:5334e111af53 248 *az = (float)a_z * _accscale;
bluefish 1:5334e111af53 249 *gx = (float)g_x * _gyroscale;
bluefish 1:5334e111af53 250 *gy = (float)g_y * _gyroscale;
bluefish 1:5334e111af53 251 *gz = (float)g_z * _gyroscale;
bluefish 1:5334e111af53 252 *mx = (float)m_x * MPU9250M_4800uT;
bluefish 1:5334e111af53 253 *my = (float)m_y * MPU9250M_4800uT;
bluefish 1:5334e111af53 254 *mz = (float)m_z * MPU9250M_4800uT;
bluefish 1:5334e111af53 255 return;
bluefish 1:5334e111af53 256 }
bluefish 1:5334e111af53 257
bluefish 1:5334e111af53 258 // 加速度データの取得
bluefish 1:5334e111af53 259 void MPU9250::readAccel( float* ax, float* ay, float* az ){
bluefish 1:5334e111af53 260 int16_t x, y ,z;
bluefish 1:5334e111af53 261 readAccelRaw( &x, &y, &z );
bluefish 1:5334e111af53 262 *ax = (float)x * _accscale;
bluefish 1:5334e111af53 263 *ay = (float)y * _accscale;
bluefish 1:5334e111af53 264 *az = (float)z * _accscale;
bluefish 1:5334e111af53 265 return;
bluefish 1:5334e111af53 266 }
bluefish 1:5334e111af53 267
bluefish 1:5334e111af53 268 // ジャイロデータの取得
bluefish 1:5334e111af53 269 void MPU9250::readGyro( float* gx, float* gy, float* gz ){
bluefish 1:5334e111af53 270 int16_t x, y ,z;
bluefish 1:5334e111af53 271 readGyroRaw( &x, &y, &z );
bluefish 1:5334e111af53 272 *gx = (float)x * _gyroscale;
bluefish 1:5334e111af53 273 *gy = (float)y * _gyroscale;
bluefish 1:5334e111af53 274 *gz = (float)z * _gyroscale;
bluefish 1:5334e111af53 275 return;
bluefish 1:5334e111af53 276 }
bluefish 1:5334e111af53 277
bluefish 1:5334e111af53 278 // 地磁気データの取得
bluefish 1:5334e111af53 279 void MPU9250::readMag( float* mx, float* my, float* mz ){
bluefish 1:5334e111af53 280 int16_t x, y ,z;
bluefish 1:5334e111af53 281 readMagRaw( &x, &y, &z );
bluefish 1:5334e111af53 282 *mx = (float)x * MPU9250M_4800uT;
bluefish 1:5334e111af53 283 *my = (float)y * MPU9250M_4800uT;
bluefish 1:5334e111af53 284 *mz = (float)z * MPU9250M_4800uT;
bluefish 1:5334e111af53 285 return;
bluefish 1:5334e111af53 286 }
bluefish 1:5334e111af53 287
bluefish 1:5334e111af53 288 // get temperature
bluefish 1:5334e111af53 289 float MPU9250::readTemp(){
bluefish 1:5334e111af53 290 int16_t tmp;
bluefish 1:5334e111af53 291 tmp = readTempRaw();
bluefish 1:5334e111af53 292 return (float)tmp * MPU9250T_85degC;
bluefish 1:5334e111af53 293 }
bluefish 1:5334e111af53 294
bluefish 1:5334e111af53 295 // read register
bluefish 1:5334e111af53 296 uint8_t MPU9250::_readRegister( MPU9250REG addr ){
bluefish 1:5334e111af53 297 uint8_t ret;
bluefish 2:c142d5a352c5 298 if( _imu_if == MPU9250_SPI ){
bluefish 2:c142d5a352c5 299 (*_cs) = 0;
bluefish 2:c142d5a352c5 300 ret = _spi->write( addr | 0x80 ); // send address
bluefish 2:c142d5a352c5 301 ret = _spi->write( 0x00 );
bluefish 2:c142d5a352c5 302 (*_cs) = 1;
bluefish 2:c142d5a352c5 303 }else{
bluefish 2:c142d5a352c5 304 _i2c->write( _i2c_addr * 2, (char*)(&addr), 1, 1 );
bluefish 2:c142d5a352c5 305 _i2c->read( _i2c_addr * 2, (char*)(&ret), 1 );
bluefish 2:c142d5a352c5 306 }
bluefish 1:5334e111af53 307 return ret;
bluefish 1:5334e111af53 308 }
bluefish 1:5334e111af53 309
bluefish 1:5334e111af53 310 // write register
bluefish 1:5334e111af53 311 uint8_t MPU9250::_writeRegister( MPU9250REG addr, uint8_t data ){
bluefish 2:c142d5a352c5 312 if( _imu_if == MPU9250_SPI ){
bluefish 2:c142d5a352c5 313 (*_cs) = 0;
bluefish 2:c142d5a352c5 314 _spi->write( addr );
bluefish 2:c142d5a352c5 315 _spi->write( data );
bluefish 2:c142d5a352c5 316 (*_cs) = 1;
bluefish 2:c142d5a352c5 317 }else{
bluefish 2:c142d5a352c5 318 uint8_t buf[2] = { addr, data };
bluefish 2:c142d5a352c5 319 _i2c->write( _i2c_addr * 2, (char*)buf, 2 );
bluefish 2:c142d5a352c5 320 }
bluefish 1:5334e111af53 321 return 0;
bluefish 1:5334e111af53 322 }
bluefish 1:5334e111af53 323
bluefish 1:5334e111af53 324 // レジスタの読み込み(バッファ)
bluefish 1:5334e111af53 325 uint8_t MPU9250::_readBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
bluefish 2:c142d5a352c5 326 if( _imu_if == MPU9250_SPI ){
bluefish 2:c142d5a352c5 327 (*_cs) = 0;
bluefish 2:c142d5a352c5 328 _spi->write( addr | 0x80 ); // send address
bluefish 2:c142d5a352c5 329 while( len-- ){
bluefish 2:c142d5a352c5 330 *(buf++) = _spi->write( 0x00 ); // read data
bluefish 2:c142d5a352c5 331 }
bluefish 2:c142d5a352c5 332 (*_cs) = 1;
bluefish 2:c142d5a352c5 333 }else{
bluefish 2:c142d5a352c5 334 _i2c->write( _i2c_addr * 2, (char*)(&addr), 1 );
bluefish 2:c142d5a352c5 335 _i2c->read( _i2c_addr * 2, (char*)buf, len );
bluefish 1:5334e111af53 336 }
bluefish 1:5334e111af53 337 return 0;
bluefish 1:5334e111af53 338 }
bluefish 1:5334e111af53 339
bluefish 1:5334e111af53 340 // レジスタの書き込み(バッファ)
bluefish 1:5334e111af53 341 uint8_t MPU9250::_writeBuffer( MPU9250REG addr, uint8_t len, uint8_t* buf ){
bluefish 2:c142d5a352c5 342 if( _imu_if == MPU9250_SPI ){
bluefish 2:c142d5a352c5 343 (*_cs) = 0;
bluefish 2:c142d5a352c5 344 _spi->write( addr ); // send address
bluefish 2:c142d5a352c5 345 while( len-- ){
bluefish 2:c142d5a352c5 346 _spi->write( *(buf++) ); // send data
bluefish 2:c142d5a352c5 347 }
bluefish 2:c142d5a352c5 348 (*_cs) = 1;
bluefish 2:c142d5a352c5 349 }else{
bluefish 2:c142d5a352c5 350 _i2c->write( _i2c_addr * 2, (char*)(&addr), 1 );
bluefish 2:c142d5a352c5 351 _i2c->write( _i2c_addr * 2, (char*)buf, len );
bluefish 1:5334e111af53 352 }
bluefish 1:5334e111af53 353 return 0;
bluefish 1:5334e111af53 354 }