Driver for MPU9250 with SPI .

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU9205_SPI.cpp Source File

MPU9205_SPI.cpp

00001 #include "mbed.h"
00002 #include  "MPU9250_SPI.h"
00003 #include  "MPU9250RegisterMap.h"
00004 #include <cmath>
00005 // MPU9250 with SPI interface library Ver. 0.98
00006 // Made by HeeJae Park 
00007 // 2019.05.27
00008 //extern Serial pc;
00009 volatile bool MPU9250_SPI::_dataReady=false;
00010 MPU9250_SPI::MPU9250_SPI(PinName mosi,PinName miso,PinName sclk, PinName cs, PinName intpin)
00011 : _spi(mosi,miso,sclk), _csPin(cs), _intPin(intpin),_mMode(MGN_CONT_MEAS2),_mBits( MGN_16BITS),_srd(SR_100HZ)  {
00012       magCalibration.x=0;magCalibration.y=0;magCalibration.z=0;   
00013       magBias.x=0; magBias.y=0; magBias.z=0;
00014       magScale.x=1;magScale.y=1;magScale.z=1;
00015       gyroBias.x =0; gyroBias.y =0; gyroBias.z =0; 
00016       accelBias.x=0; accelBias.y=0; accelBias.z=0; 
00017       magnetic_declination = 8.5;
00018       _csPin=1;
00019 }
00020 void  MPU9250_SPI::setup() {      
00021     _csPin=1;      // setting CS pin high  
00022     _spi.format(8,3); // SPI mode 3
00023     _spi.frequency(SPI_HS_CLOCK); // 1Mega
00024     uint8_t m_whoami = 0x00;
00025     uint8_t a_whoami = 0x00;
00026     m_whoami = isConnectedMPU9250();
00027     if (m_whoami==MPU9250_WHOAMI_DEFAULT_VALUE) {
00028       initMPU9250();
00029       a_whoami = isConnectedAK8963();
00030       if (a_whoami == AK8963_WHOAMI_DEFAULT_VALUE){
00031           initAK8963();
00032       }
00033       else {
00034           while(1);
00035       }
00036   }
00037   else {
00038       while(1);
00039   }  
00040   _intPin.rise(callback(this, &MPU9250_SPI::intService)); 
00041   _tmr.start();
00042 } 
00043 void MPU9250_SPI::update(Vect3& _a,Vect3& _g,Vect3& _m)    {
00044   if (_dataReady){  // On interrupt, check if data ready interrupt
00045       updateSensors();
00046       _a=a;_g=g;_m=m;
00047    }
00048 }
00049 
00050 uint8_t MPU9250_SPI::isConnectedMPU9250() {
00051     uint8_t c = readByte(WHO_AM_I_MPU9250);
00052     return c; // (c == MPU9250_WHOAMI_DEFAULT_VALUE);
00053 }
00054 uint8_t MPU9250_SPI::isConnectedAK8963() {
00055     uint8_t c = readAK8963Byte(AK8963_WHO_AM_I);
00056     return c; // (c == AK8963_WHOAMI_DEFAULT_VALUE);
00057 }
00058 
00059 void MPU9250_SPI::initMPU9250()    {
00060     wait_ms(100);
00061     writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
00062     writeByte(USER_CTRL,I2C_MST_EN);       // Master enable
00063     writeByte(I2C_MST_CTRL,I2C_MST_CLK);   // I2C master clock =400HZ
00064     replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down
00065     writeByte(PWR_MGMT_1, PWR_RESET); // Clear sleep mode bit (6), enable all sensors  
00066     wait_ms(100);
00067     writeByte(PWR_MGMT_1, CLOCK_SEL_PLL);
00068     setDlpfBandwidth( DLPF_BANDWIDTH_5HZ);
00069     writeByte(SMPLRT_DIV, SR_100HZ);  //{SR_1000HZ=0, SR_200HZ=4, SR_100HZ=9 }
00070     setGyroRange(GYRO_RANGE_2000DPS);             
00071     writeByte(PWR_MGMT_2,SEN_ENABLE);      
00072     setAccelRange(ACCEL_RANGE_16G);//{ _2G, _4G,  _8G,  _16G  }
00073     setDlpfBandwidth(DLPF_BANDWIDTH_184HZ);  // [250HZ, 184HZ,  92HZ,  41HZ, 20HZ,  10HZ,  5HZ]    
00074     writeByte(INT_PIN_CFG, 0x20);  // LATCH_INT_EN=1,  BYPASS_EN=1-->0 (0x22)
00075     writeByte(INT_ENABLE, 0x01);  // Enable raw data ready (bit 0) interrupt
00076     writeByte(USER_CTRL,I2C_MST_EN);
00077     wait_ms(100);
00078     writeByte(I2C_MST_CTRL,I2C_MST_CLK);         
00079     wait_ms(100);
00080 }
00081 
00082 void MPU9250_SPI::initAK8963()    {
00083     uint8_t rawData[3];  // x/y/z gyro calibration data stored here
00084     replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
00085     wait_ms(50);
00086     replaceBlockAK(AK8963_CNTL,MGN_FUSE_ROM,0,4);
00087     wait_ms(50);
00088     readAK8963Bytes( AK8963_ASAX, 3, rawData);  // Read the x-, y-, and z-axis calibration values
00089     magCalibration.x =  (float)(rawData[0] - 128)/256.f + 1.f;   // Return x-axis sensitivity adjustment values, etc.
00090     magCalibration.y =  (float)(rawData[1] - 128)/256.f + 1.f;
00091     magCalibration.z =  (float)(rawData[2] - 128)/256.f + 1.f;
00092     replaceBlockAK(AK8963_CNTL,MGN_POWER_DN,0,4); // Power down magnetometer
00093     wait_ms(50);
00094     replaceBlockAK(AK8963_CNTL,((_mBits << 4 )| _mMode),0,5); // Set measurment mode, mMode[0:3]
00095     writeByte(PWR_MGMT_1,CLOCK_SEL_PLL);
00096     wait_ms(50);   
00097     mRes=10. * 4912. / 32760.0;  // for Magenetometer 16BITS
00098 }
00099 
00100 void MPU9250_SPI::setAccelRange(AccelRange range) {
00101    switch(range) {
00102      case ACCEL_RANGE_2G: 
00103      aRes =  2.0f/32767.5f;  break;     
00104     case ACCEL_RANGE_4G: 
00105      aRes =  4.0f/32767.5f;   break;    
00106     case ACCEL_RANGE_8G: 
00107       aRes =  8.0f/32767.5f;  break;    
00108     case ACCEL_RANGE_16G: 
00109       aRes = 16.0f/32767.5f; // setting the accel scale to 16G
00110       break;    
00111    }
00112    replaceBlock(ACCEL_CONFIG,range,3,2); // addr, value, at, size 
00113    _accelRange = range;
00114 }
00115 void MPU9250_SPI::setGyroRange(GyroRange range) {
00116   switch(range) {
00117     case GYRO_RANGE_250DPS: 
00118       gRes =  250.0f/32767.5f;  break;   
00119     case GYRO_RANGE_500DPS: 
00120       gRes =  500.0f/32767.5f; break;      
00121     case GYRO_RANGE_1000DPS:
00122       gRes =  1000.0f/32767.5f; break; 
00123     case GYRO_RANGE_2000DPS:   
00124      gRes =  2000.0f/32767.5f ; break; 
00125   }
00126   replaceBlock(GYRO_CONFIG,range,3,2);
00127   _gyroRange = range;
00128 }
00129 void MPU9250_SPI::setDlpfBandwidth(DlpfBandwidth bandwidth) {
00130   replaceBlock(ACCEL_CONFIG2,bandwidth,0,4);     //Accel DLPF [0:2]
00131   replaceBlock(MPU_CONFIG,bandwidth,0,3);        //Gyro DLPF [0:2]
00132   _bandwidth = bandwidth;
00133 }
00134 
00135 void MPU9250_SPI::setSampleRate(SampleRate srd){
00136    writeByte(SMPLRT_DIV, srd);   // sampling rate set
00137    _srd = srd;
00138 }
00139 
00140 void MPU9250_SPI::enableDataReadyInterrupt() {
00141   writeByte(INT_PIN_CFG,0x00);  // setup interrupt, 50 us pulse
00142   writeByte(INT_ENABLE,0x01) ; // set to data ready
00143 }
00144 
00145 void MPU9250_SPI::updateSensors(){
00146   int16_t MPU9250Data[10]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
00147   uint8_t rawData[21];  // 가속도 자이로 원시 데이터 보관
00148   writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR|SPI_READ); // Set the I2C slave addres of AK8963 and set for read.
00149   writeByte(I2C_SLV0_REG,AK8963_XOUT_L);   // I2C slave 0 register address from where to begin data transfer
00150   writeByte(I2C_SLV0_CTRL, 0x87);                     // Read 7 bytes from the magnetometer
00151   readBytes(ACCEL_XOUT_H, 21, rawData);  // 16비트 정수로 7개 저장--> 14byte
00152   MPU9250Data[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // signed 16-bit  (MSB + LSB)
00153   MPU9250Data[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
00154   MPU9250Data[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
00155   MPU9250Data[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
00156   MPU9250Data[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
00157   MPU9250Data[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
00158   MPU9250Data[6] = ((int16_t)rawData[12] << 8) | rawData[13] ; 
00159   MPU9250Data[7] = (((int16_t)rawData[15]) << 8) |rawData[14];
00160   MPU9250Data[8] = (((int16_t)rawData[17]) << 8) |rawData[16];
00161   MPU9250Data[9] = (((int16_t)rawData[19]) << 8) |rawData[18];                
00162   a.x = (float)MPU9250Data[0] * aRes - accelBias.x;  // 가속도 해상도와 바이어스 보정 
00163   a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
00164   a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
00165   g.x = (float)MPU9250Data[4] * gRes - gyroBias.x;  // 자이로 해상도 보정
00166   g.y = (float)MPU9250Data[5] * gRes - gyroBias.y;  // 자이로 바이어스는 칩내부에서 보정함!!!
00167   g.z = (float)MPU9250Data[6] * gRes - gyroBias.z;  
00168   m.x = (float)(MPU9250Data[7] * mRes * magCalibration.x - magBias.x) * magScale.x;  
00169   m.y = (float)(MPU9250Data[8] * mRes * magCalibration.y - magBias.y) * magScale.y;
00170   m.z = (float)(MPU9250Data[9] * mRes * magCalibration.z - magBias.z) * magScale.z;               
00171 }
00172 void MPU9250_SPI::updateAccelGyro()    {
00173     int16_t MPU9250Data[7]; // MPU9250 accel/gyro 에서 16비트 정수로 7개 저장
00174     readMPU9250Data(MPU9250Data); // 읽으면 INT 핀 해제 
00175     a.x = (float)MPU9250Data[0] * aRes - accelBias.x;  // 가속도 해상도와 바이어스 보정 
00176     a.y = (float)MPU9250Data[1] * aRes - accelBias.y;
00177     a.z = (float)MPU9250Data[2] * aRes - accelBias.z;
00178     g.x = (float)MPU9250Data[4] * gRes - gyroBias.x;  // 자이로 해상도 보정
00179     g.y = (float)MPU9250Data[5] * gRes - gyroBias.y;  // 
00180     g.z = (float)MPU9250Data[6] * gRes - gyroBias.z;
00181 }
00182 
00183 void MPU9250_SPI::readMPU9250Data(int16_t * destination)     {
00184     uint8_t rawData[14];  // 가속도 자이로 원시 데이터 보관
00185     readBytes(ACCEL_XOUT_H, 14, rawData);  // 16비트 정수로 7개 저장--> 14byte
00186     destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ;  // signed 16-bit  (MSB + LSB)
00187     destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
00188     destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
00189     destination[3] = ((int16_t)rawData[6] << 8) | rawData[7] ;
00190     destination[4] = ((int16_t)rawData[8] << 8) | rawData[9] ;
00191     destination[5] = ((int16_t)rawData[10] << 8) | rawData[11] ;
00192     destination[6] = ((int16_t)rawData[12] << 8) | rawData[13] ;
00193 }
00194 
00195 void MPU9250_SPI::updateMag()    {
00196     int16_t magCount[3] = {0, 0, 0};    // 16-bit 지자기 데이터
00197     readMagData(magCount);  // 지자기 데이터 읽기
00198     // 지자기 해상도, 검정값, 바이어스 보정,  검정값 (magCalibration[] )은 칩의 ROM에서 
00199     m.x = (float)(magCount[0] * mRes * magCalibration.x - magBias.x) * magScale.x;  
00200     m.y = (float)(magCount[1] * mRes * magCalibration.y - magBias.y) * magScale.y;
00201     m.z = (float)(magCount[2] * mRes * magCalibration.z - magBias.z) * magScale.z;
00202 }
00203 void MPU9250_SPI::readMagData(int16_t * destination)    {
00204     uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
00205     if(readAK8963Byte(AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
00206         readAK8963Bytes(AK8963_XOUT_L, 7,rawData);  // Read the six raw data and ST2 registers sequentially into data array
00207         uint8_t c = rawData[6]; // End data read by reading ST2 register
00208         if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
00209             destination[0] = ((int16_t)rawData[1] << 8) | rawData[0];  // Turn the MSB and LSB into a signed 16-bit value
00210             destination[1] = ((int16_t)rawData[3] << 8) | rawData[2];  // Data stored as little Endian
00211             destination[2] = ((int16_t)rawData[5] << 8) | rawData[4];
00212         }
00213     }
00214 }  
00215 
00216 void MPU9250_SPI::writeByte(uint8_t subAddress, uint8_t data){   /* write data to device */
00217     // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
00218     // digitalWrite(_csPin,LOW); // select the MPU9250 chip
00219     // _spi->transfer(subAddress); // write the register address
00220     // _spi->transfer(data); // write the data
00221     // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
00222     // _spi->endTransaction(); // end the transaction
00223     _spi.frequency(SPI_LS_CLOCK); // setup clock
00224     _csPin=0; // select the MPU9250 chip
00225     _spi.write(subAddress); // write the register address
00226     _spi.write(data); // write the data
00227     _csPin=1; // deselect the MPU9250 chip
00228 }
00229 uint8_t MPU9250_SPI::readByte(uint8_t subAddress){
00230     // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
00231     // digitalWrite(_csPin,LOW); // select the MPU9250 chip
00232     // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
00233     // uint8_t data = _spi->transfer(0x00); // read the data
00234     // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
00235     // _spi->endTransaction(); // end the transaction
00236 
00237     _spi.frequency(SPI_LS_CLOCK); // setup clock
00238     _csPin=0; // select the MPU9250 chip
00239     _spi.write(subAddress| SPI_READ); // use READ MASK
00240     uint8_t data =_spi.write(0);   // write any to get data
00241     _csPin=1; // deselect the MPU9250 chip 
00242     return data;
00243 }
00244 
00245 void MPU9250_SPI::readBytes(uint8_t subAddress, uint8_t cnt, uint8_t* dest){
00246     // _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
00247     // digitalWrite(_csPin,LOW); // select the MPU9250 chip
00248     // _spi->transfer(subAddress | SPI_READ); // specify the starting register address
00249     // for(uint8_t i = 0; i < count; i++){
00250     //   dest[i] = _spi->transfer(0x00); // read the data
00251     // }
00252     // digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
00253     // _spi->endTransaction(); // end the transaction
00254     _spi.frequency(SPI_HS_CLOCK); // setup clock
00255      _csPin=0; // select the MPU9250 chip
00256     _spi.write(subAddress | SPI_READ); // specify the starting register address
00257     for(uint8_t i = 0; i < cnt; i++){
00258       dest[i] = _spi.write(0x00); // read the data
00259     }
00260     _csPin=1; // deselect the MPU9250 chip
00261 }
00262 
00263 void MPU9250_SPI::writeAK8963Byte(uint8_t subAddress, uint8_t data){   
00264     writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR) ; // set slave 0 to the AK8963 and set for write
00265     writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address 
00266     writeByte(I2C_SLV0_DO,data) ; // store the data for write
00267     writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1); // enable I2C and send 1 byte
00268 }
00269 
00270 void MPU9250_SPI::readAK8963Bytes(uint8_t subAddress, uint8_t count, uint8_t* dest){
00271    writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
00272    writeByte(I2C_SLV0_REG,subAddress) ; // set the register to the desired AK8963 sub address
00273    writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and request the bytes
00274    wait_ms(1); // takes some time for these registers to fill
00275    readBytes(EXT_SENS_DATA_00,count,dest);  // read the bytes off the MPU9250 EXT_SENS_DATA registers
00276 }
00277 
00278 uint8_t MPU9250_SPI::readAK8963Byte(uint8_t subAddress){
00279   writeByte(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG) ; // set slave 0 to the AK8963 and set for read
00280   writeByte(I2C_SLV0_REG,subAddress) ;  // set the register to the desired AK8963 sub address
00281   writeByte(I2C_SLV0_CTRL,I2C_SLV0_EN | (uint8_t)1);   // enable I2C and request the bytes
00282   wait_ms(11); // takes some time for these registers to fill
00283   return  readByte(EXT_SENS_DATA_00);  // read the bytes off the MPU9250 EXT_SENS_DATA registers 
00284 }
00285 void MPU9250_SPI::replaceBlock(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){
00286   uint8_t data=readByte(address);
00287   data &= ~(((1<<sz)-1)<<at);
00288   data |= block<<at;
00289   writeByte(address, data );
00290 }
00291 void MPU9250_SPI::replaceBlockAK(uint8_t address, uint8_t block, uint8_t at, uint8_t sz){
00292   uint8_t data=readByte(address);
00293   data &= ~(((1<<sz)-1)<<at);
00294   data |= block<<at;
00295   writeAK8963Byte(address, data );
00296 }    
00297