MPU9250_SPI.cpp

00001 /*CODED by Qiyong Mu on 21/06/2014
00002 kylongmu@msn.com
00003 */
00004 
00005 #include <mbed.h>
00006 #include "MPU9250_SPI.h"
00007 
00008 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
00009 
00010 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
00011 {
00012     unsigned int temp_val;
00013     select();
00014     spi.write(WriteAddr);
00015     temp_val=spi.write(WriteData);
00016     deselect();
00017     wait_us(5);
00018     return temp_val;
00019 }
00020 unsigned int  mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
00021 {
00022     return WriteReg(WriteAddr | READ_FLAG,WriteData);
00023 }
00024 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
00025 {
00026     unsigned int  i = 0;
00027 
00028     select();
00029     spi.write(ReadAddr | READ_FLAG);
00030     for(i=0; i<Bytes; i++)
00031         ReadBuf[i] = spi.write(0x00);
00032     deselect();
00033     //wait_us(50);
00034 }
00035 
00036 /*-----------------------------------------------------------------------------------------------
00037                                     INITIALIZATION
00038 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
00039 low pass filter value; suitable values are:
00040 BITS_DLPF_CFG_256HZ_NOLPF2
00041 BITS_DLPF_CFG_188HZ
00042 BITS_DLPF_CFG_98HZ
00043 BITS_DLPF_CFG_42HZ
00044 BITS_DLPF_CFG_20HZ
00045 BITS_DLPF_CFG_10HZ 
00046 BITS_DLPF_CFG_5HZ 
00047 BITS_DLPF_CFG_2100HZ_NOLPF
00048 returns 1 if an error occurred
00049 -----------------------------------------------------------------------------------------------*/
00050 #define MPU_InitRegNum 17
00051 
00052 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
00053     uint8_t i = 0;
00054     uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
00055         {0x80, MPUREG_PWR_MGMT_1},     // Reset Device
00056         {0x01, MPUREG_PWR_MGMT_1},     // Clock Source
00057         {0x00, MPUREG_PWR_MGMT_2},     // Enable Acc & Gyro
00058         {0x01, MPUREG_CONFIG},         // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz //3
00059         {0x08, MPUREG_GYRO_CONFIG},    // +-1000dps = 10 / 500
00060         {0x00, MPUREG_ACCEL_CONFIG},   // +-2G
00061         {0x03, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 41Hz
00062         {0x30, MPUREG_INT_PIN_CFG},    //
00063         //{0x40, MPUREG_I2C_MST_CTRL},   // I2C Speed 348 kHz
00064         //{0x20, MPUREG_USER_CTRL},      // Enable AUX
00065         {0x20, MPUREG_USER_CTRL},       // I2C Master mode
00066         {0x0D, MPUREG_I2C_MST_CTRL}, //  I2C configuration multi-master  IIC 400KHz
00067         
00068         {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR},  //Set the I2C slave addres of AK8963 and set for write.
00069         //{0x09, MPUREG_I2C_SLV4_CTRL},
00070         //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
00071 
00072         {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
00073         {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
00074         {0x81, MPUREG_I2C_SLV0_CTRL},  //Enable I2C and set 1 byte
00075 
00076         {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
00077         {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
00078         {0x81, MPUREG_I2C_SLV0_CTRL}  //Enable I2C and set 1 byte
00079         
00080     };
00081     spi.format(8,0);
00082     spi.frequency(4000000);
00083 
00084     for(i=0; i<MPU_InitRegNum; i++) {
00085         WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
00086         wait(0.001);  //I2C must slow down the write speed, otherwise it won't work
00087     }
00088 
00089     set_acc_scale(BITS_FS_2G);
00090     set_gyro_scale(BITS_FS_1000DPS);
00091     
00092     //AK8963_calib_Magnetometer();  //Can't load this function here , strange problem?
00093     return 0;
00094 }
00095 
00096 bool mpu9250_spi::init2(int sample_rate_div,int low_pass_filter){
00097     uint8_t i = 0;
00098     uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
00099         {0x80, MPUREG_PWR_MGMT_1},     // Reset Device
00100         {0x01, MPUREG_PWR_MGMT_1},     // Clock Source
00101         {0x00, MPUREG_PWR_MGMT_2},     // Enable Acc & Gyro
00102         {0x01, MPUREG_CONFIG},         // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz //3
00103         {0x08, MPUREG_GYRO_CONFIG},    // +-1000dps = 10 / 500
00104         {0x00, MPUREG_ACCEL_CONFIG},   // +-2G
00105         {0x08, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 41Hz
00106         {0x30, MPUREG_INT_PIN_CFG},    //
00107         //{0x40, MPUREG_I2C_MST_CTRL},   // I2C Speed 348 kHz
00108         //{0x20, MPUREG_USER_CTRL},      // Enable AUX
00109         {0x20, MPUREG_USER_CTRL},       // I2C Master mode
00110         {0x0D, MPUREG_I2C_MST_CTRL}, //  I2C configuration multi-master  IIC 400KHz
00111         
00112         {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR},  //Set the I2C slave addres of AK8963 and set for write.
00113         //{0x09, MPUREG_I2C_SLV4_CTRL},
00114         //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
00115 
00116         {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
00117         {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
00118         {0x81, MPUREG_I2C_SLV0_CTRL},  //Enable I2C and set 1 byte
00119 
00120         {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
00121         {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
00122         {0x81, MPUREG_I2C_SLV0_CTRL}  //Enable I2C and set 1 byte
00123         
00124     };
00125     spi.format(8,0);
00126     spi.frequency(4000000);
00127 
00128     for(i=0; i<MPU_InitRegNum; i++) {
00129         WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
00130         wait(0.001);  //I2C must slow down the write speed, otherwise it won't work
00131     }
00132 
00133     set_acc_scale(BITS_FS_2G);
00134     set_gyro_scale(BITS_FS_1000DPS);
00135     
00136     //AK8963_calib_Magnetometer();  //Can't load this function here , strange problem?
00137     return 0;
00138 }
00139 /*-----------------------------------------------------------------------------------------------
00140                                 ACCELEROMETER SCALE
00141 usage: call this function at startup, after initialization, to set the right range for the
00142 accelerometers. Suitable ranges are:
00143 BITS_FS_2G
00144 BITS_FS_4G
00145 BITS_FS_8G
00146 BITS_FS_16G
00147 returns the range set (2,4,8 or 16)
00148 -----------------------------------------------------------------------------------------------*/
00149 unsigned int mpu9250_spi::set_acc_scale(int scale){
00150     unsigned int temp_scale;
00151     //WriteReg(MPUREG_ACCEL_CONFIG, scale);
00152     
00153     switch (scale){
00154         case BITS_FS_2G:
00155             acc_divider=16384;
00156         break;
00157         case BITS_FS_4G:
00158             acc_divider=8192;
00159         break;
00160         case BITS_FS_8G:
00161             acc_divider=4096;
00162         break;
00163         case BITS_FS_16G:
00164             acc_divider=2048;
00165         break;   
00166     }
00167     //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
00168     
00169     switch (temp_scale){
00170         case BITS_FS_2G:
00171             temp_scale=2;
00172         break;
00173         case BITS_FS_4G:
00174             temp_scale=4;
00175         break;
00176         case BITS_FS_8G:
00177             temp_scale=8;
00178         break;
00179         case BITS_FS_16G:
00180             temp_scale=16;
00181         break;   
00182     }
00183     return temp_scale;
00184 }
00185 
00186 
00187 /*-----------------------------------------------------------------------------------------------
00188                                 GYROSCOPE SCALE
00189 usage: call this function at startup, after initialization, to set the right range for the
00190 gyroscopes. Suitable ranges are:
00191 BITS_FS_250DPS
00192 BITS_FS_500DPS
00193 BITS_FS_1000DPS
00194 BITS_FS_2000DPS
00195 returns the range set (250,500,1000 or 2000)
00196 -----------------------------------------------------------------------------------------------*/
00197 unsigned int mpu9250_spi::set_gyro_scale(int scale){
00198     unsigned int temp_scale;
00199     //WriteReg(MPUREG_GYRO_CONFIG, scale);
00200     switch (scale){
00201         case BITS_FS_250DPS:
00202             gyro_divider=131;
00203         break;
00204         case BITS_FS_500DPS:
00205             gyro_divider=65.5;
00206         break;
00207         case BITS_FS_1000DPS:
00208             gyro_divider=32.8;
00209         break;
00210         case BITS_FS_2000DPS:
00211             gyro_divider=16.4;
00212         break;   
00213     }
00214     //temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
00215     switch (temp_scale){
00216         case BITS_FS_250DPS:
00217             temp_scale=250;
00218         break;
00219         case BITS_FS_500DPS:
00220             temp_scale=500;
00221         break;
00222         case BITS_FS_1000DPS:
00223             temp_scale=1000;
00224         break;
00225         case BITS_FS_2000DPS:
00226             temp_scale=2000;
00227         break;   
00228     }
00229     return temp_scale;
00230 }
00231 
00232 
00233 /*-----------------------------------------------------------------------------------------------
00234                                 WHO AM I?
00235 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
00236 mpu9250 which should be 104 when in SPI mode.
00237 returns the I2C address (104)
00238 -----------------------------------------------------------------------------------------------*/
00239 unsigned int mpu9250_spi::whoami(){
00240     unsigned int response;
00241     response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
00242     return response;
00243 }
00244 
00245 
00246 /*-----------------------------------------------------------------------------------------------
00247                                 READ ACCELEROMETER
00248 usage: call this function to read accelerometer data. Axis represents selected axis:
00249 0 -> X axis
00250 1 -> Y axis
00251 2 -> Z axis
00252 -----------------------------------------------------------------------------------------------*/
00253 void mpu9250_spi::read_acc()
00254 {
00255     uint8_t response[6];
00256     int16_t bit_data;
00257     float data;
00258     int i;
00259     ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
00260     for(i=0; i<3; i++) {
00261         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00262         data=(float)bit_data;
00263         accelerometer_data[i]=data/acc_divider;
00264     }
00265     
00266 }
00267 
00268 /*-----------------------------------------------------------------------------------------------
00269                                 READ GYROSCOPE
00270 usage: call this function to read gyroscope data. Axis represents selected axis:
00271 0 -> X axis
00272 1 -> Y axis
00273 2 -> Z axis
00274 -----------------------------------------------------------------------------------------------*/
00275 void mpu9250_spi::read_rot()
00276 {
00277     uint8_t response[6];
00278     int16_t bit_data;
00279     float data;
00280     int i;
00281     ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
00282     for(i=0; i<3; i++) {
00283         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00284         data=(float)bit_data;
00285         gyroscope_data[i]=data/gyro_divider;
00286     }
00287 
00288 }
00289 
00290 /*-----------------------------------------------------------------------------------------------
00291                                 READ TEMPERATURE
00292 usage: call this function to read temperature data. 
00293 returns the value in °C
00294 -----------------------------------------------------------------------------------------------*/
00295 void mpu9250_spi::read_temp(){
00296     uint8_t response[2];
00297     int16_t bit_data;
00298     float data;
00299     ReadRegs(MPUREG_TEMP_OUT_H,response,2);
00300 
00301     bit_data=((int16_t)response[0]<<8)|response[1];
00302     data=(float)bit_data;
00303     Temperature=(data/340)+36.53;
00304     deselect();
00305 }
00306 
00307 /*-----------------------------------------------------------------------------------------------
00308                                 READ ACCELEROMETER CALIBRATION
00309 usage: call this function to read accelerometer data. Axis represents selected axis:
00310 0 -> X axis
00311 1 -> Y axis
00312 2 -> Z axis
00313 returns Factory Trim value
00314 -----------------------------------------------------------------------------------------------*/
00315 void mpu9250_spi::calib_acc()
00316 {
00317     uint8_t response[4];
00318     int temp_scale;
00319     //READ CURRENT ACC SCALE
00320     temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
00321     set_acc_scale(BITS_FS_8G);
00322     //ENABLE SELF TEST need modify
00323     //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
00324 
00325     ReadRegs(MPUREG_SELF_TEST_X,response,4);
00326     calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
00327     calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
00328     calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
00329 
00330     set_acc_scale(temp_scale);
00331 }
00332 uint8_t mpu9250_spi::AK8963_whoami(){
00333     uint8_t response;
00334     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00335     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
00336     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
00337 
00338     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
00339     wait(0.001);
00340     response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00);    //Read I2C 
00341     //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
00342     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
00343 
00344     return response;
00345 }
00346 void mpu9250_spi::AK8963_calib_Magnetometer(){
00347     uint8_t response[3];
00348     float data;
00349     int i;
00350 
00351     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00352     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
00353     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
00354 
00355     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
00356     wait(0.001);
00357     //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00);    //Read I2C 
00358     ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
00359     
00360     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
00361     for(i=0; i<3; i++) {
00362         data=response[i];
00363         Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
00364     }
00365 }
00366 void mpu9250_spi::AK8963_read_Magnetometer(){
00367     uint8_t response[7];
00368     int16_t bit_data;
00369     float data;
00370     int i;
00371 
00372     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00373     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
00374     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
00375 
00376     wait(0.001);
00377     ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
00378     //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
00379     for(i=0; i<3; i++) {
00380         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00381         data=(float)bit_data;
00382         Magnetometer[i]=data*Magnetometer_ASA[i];
00383     }
00384 }
00385 void mpu9250_spi::read_all(){
00386     uint8_t response[21];
00387     int16_t bit_data;
00388     float data;
00389     int i;
00390         spi.format(8,0);
00391     spi.frequency(4000000);
00392     //Send I2C command at first
00393     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00394     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
00395     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
00396     //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
00397 
00398     //wait(0.001);
00399     ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
00400     //Get accelerometer value
00401     for(i=0; i<3; i++) {
00402         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00403         data=(float)bit_data;
00404         accelerometer_data[i]=data/16384;
00405     }
00406     //Get temperature
00407     bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00408     data=(float)bit_data;
00409     Temperature=((data-21)/333.87)+21;
00410     //Get gyroscop value
00411     for(i=4; i<7; i++) {
00412         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00413         data=(float)bit_data;
00414         gyroscope_data[i-4]=data/65.5; //32.8
00415     }
00416     //Get Magnetometer value
00417     for(i=7; i<10; i++) {
00418         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00419         data=(float)bit_data;
00420         Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
00421     }
00422 }
00423 
00424 /*-----------------------------------------------------------------------------------------------
00425                                 SPI SELECT AND DESELECT
00426 usage: enable and disable mpu9250 communication bus
00427 -----------------------------------------------------------------------------------------------*/
00428 void mpu9250_spi::select() {
00429     //Set CS low to start transmission (interrupts conversion)
00430     cs = 0;
00431 }
00432 void mpu9250_spi::deselect() {
00433     //Set CS high to stop transmission (restarts conversion)
00434     cs = 1;
00435 }
00436