MPU9250Sensor.cpp

00001 #include "MPU9250Sensor.h"
00002 #include "mbed.h"
00003 
00004 #ifndef LOG
00005 #define LOG(...) do printf(__VA_ARGS__); while (0)
00006 #endif
00007 
00008 #define SELF_TEST_COUNT 200
00009 
00010 MPU9250Sensor::MPU9250Sensor(SPI& spi_,DigitalOut& cs_,void (*debug_)(const char* format, ...)) : BaseSensor(debug_), spi(spi_), cs(cs_) {
00011    cs = UP;
00012     //To prevent switching into I2C mode when using SPI, the I2C interface should be disabled by setting the I2C_IF_DIS 
00013     //configuration bit. Setting this bit should be performed immediately after waiting for the time specified by the 
00014     //“Start-Up Time for Register Read/Write” in Section 6.3.
00015     
00016     writeRegister(MPU9250_USER_CTRL,0x01,0x01,4);
00017     
00018     //read Sensitivity Adjustment values
00019     initMagnetometr();    
00020 }
00021 
00022 void MPU9250Sensor::initMagnetometr(){
00023     uint8_t calibration[3];
00024     //3. Set the compas into fuse mode
00025     writeRegisterI2C(AK8963_CNTL, 0x0F);
00026     
00027     readRegistersI2C(AK8963_ASAX,3,calibration);
00028     for (int i =0;i<3;i++){        
00029         magnetometerCalibration[i]=(((float)calibration[i]-128.0)/256.0+1);
00030         LOG("Magnet calib. %d: %+5.3f\r\n",i,magnetometerCalibration[i]);
00031     }
00032 }
00033 
00034 char* MPU9250Sensor::getSimpleName() {
00035     return "MPU9250";
00036 }
00037 
00038 
00039 uint32_t MPU9250Sensor::verifyIntegrity(uint32_t* errorResult) {
00040     LOG("Start verfication of MPU9250 Sensor\r\n");
00041     uint32_t errors = 0;
00042     //who am I register value is 0x71
00043     uint8_t sensorId = readRegister(MPU9250_WHOAMI);
00044     
00045     if (sensorId !=0x71){
00046         errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
00047         LOG("Wrong sensorId: %X\r\n",sensorId);
00048     }
00049     
00050 
00051     //check magnetometer chip id  0x48
00052     uint8_t magSensorId = readRegisterI2C(AK8963_WIA);
00053     if (magSensorId !=0x48){
00054         errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
00055         LOG("Wrong magnetometr sensorId: %X\r\n",magSensorId);
00056     }
00057     
00058     //perform self test
00059     errors+=selfTest(&errorResult[errors]); 
00060     
00061     return errors;
00062 }
00063 
00064 uint32_t MPU9250Sensor::selfTest(uint32_t* errorResult){
00065     uint32_t errors = 0;
00066     // gyroscope Self-Test
00067     errors+=gyroscopeSelfTest(&errorResult[errors]); 
00068     // accelerometer’s Self-Test
00069     errors+=accelerometerSelfTest(&errorResult[errors]); 
00070     // compass Self-Test
00071     errors+=magnetometerSelfTest(&errorResult[errors]); 
00072      
00073     return errors;
00074 }
00075 
00076 
00077 uint32_t MPU9250Sensor::gyroscopeSelfTest(uint32_t* errorResult){
00078     uint32_t errors = 0;
00079     int16_t rotation_speed[3],rotation_speed_self_test[3];
00080     uint8_t selfTest[3];
00081     float self_test_otp[3],self_test_response[3],gyro_offset[3];
00082 
00083     //1) The routine starts by measuring the digital output of all three gyroscopes. 
00084     //In order to do this, the following registers are modified:
00085     
00086     // Change the digital low pass filter (DLPF) code to 2 (Register Address: 26 (1Ah)
00087     //Bit [2:0] – USR). The following table details the configuration of the component when the
00088     //DLPF is configured to 2.
00089     writeRegister(MPU9250_CONFIG,0x02,0x03);
00090  
00091     // Store the existing full scale range select code (Register Address: 27 (1Bh) Bit
00092     //[4:3] – USR) as Old_FS, then select a full scale range of 250dps by setting the
00093     //ACCEL_FS_SEL bits to 00.
00094     writeRegister(MPU9250_GYRO_CONFIG,MPU9250_GFS_250DPS,0x03,3);
00095     
00096     //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
00097     //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
00098     //AZ_OS in the software.
00099     averageData(MPU9250_GYRO_XOUT_H,rotation_speed);
00100     
00101     //3) Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b111 to enable Self-Test.
00102     writeRegister(MPU9250_GYRO_CONFIG, 0x07,0x07,5); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
00103     
00104     //4) Wait 20ms for oscillations to stabilize 
00105     wait_us(20);
00106     
00107     //5) Read the gyroscope  output at a 1kHz rate and average 200 readings.
00108     //The averaged values will be the LSB of GX_ST_OS, GY_ST_OS, GZ_ST_OS in the software.
00109     averageData(MPU9250_GYRO_XOUT_H,rotation_speed_self_test);
00110     
00111     //6) Calculate the Self-Test response as follows:
00112     
00113     //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
00114     //X-gyro: selftest1 (00): xg_st_data [0-7]
00115     //Y-gyro: selftest1 (01): yg_st_data [0-7]
00116     //Z-gyro: selftest1 (02): zg_st_data [0-7]
00117     readRegisters(MPU9250_SELF_TEST_X_GYRO, 3, &selfTest[0]);
00118     
00119     //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
00120     for (int i =0; i < 3; i++) { 
00121         self_test_response[i] = (float)rotation_speed_self_test[i]-(float)rotation_speed[i];
00122         self_test_otp[i] = (float)(2620.0f)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
00123     }
00124     
00125     //3) Determine passing or failing Self-Test
00126 
00127     
00128     for (int i =0; i < 3; i++) {
00129         if (self_test_otp[i]!=0){
00130             //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
00131             //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
00132             //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
00133             if (self_test_response[i]/self_test_otp[i]<0.5F){
00134                 errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
00135                 LOG("3.a Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
00136             }
00137         } else {
00138             //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
00139             //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
00140             //and report Self-Test is passing if all the following criteria are fulfilled. 
00141             if (abs((float)MPU9250_GFS_250DPS_MULTIPLIER*self_test_response[i])<60){
00142                 errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
00143                 LOG("3.b Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
00144             } 
00145         }
00146     }
00147     
00148     //read offset
00149     readGyroOffset(MPU9250_XG_OFFSET_H, &gyro_offset[0]);
00150     
00151     //c. If the Self-Test passes criteria (a) and (b), it’s necessary to check gyro offset values.
00152     //Report passing Self-Test if the following criteria fulfilled.
00153     for (int i =0; i < 3; i++) {
00154         if (abs(gyro_offset[i]) < 20.0){
00155                 errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
00156                 LOG("3.c Gyro offset %d: %+5.3f\r\n",i,gyro_offset[i]);
00157         }
00158     }
00159     
00160     //3.1. External Configuration Cleanup
00161     writeRegister(MPU9250_GYRO_CONFIG, 0x00,0x07,5);
00162     wait_us(20);
00163     
00164     return errors;
00165 }
00166 
00167 
00168 uint32_t MPU9250Sensor::accelerometerSelfTest(uint32_t* errorResult){
00169     uint32_t errors = 0;
00170     int16_t acceleration[3],acceleration_self_test[3];
00171     uint8_t selfTest[3];
00172     float self_test_otp[3],self_test_response[3];
00173     
00174      //1) The routine starts by measuring the digital output of all three gyroscopes. 
00175     //In order to do this, the following registers are modified:
00176     
00177     //Change the DLPF Code to 2 (Register Address: 29 (1Dh) Bit [2:0] – USR).
00178     //The following table details the configuration of the component when the DLPF is configured
00179     //to 2.
00180 
00181     writeRegister(MPU9250_ACCEL_CONFIG_2,0x02,0x03 );
00182     
00183     //Store the existing full scale range select code (Register Address: 28 (1Ch)
00184     //Bit [4:3] – USR) as Old_FS, then select a full scale range of ±2g by setting the
00185     //ACCEL_FS_SEL bits to 00.
00186     writeRegister(MPU9250_ACCEL_CONFIG,MPU9250_AFS_2G,0x03,3);
00187     
00188     //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
00189     //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
00190     //AZ_OS in the software.
00191     averageData(MPU9250_ACCEL_XOUT_H,acceleration);
00192     
00193     //3) Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b111 to enable Self-Test.
00194     writeRegister(MPU9250_ACCEL_CONFIG, 0x07,0x07,5); 
00195     
00196      //4) Wait 20ms for oscillations to stabilize 
00197     wait_us(20);
00198     
00199     //5) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
00200     //The averaged values will be the LSB of AX_ST_OS, AY_ST_OS and AZ_ST_OS in the software.
00201     averageData(MPU9250_ACCEL_XOUT_H,acceleration_self_test);
00202     
00203     //6) Calculate the Self-Test response as follows:
00204     
00205        //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
00206     //X-gyro: selftest1 (00): xg_st_data [0-7]
00207     //Y-gyro: selftest1 (01): yg_st_data [0-7]
00208     //Z-gyro: selftest1 (02): zg_st_data [0-7]
00209     readRegisters(MPU9250_SELF_TEST_X_ACCEL, 3, &selfTest[0]);
00210     
00211     //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
00212     for (int i =0; i < 3; i++) { 
00213         self_test_response[i] = (float)acceleration_self_test[i]-(float)acceleration[i];
00214         self_test_otp[i] = (float)(2620.0)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
00215     }
00216     
00217     
00218     //3) Determine passing or failing Self-Test
00219     for (int i =0; i < 3; i++) {
00220         if (self_test_otp[i]!=0){
00221             //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
00222             //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
00223             //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
00224             float ratio = self_test_response[i]/self_test_otp[i];
00225             if (ratio<0.5F || ratio > 1.5f){
00226                 errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
00227                 LOG("3.a Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
00228             }
00229         } else {
00230             //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
00231             //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
00232             //and report Self-Test is passing if all the following criteria are fulfilled. 
00233             float response=MPU9250_AFS_2G_MULTIPLIER*self_test_response[i];
00234             if (abs(response)<0.225f || abs(response)>0.675f ){
00235                 errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
00236                 LOG("3.b Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
00237             } 
00238         }
00239     }
00240     
00241     
00242     //3.1. External Configuration Cleanup
00243     writeRegister(MPU9250_ACCEL_CONFIG, 0x00,0x07,5);
00244     wait_us(20);
00245     
00246     return errors;
00247 }
00248 
00249 uint32_t MPU9250Sensor::magnetometerSelfTest(uint32_t* errorResult){    
00250     uint32_t errors = 0;
00251     uint8_t rawData[7];
00252     int16_t magnetometer[3];
00253     float self_test_response[3];
00254     //1. Set the compass into power-down mode.
00255     writeRegisterI2C(AK8963_CNTL, 0x00);
00256     
00257     //2. Write “1” to the SELF bit of the ASTC register. Other bits in this register should be set to zero.
00258     writeRegisterI2C(AK8963_ASTC, 0x01,0x01,6);
00259     
00260     //3. Set the self test mode in the “Mode” register.
00261     writeRegisterI2C(AK8963_CNTL, 0x08);
00262     
00263     //4. Check if data is ready or not by polling the DRDY bit of the ST1 register. When the data is ready, proceed to step 5.
00264      while (!(readRegisterI2C(AK8963_ST1) & 0x01)){
00265           wait_us(1);
00266      }
00267           
00268      //5. Read the measurement data in the compass measurement data registers.
00269      readRegistersI2C(MPU9250_EXT_SENS_DATA_00, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
00270     uint8_t c = rawData[6]; // End data read by reading ST2 register
00271     if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
00272         magnetometer[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
00273         magnetometer[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ;  // Data stored as little Endian
00274         magnetometer[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 
00275     } else {
00276         magnetometer[0] = 0xffff;
00277         magnetometer[1] = 0xffff;
00278         magnetometer[2] = 0xffff;
00279     }
00280     
00281     //6. Write “0” to SELF bit of the ASTC register.
00282     writeRegisterI2C(AK8963_ASTC, 0x00,0x01,6);
00283               
00284     //7. Set the compass to power-down mode.
00285     writeRegisterI2C(AK8963_CNTL, 0x00);
00286     
00287         //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
00288     for (int i =0; i < 3; i++) { 
00289         self_test_response[i] = (float)magnetometer[i]*magnetometerCalibration[i]*MPU9250M_4800uT;
00290     }
00291     
00292     if(!(c & 0x10)) {
00293         //Set Pass/Fail Criteria 16bit
00294         if (self_test_response[0]<-200.0 || self_test_response[0] > 200.0){
00295                     errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
00296                     LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
00297         }
00298         if (self_test_response[1]<-200.0 || self_test_response[1] > 200.0){
00299                     errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
00300                     LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
00301         }
00302         if (self_test_response[2]<-3200.0 || self_test_response[2] > 800.0){
00303                     errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
00304                     LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
00305         }
00306     } else {
00307         //Set Pass/Fail Criteria 14bit
00308         if (self_test_response[0]<-50.0 || self_test_response[0] > 50.0){
00309                     errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
00310                     LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
00311         }
00312         if (self_test_response[1]<-50.0 || self_test_response[1] > 50.0){
00313                     errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
00314                     LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
00315         }
00316         if (self_test_response[2]<-800.0 || self_test_response[2] > 200.0){
00317                     errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
00318                     LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
00319         }
00320     }
00321                      
00322     return errors;
00323 }
00324 
00325 void MPU9250Sensor::getSensorDetails(sensor_t* sensorDetails) {
00326 
00327 }
00328 
00329 void MPU9250Sensor::readGyroOffset(uint8_t reg, float* data){
00330     uint8_t rawData[6];
00331     readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
00332     data[0] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
00333     data[1] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
00334     data[2] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
00335 }
00336 
00337 void MPU9250Sensor::averageData(uint8_t reg,int16_t* data){
00338     uint8_t rawData[6];
00339     for( int i = 0; i < SELF_TEST_COUNT; i++) { // get average current values of gyro
00340         readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
00341         data[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
00342         data[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
00343         data[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
00344     }
00345     
00346     for (int i =0; i < 3; i++) { // Get average of 200 values and store as average current readings
00347         data[i] /= SELF_TEST_COUNT;
00348     }
00349 }
00350 
00351 uint8_t MPU9250Sensor::readRegister( uint8_t reg){
00352     cs = DOWN;
00353     spi.write(reg| MPU9250_READ_FLAG);
00354     uint8_t val = spi.write(0x00);
00355     cs = UP;
00356     return val;
00357 }
00358 
00359 uint8_t MPU9250Sensor::readRegisterI2C(uint8_t reg){
00360     cs = DOWN;
00361     spi.write(MPU9250_I2C_SLV0_ADDR);
00362     spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
00363     spi.write(MPU9250_I2C_SLV0_REG);
00364     spi.write(reg);
00365     spi.write(MPU9250_I2C_SLV0_CTRL);
00366     spi.write(0x81);
00367     spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
00368     uint8_t val = spi.write(0x00);
00369     cs = UP;
00370     return val;
00371 }
00372 
00373 void MPU9250Sensor::readRegisters(uint8_t reg, uint8_t count, uint8_t * dest){
00374     cs = DOWN;
00375     spi.write(reg| MPU9250_READ_FLAG);
00376     for(int i = 0; i < count; i++) {
00377         dest[i] = spi.write(0x00);
00378     }
00379     cs = UP;
00380 }
00381 
00382 void MPU9250Sensor::readRegistersI2C(uint8_t reg, uint8_t count, uint8_t * dest){
00383     cs = DOWN;
00384     spi.write(MPU9250_I2C_SLV0_ADDR);
00385     spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
00386     spi.write(MPU9250_I2C_SLV0_REG);
00387     spi.write(reg);
00388     spi.write(MPU9250_I2C_SLV0_CTRL);
00389     spi.write(0x80| count);
00390     spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
00391     for(int i = 0; i < count; i++) {
00392         dest[i] = spi.write(0x00);
00393     }
00394     cs = UP;
00395 }
00396 
00397 void MPU9250Sensor::writeRegister( uint8_t reg,  uint8_t data, uint8_t mask,uint8_t pos){
00398     cs = DOWN;
00399     spi.write(reg| MPU9250_READ_FLAG);
00400     uint8_t val = spi.write(0x00);
00401     spi.write(reg);
00402     spi.write(val & ~(mask<<pos) | (data<<pos));
00403     cs = UP;
00404 }
00405 
00406 void MPU9250Sensor::writeRegisterI2C( uint8_t reg,  uint8_t data, uint8_t mask,uint8_t pos){
00407     uint8_t val;
00408     cs = DOWN;
00409     spi.write(MPU9250_I2C_SLV0_ADDR);
00410     spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);    
00411     spi.write(MPU9250_I2C_SLV0_REG);
00412     spi.write(reg);
00413     spi.write(MPU9250_I2C_SLV0_CTRL);
00414     spi.write(0x81);
00415     spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
00416     val = spi.write(0x00);
00417     spi.write(MPU9250_I2C_SLV0_ADDR);
00418     spi.write(AK8963_I2C_ADDR);
00419     spi.write(MPU9250_I2C_SLV0_DO);
00420     val = spi.write(val & ~(mask<<pos) | (data<<pos));
00421     cs = UP;
00422 }