Diff: Lib_MPU9250.cpp

Revision:

2:c142d5a352c5

Parent:

1:5334e111af53

--- a/Lib_MPU9250.cpp	Fri Apr 20 18:13:45 2018 +0000
+++ b/Lib_MPU9250.cpp	Wed May 02 10:56:50 2018 +0000
@@ -16,6 +16,8 @@
     _accscale   =   MPU9250A_2G;
     _gyroscale  =   MPU9250G_250DPS;
     
+    _imu_if     =   MPU9250_SPI;
+    
     return;
 }
 
@@ -28,37 +30,61 @@
     _i2c_addr   =   addr;
     
     // I2C setting
-    _i2c->frequency( 400000 );
+    _i2c->frequency( 100000 );
     
     _accscale   =   MPU9250A_2G;
     _gyroscale  =   MPU9250G_250DPS;
+    
+    _imu_if     =   MPU9250_I2C;
+    
     return;
 }
 
 
 MPU9250::~MPU9250(){
-    return;    
+    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
+    if( _imu_if == MPU9250_SPI ){
+        // 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  );
     
-    // reset AK8963
-    _writeRegister( I2C_SLV0_ADDR, AK8963_ADDR  );
-    _writeRegister( I2C_SLV0_REG,  CNTL2 );
-    _writeRegister( I2C_SLV0_DO,   0x01  );
-    _writeRegister( I2C_SLV0_CTRL, 0x81  );
+    }else if( _imu_if == MPU9250_I2C ){
+        // initialize state
+        _writeRegister( PWR_MGMT_1,    0x01  );
+        _writeRegister( PWR_MGMT_2,    0x00  );
+        _writeRegister( USER_CTRL,     0x00  );
+        _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  );
 
-    // 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;
 }
 
@@ -68,6 +94,13 @@
     return;
 }
 
+// WhoAmI
+uint8_t MPU9250::getWhoAmI(){
+    uint8_t ret = 0x00;
+    ret = _readRegister( WHO_AM_I );
+    return ret;    
+}
+
 // 加速度センサのレンジ設定
 void MPU9250::setAccelRange( MPU9250BIT range ){
     switch( range ){
@@ -262,59 +295,60 @@
 // 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  );
-    */
+    if( _imu_if == MPU9250_SPI ){
+        (*_cs) =   0;
+        ret = _spi->write( addr | 0x80 );        // send address
+        ret = _spi->write( 0x00 );
+        (*_cs) =   1;
+    }else{
+        _i2c->write( _i2c_addr * 2, (char*)(&addr), 1, 1 );
+        _i2c->read(  _i2c_addr * 2, (char*)(&ret),  1 );
+    }
     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 );
-    */
+    if( _imu_if == MPU9250_SPI ){
+        (*_cs) =   0;
+        _spi->write( addr );
+        _spi->write( data );    
+        (*_cs) =   1;
+    }else{
+        uint8_t buf[2] = { addr, data };
+        _i2c->write( _i2c_addr * 2, (char*)buf, 2 );
+    }
     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
+    if( _imu_if == MPU9250_SPI ){    
+        (*_cs) =   0;
+        _spi->write( addr | 0x80 );                  // send address
+        while( len-- ){
+            *(buf++) = _spi->write( 0x00 );          // read data
+        }
+        (*_cs) =   1;
+    }else{
+        _i2c->write( _i2c_addr * 2, (char*)(&addr),   1 );
+        _i2c->read(  _i2c_addr * 2, (char*)buf,     len );
     }
-    (*_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
+    if( _imu_if == MPU9250_SPI ){
+        (*_cs) =   0;
+        _spi->write( addr );                         // send address
+        while( len-- ){
+            _spi->write( *(buf++) );                 // send data
+        }
+        (*_cs) =   1;
+    }else{
+        _i2c->write( _i2c_addr * 2, (char*)(&addr),   1 );
+        _i2c->write( _i2c_addr * 2, (char*)buf,     len );
     }
-    (*_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;
 }