forked

Fork of MPU9250_SPI by Mu kylong

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU9250.cpp Source File

MPU9250.cpp

00001 /*CODED by Qiyong Mu on 21/06/2014
00002 kylongmu@msn.com
00003 */
00004 
00005 #include <mbed.h>
00006 #include "MPU9250.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(50);
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         {low_pass_filter, MPUREG_CONFIG},         // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
00059         {0x18, MPUREG_GYRO_CONFIG},    // +-2000dps
00060         {0x08, MPUREG_ACCEL_CONFIG},   // +-4G
00061         {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
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(1000000);
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(2);
00090     set_gyro_scale(250);
00091     
00092     //AK8963_calib_Magnetometer();  //Can't load this function here , strange problem?
00093     return 0;
00094 }
00095 /*-----------------------------------------------------------------------------------------------
00096                                 ACCELEROMETER SCALE
00097 usage: call this function at startup, after initialization, to set the right range for the
00098 accelerometers. Suitable ranges are:
00099 BITS_FS_2G
00100 BITS_FS_4G
00101 BITS_FS_8G
00102 BITS_FS_16G
00103 returns the range set (2,4,8 or 16)
00104 -----------------------------------------------------------------------------------------------*/
00105 unsigned int mpu9250_spi::set_acc_scale(int scale){
00106     unsigned int temp_scale;
00107     WriteReg(MPUREG_ACCEL_CONFIG, scale);
00108     
00109     switch (scale){
00110         case BITS_FS_2G:
00111             acc_divider=16384;
00112         break;
00113         case BITS_FS_4G:
00114             acc_divider=8192;
00115         break;
00116         case BITS_FS_8G:
00117             acc_divider=4096;
00118         break;
00119         case BITS_FS_16G:
00120             acc_divider=2048;
00121         break;   
00122     }
00123     temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
00124     
00125     switch (temp_scale){
00126         case BITS_FS_2G:
00127             temp_scale=2;
00128         break;
00129         case BITS_FS_4G:
00130             temp_scale=4;
00131         break;
00132         case BITS_FS_8G:
00133             temp_scale=8;
00134         break;
00135         case BITS_FS_16G:
00136             temp_scale=16;
00137         break;   
00138     }
00139     return temp_scale;
00140 }
00141 
00142 
00143 /*-----------------------------------------------------------------------------------------------
00144                                 GYROSCOPE SCALE
00145 usage: call this function at startup, after initialization, to set the right range for the
00146 gyroscopes. Suitable ranges are:
00147 BITS_FS_250DPS
00148 BITS_FS_500DPS
00149 BITS_FS_1000DPS
00150 BITS_FS_2000DPS
00151 returns the range set (250,500,1000 or 2000)
00152 -----------------------------------------------------------------------------------------------*/
00153 unsigned int mpu9250_spi::set_gyro_scale(int scale){
00154     unsigned int temp_scale;
00155     WriteReg(MPUREG_GYRO_CONFIG, scale);
00156     switch (scale){
00157         case BITS_FS_250DPS:
00158             gyro_divider=131;
00159         break;
00160         case BITS_FS_500DPS:
00161             gyro_divider=65.5;
00162         break;
00163         case BITS_FS_1000DPS:
00164             gyro_divider=32.8;
00165         break;
00166         case BITS_FS_2000DPS:
00167             gyro_divider=16.4;
00168         break;   
00169     }
00170     temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
00171     switch (temp_scale){
00172         case BITS_FS_250DPS:
00173             temp_scale=250;
00174         break;
00175         case BITS_FS_500DPS:
00176             temp_scale=500;
00177         break;
00178         case BITS_FS_1000DPS:
00179             temp_scale=1000;
00180         break;
00181         case BITS_FS_2000DPS:
00182             temp_scale=2000;
00183         break;   
00184     }
00185     return temp_scale;
00186 }
00187 
00188 
00189 /*-----------------------------------------------------------------------------------------------
00190                                 WHO AM I?
00191 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
00192 mpu9250 which should be 104 when in SPI mode.
00193 returns the I2C address (104)
00194 -----------------------------------------------------------------------------------------------*/
00195 unsigned int mpu9250_spi::whoami(){
00196     unsigned int response;
00197     response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
00198     return response;
00199 }
00200 
00201 
00202 /*-----------------------------------------------------------------------------------------------
00203                                 READ ACCELEROMETER
00204 usage: call this function to read accelerometer data. Axis represents selected axis:
00205 0 -> X axis
00206 1 -> Y axis
00207 2 -> Z axis
00208 -----------------------------------------------------------------------------------------------*/
00209 void mpu9250_spi::read_acc()
00210 {
00211     uint8_t response[6];
00212     int16_t bit_data;
00213     float data;
00214     int i;
00215     ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
00216     for(i=0; i<3; i++) {
00217         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00218         data=(float)bit_data;
00219         accelerometer_data[i]=data/acc_divider;
00220     }
00221     
00222 }
00223 
00224 /*-----------------------------------------------------------------------------------------------
00225                                 READ GYROSCOPE
00226 usage: call this function to read gyroscope data. Axis represents selected axis:
00227 0 -> X axis
00228 1 -> Y axis
00229 2 -> Z axis
00230 -----------------------------------------------------------------------------------------------*/
00231 void mpu9250_spi::read_rot()
00232 {
00233     uint8_t response[6];
00234     int16_t bit_data;
00235     float data;
00236     int i;
00237     ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
00238     for(i=0; i<3; i++) {
00239         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00240         data=(float)bit_data;
00241         gyroscope_data[i]=data/gyro_divider;
00242     }
00243 
00244 }
00245 
00246 /*-----------------------------------------------------------------------------------------------
00247                                 READ TEMPERATURE
00248 usage: call this function to read temperature data. 
00249 returns the value in °C
00250 -----------------------------------------------------------------------------------------------*/
00251 void mpu9250_spi::read_temp(){
00252     uint8_t response[2];
00253     int16_t bit_data;
00254     float data;
00255     ReadRegs(MPUREG_TEMP_OUT_H,response,2);
00256 
00257     bit_data=((int16_t)response[0]<<8)|response[1];
00258     data=(float)bit_data;
00259     Temperature=((double)data/340)+36.53;
00260     deselect();
00261 }
00262 
00263 /*-----------------------------------------------------------------------------------------------
00264                                 READ ACCELEROMETER CALIBRATION
00265 usage: call this function to read accelerometer data. Axis represents selected axis:
00266 0 -> X axis
00267 1 -> Y axis
00268 2 -> Z axis
00269 returns Factory Trim value
00270 -----------------------------------------------------------------------------------------------*/
00271 void mpu9250_spi::calib_acc()
00272 {
00273     uint8_t response[4];
00274     int temp_scale;
00275     //READ CURRENT ACC SCALE
00276     temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
00277     set_acc_scale(BITS_FS_8G);
00278     //ENABLE SELF TEST need modify
00279     //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
00280 
00281     ReadRegs(MPUREG_SELF_TEST_X,response,4);
00282     calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
00283     calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
00284     calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
00285 
00286     set_acc_scale(temp_scale);
00287 }
00288 uint8_t mpu9250_spi::AK8963_whoami(){
00289     uint8_t response;
00290     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00291     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
00292     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
00293 
00294     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
00295     wait(0.001);
00296     response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00);    //Read I2C 
00297     //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
00298     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
00299 
00300     return response;
00301 }
00302 void mpu9250_spi::AK8963_calib_Magnetometer(){
00303     uint8_t response[3];
00304     float data;
00305     int i;
00306 
00307     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00308     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
00309     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
00310 
00311     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
00312     wait(0.001);
00313     //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00);    //Read I2C 
00314     ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
00315     
00316     //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
00317     for(i=0; i<3; i++) {
00318         data=response[i];
00319         Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
00320     }
00321 }
00322 void mpu9250_spi::AK8963_read_Magnetometer(){
00323     uint8_t response[7];
00324     int16_t bit_data;
00325     float data;
00326     int i;
00327 
00328     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00329     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
00330     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
00331 
00332     wait(0.001);
00333     ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
00334     //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.
00335     for(i=0; i<3; i++) {
00336         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00337         data=(float)bit_data;
00338         Magnetometer[i]=data*Magnetometer_ASA[i];
00339     }
00340 }
00341 void mpu9250_spi::read_all(){
00342     uint8_t response[21];
00343     int16_t bit_data;
00344     float data;
00345     int i;
00346 
00347     //Send I2C command at first
00348     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
00349     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
00350     WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
00351     //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.
00352 
00353     //wait(0.001);
00354     ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
00355     //Get accelerometer value
00356     for(i=0; i<3; i++) {
00357         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00358         data=(float)bit_data;
00359         accelerometer_data[i]=data/acc_divider;
00360     }
00361     //Get temperature
00362     bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00363     data=(float)bit_data;
00364     Temperature=(((double)data-21)/333.87)+21;
00365     //Get gyroscop value
00366     for(i=4; i<7; i++) {
00367         bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
00368         data=(float)bit_data;
00369         gyroscope_data[i-4]=data/gyro_divider;
00370     }
00371     //Get Magnetometer value
00372     for(i=7; i<10; i++) {
00373         bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
00374         data=(float)bit_data;
00375         Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
00376     }
00377 }
00378 
00379 /*-----------------------------------------------------------------------------------------------
00380                                 SPI SELECT AND DESELECT
00381 usage: enable and disable mpu9250 communication bus
00382 -----------------------------------------------------------------------------------------------*/
00383 void mpu9250_spi::select() {
00384     //Set CS low to start transmission (interrupts conversion)
00385     cs = 0;
00386 }
00387 void mpu9250_spi::deselect() {
00388     //Set CS high to stop transmission (restarts conversion)
00389     cs = 1;
00390 }