Shimon AJISAKA / Lib_MPU9250_SPI

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   InvertedPendulum2017

Lib_MPU9250_SPI.cpp@0:551dbbd41a10, 2017-09-15 (annotated)

Committer:
bluefish
Date:
Fri Sep 15 14:54:49 2017 +0000
Revision:
0:551dbbd41a10
???????

Who changed what in which revision?

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