Diff: MPU9250Sensor.cpp

Revision:

14:cb369746225d

Parent:

13:ef0ce8fa871f

--- a/MPU9250Sensor.cpp	Sun Apr 05 09:54:04 2015 +0000
+++ b/MPU9250Sensor.cpp	Wed Apr 15 20:01:16 2015 +0000
@@ -5,13 +5,30 @@
 #define LOG(...) do printf(__VA_ARGS__); while (0)
 #endif
 
+#define SELF_TEST_COUNT 200
+
 MPU9250Sensor::MPU9250Sensor(SPI& spi_,DigitalOut& cs_,void (*debug_)(const char* format, ...)) : BaseSensor(debug_), spi(spi_), cs(cs_) {
    cs = UP;
     //To prevent switching into I2C mode when using SPI, the I2C interface should be disabled by setting the I2C_IF_DIS 
     //configuration bit. Setting this bit should be performed immediately after waiting for the time specified by the 
     //“Start-Up Time for Register Read/Write” in Section 6.3.
     
+    writeRegister(MPU9250_USER_CTRL,0x01,0x01,4);
     
+    //read Sensitivity Adjustment values
+    initMagnetometr();    
+}
+
+void MPU9250Sensor::initMagnetometr(){
+    uint8_t calibration[3];
+    //3. Set the compas into fuse mode
+    writeRegisterI2C(AK8963_CNTL, 0x0F);
+    
+    readRegistersI2C(AK8963_ASAX,3,calibration);
+    for (int i =0;i<3;i++){        
+        magnetometerCalibration[i]=(((float)calibration[i]-128.0)/256.0+1);
+        LOG("Magnet calib. %d: %+5.3f\r\n",i,magnetometerCalibration[i]);
+    }
 }
 
 char* MPU9250Sensor::getSimpleName() {
@@ -20,29 +37,23 @@
 
 
 uint32_t MPU9250Sensor::verifyIntegrity(uint32_t* errorResult) {
-    LOG("Start verfication of MPU9250 Sensor");
+    LOG("Start verfication of MPU9250 Sensor\r\n");
     uint32_t errors = 0;
     //who am I register value is 0x71
     uint8_t sensorId = readRegister(MPU9250_WHOAMI);
     
     if (sensorId !=0x71){
         errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
-        LOG("Wrong sensorId: %X",sensorId);
+        LOG("Wrong sensorId: %X\r\n",sensorId);
     }
     
-    //check status registry
-    uint8_t status  = readRegister(MPU9250_I2C_MST_STATUS);
-    
-    //indicate that SEU error was detetcted 
-    if (status & (1 << 7)){
-        errorResult[errors++] = ERROR_SEU_ERROR_DETECT;
-        LOG("SEU error detected: %X",status);        
+
+    //check magnetometer chip id  0x48
+    uint8_t magSensorId = readRegisterI2C(AK8963_WIA);
+    if (magSensorId !=0x48){
+        errorResult[errors++] = ERROR_WRONG_DEVICE_ID;
+        LOG("Wrong magnetometr sensorId: %X\r\n",magSensorId);
     }
-    //check that chip is in awaken state  
-    if (!(status & (1 << 6))){
-        errorResult[errors++] = ERROR_DEVICE_SLEEPING;
-        LOG("Chip not awaken: %X",status);
-    }    
     
     //perform self test
     errors+=selfTest(&errorResult[errors]); 
@@ -52,14 +63,291 @@
 
 uint32_t MPU9250Sensor::selfTest(uint32_t* errorResult){
     uint32_t errors = 0;
+    // gyroscope Self-Test
+    errors+=gyroscopeSelfTest(&errorResult[errors]); 
+    // accelerometer’s Self-Test
+    errors+=accelerometerSelfTest(&errorResult[errors]); 
+    // compass Self-Test
+    errors+=magnetometerSelfTest(&errorResult[errors]); 
+     
+    return errors;
+}
+
+
+uint32_t MPU9250Sensor::gyroscopeSelfTest(uint32_t* errorResult){
+    uint32_t errors = 0;
+    int16_t rotation_speed[3],rotation_speed_self_test[3];
+    uint8_t selfTest[3];
+    float self_test_otp[3],self_test_response[3],gyro_offset[3];
+
+    //1) The routine starts by measuring the digital output of all three gyroscopes. 
+    //In order to do this, the following registers are modified:
     
-     return errors;
+    // Change the digital low pass filter (DLPF) code to 2 (Register Address: 26 (1Ah)
+    //Bit [2:0] – USR). The following table details the configuration of the component when the
+    //DLPF is configured to 2.
+    writeRegister(MPU9250_CONFIG,0x02,0x03);
+ 
+    // Store the existing full scale range select code (Register Address: 27 (1Bh) Bit
+    //[4:3] – USR) as Old_FS, then select a full scale range of 250dps by setting the
+    //ACCEL_FS_SEL bits to 00.
+    writeRegister(MPU9250_GYRO_CONFIG,MPU9250_GFS_250DPS,0x03,3);
+    
+    //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
+    //AZ_OS in the software.
+    averageData(MPU9250_GYRO_XOUT_H,rotation_speed);
+    
+    //3) Set USR_Reg: (1Bh) Gyro_Config, gdrive_axisCTST [0-2] to b111 to enable Self-Test.
+    writeRegister(MPU9250_GYRO_CONFIG, 0x07,0x07,5); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
+    
+    //4) Wait 20ms for oscillations to stabilize 
+    wait_us(20);
+    
+    //5) Read the gyroscope  output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of GX_ST_OS, GY_ST_OS, GZ_ST_OS in the software.
+    averageData(MPU9250_GYRO_XOUT_H,rotation_speed_self_test);
+    
+    //6) Calculate the Self-Test response as follows:
+    
+    //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
+    //X-gyro: selftest1 (00): xg_st_data [0-7]
+    //Y-gyro: selftest1 (01): yg_st_data [0-7]
+    //Z-gyro: selftest1 (02): zg_st_data [0-7]
+    readRegisters(MPU9250_SELF_TEST_X_GYRO, 3, &selfTest[0]);
+    
+    //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
+    for (int i =0; i < 3; i++) { 
+        self_test_response[i] = (float)rotation_speed_self_test[i]-(float)rotation_speed[i];
+        self_test_otp[i] = (float)(2620.0f)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
+    }
+    
+    //3) Determine passing or failing Self-Test
+
+    
+    for (int i =0; i < 3; i++) {
+        if (self_test_otp[i]!=0){
+            //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
+            //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
+            if (self_test_response[i]/self_test_otp[i]<0.5F){
+                errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
+                LOG("3.a Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            }
+        } else {
+            //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
+            //and report Self-Test is passing if all the following criteria are fulfilled. 
+            if (abs((float)MPU9250_GFS_250DPS_MULTIPLIER*self_test_response[i])<60){
+                errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
+                LOG("3.b Gyro resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            } 
+        }
+    }
+    
+    //read offset
+    readGyroOffset(MPU9250_XG_OFFSET_H, &gyro_offset[0]);
+    
+    //c. If the Self-Test passes criteria (a) and (b), it’s necessary to check gyro offset values.
+    //Report passing Self-Test if the following criteria fulfilled.
+    for (int i =0; i < 3; i++) {
+        if (abs(gyro_offset[i]) < 20.0){
+                errorResult[errors++] = ERROR_GYRO_SELF_TEST_FAILED;
+                LOG("3.c Gyro offset %d: %+5.3f\r\n",i,gyro_offset[i]);
+        }
+    }
+    
+    //3.1. External Configuration Cleanup
+    writeRegister(MPU9250_GYRO_CONFIG, 0x00,0x07,5);
+    wait_us(20);
+    
+    return errors;
+}
+
+
+uint32_t MPU9250Sensor::accelerometerSelfTest(uint32_t* errorResult){
+    uint32_t errors = 0;
+    int16_t acceleration[3],acceleration_self_test[3];
+    uint8_t selfTest[3];
+    float self_test_otp[3],self_test_response[3];
+    
+     //1) The routine starts by measuring the digital output of all three gyroscopes. 
+    //In order to do this, the following registers are modified:
+    
+    //Change the DLPF Code to 2 (Register Address: 29 (1Dh) Bit [2:0] – USR).
+    //The following table details the configuration of the component when the DLPF is configured
+    //to 2.
+
+    writeRegister(MPU9250_ACCEL_CONFIG_2,0x02,0x03 );
+    
+    //Store the existing full scale range select code (Register Address: 28 (1Ch)
+    //Bit [4:3] – USR) as Old_FS, then select a full scale range of ±2g by setting the
+    //ACCEL_FS_SEL bits to 00.
+    writeRegister(MPU9250_ACCEL_CONFIG,MPU9250_AFS_2G,0x03,3);
+    
+    //2) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of GX_OS, GY_OS, GZ_OS, AX_OS, AY_OS and
+    //AZ_OS in the software.
+    averageData(MPU9250_ACCEL_XOUT_H,acceleration);
+    
+    //3) Set USR_Reg: (1Ch) Accel_Config, AX/Y/Z_ST_EN [0-2] to b111 to enable Self-Test.
+    writeRegister(MPU9250_ACCEL_CONFIG, 0x07,0x07,5); 
+    
+     //4) Wait 20ms for oscillations to stabilize 
+    wait_us(20);
+    
+    //5) Read the gyroscope and accelerometer output at a 1kHz rate and average 200 readings.
+    //The averaged values will be the LSB of AX_ST_OS, AY_ST_OS and AZ_ST_OS in the software.
+    averageData(MPU9250_ACCEL_XOUT_H,acceleration_self_test);
+    
+    //6) Calculate the Self-Test response as follows:
+    
+       //1) Retrieve factory Self-Test code (ST_Code) from USR_Reg in the software:
+    //X-gyro: selftest1 (00): xg_st_data [0-7]
+    //Y-gyro: selftest1 (01): yg_st_data [0-7]
+    //Z-gyro: selftest1 (02): zg_st_data [0-7]
+    readRegisters(MPU9250_SELF_TEST_X_ACCEL, 3, &selfTest[0]);
+    
+    //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
+    for (int i =0; i < 3; i++) { 
+        self_test_response[i] = (float)acceleration_self_test[i]-(float)acceleration[i];
+        self_test_otp[i] = (float)(2620.0)*(pow( 1.01 , ((float)selfTest[i] - 1.0) ));
+    }
+    
+    
+    //3) Determine passing or failing Self-Test
+    for (int i =0; i < 3; i++) {
+        if (self_test_otp[i]!=0){
+            //a. If factory Self-Test values ST_OTP≠0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the factory Self-Test values
+            //(ST_OTP) and report Self-Test is passing if all the following criteria are fulfilled
+            float ratio = self_test_response[i]/self_test_otp[i];
+            if (ratio<0.5F || ratio > 1.5f){
+                errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
+                LOG("3.a Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            }
+        } else {
+            //b. If factory Self-Test values ST_OTP=0, compare the current Self-Test response
+            //(GXST, GYST, GZST, AXST, AYST and AZST) to the ST absolute limits (ST_AL)
+            //and report Self-Test is passing if all the following criteria are fulfilled. 
+            float response=MPU9250_AFS_2G_MULTIPLIER*self_test_response[i];
+            if (abs(response)<0.225f || abs(response)>0.675f ){
+                errorResult[errors++] = ERROR_ACCE_SELF_TEST_FAILED;
+                LOG("3.b Acc resp %d: %+5.3f\r\n",i,self_test_response[i]);
+            } 
+        }
+    }
+    
+    
+    //3.1. External Configuration Cleanup
+    writeRegister(MPU9250_ACCEL_CONFIG, 0x00,0x07,5);
+    wait_us(20);
+    
+    return errors;
+}
+
+uint32_t MPU9250Sensor::magnetometerSelfTest(uint32_t* errorResult){    
+    uint32_t errors = 0;
+    uint8_t rawData[7];
+    int16_t magnetometer[3];
+    float self_test_response[3];
+    //1. Set the compass into power-down mode.
+    writeRegisterI2C(AK8963_CNTL, 0x00);
+    
+    //2. Write “1” to the SELF bit of the ASTC register. Other bits in this register should be set to zero.
+    writeRegisterI2C(AK8963_ASTC, 0x01,0x01,6);
+    
+    //3. Set the self test mode in the “Mode” register.
+    writeRegisterI2C(AK8963_CNTL, 0x08);
+    
+    //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.
+     while (!(readRegisterI2C(AK8963_ST1) & 0x01)){
+          wait_us(1);
+     }
+          
+     //5. Read the measurement data in the compass measurement data registers.
+     readRegistersI2C(MPU9250_EXT_SENS_DATA_00, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
+    uint8_t c = rawData[6]; // End data read by reading ST2 register
+    if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
+        magnetometer[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
+        magnetometer[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ;  // Data stored as little Endian
+        magnetometer[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 
+    } else {
+        magnetometer[0] = 0xffff;
+        magnetometer[1] = 0xffff;
+        magnetometer[2] = 0xffff;
+    }
+    
+    //6. Write “0” to SELF bit of the ASTC register.
+    writeRegisterI2C(AK8963_ASTC, 0x00,0x01,6);
+              
+    //7. Set the compass to power-down mode.
+    writeRegisterI2C(AK8963_CNTL, 0x00);
+    
+        //2) Calculate factory Self-Test value (ST_OTP) based on the following equation:    
+    for (int i =0; i < 3; i++) { 
+        self_test_response[i] = (float)magnetometer[i]*magnetometerCalibration[i]*MPU9250M_4800uT;
+    }
+    
+    if(!(c & 0x10)) {
+        //Set Pass/Fail Criteria 16bit
+        if (self_test_response[0]<-200.0 || self_test_response[0] > 200.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
+        }
+        if (self_test_response[1]<-200.0 || self_test_response[1] > 200.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
+        }
+        if (self_test_response[2]<-3200.0 || self_test_response[2] > 800.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
+        }
+    } else {
+        //Set Pass/Fail Criteria 14bit
+        if (self_test_response[0]<-50.0 || self_test_response[0] > 50.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[0]);
+        }
+        if (self_test_response[1]<-50.0 || self_test_response[1] > 50.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",1,self_test_response[1]);
+        }
+        if (self_test_response[2]<-800.0 || self_test_response[2] > 200.0){
+                    errorResult[errors++] = ERROR_MAGN_SELF_TEST_FAILED;
+                    LOG("Magn resp %d: %+5.3f\r\n",0,self_test_response[2]);
+        }
+    }
+                     
+    return errors;
 }
 
 void MPU9250Sensor::getSensorDetails(sensor_t* sensorDetails) {
 
 }
 
+void MPU9250Sensor::readGyroOffset(uint8_t reg, float* data){
+    uint8_t rawData[6];
+    readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+    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
+    data[1] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+    data[2] = MPU9250_GFS_500DPS_MULTIPLIER*(int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+}
+
+void MPU9250Sensor::averageData(uint8_t reg,int16_t* data){
+    uint8_t rawData[6];
+    for( int i = 0; i < SELF_TEST_COUNT; i++) { // get average current values of gyro
+        readRegisters(reg, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+        data[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+        data[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+        data[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+    }
+    
+    for (int i =0; i < 3; i++) { // Get average of 200 values and store as average current readings
+        data[i] /= SELF_TEST_COUNT;
+    }
+}
+
 uint8_t MPU9250Sensor::readRegister( uint8_t reg){
     cs = DOWN;
     spi.write(reg| MPU9250_READ_FLAG);
@@ -67,10 +355,68 @@
     cs = UP;
     return val;
 }
-//
-//void MPU9250Sensor::writeRegister( uint8_t reg, uint8_t cmd ){
-//    cs->write(DOWN);
-//    spi->write(reg);
-//    spi->write(cmd);
-//    cs->write(UP);
-//}
+
+uint8_t MPU9250Sensor::readRegisterI2C(uint8_t reg){
+    cs = DOWN;
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
+    spi.write(MPU9250_I2C_SLV0_REG);
+    spi.write(reg);
+    spi.write(MPU9250_I2C_SLV0_CTRL);
+    spi.write(0x81);
+    spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
+    uint8_t val = spi.write(0x00);
+    cs = UP;
+    return val;
+}
+
+void MPU9250Sensor::readRegisters(uint8_t reg, uint8_t count, uint8_t * dest){
+    cs = DOWN;
+    spi.write(reg| MPU9250_READ_FLAG);
+    for(int i = 0; i < count; i++) {
+        dest[i] = spi.write(0x00);
+    }
+    cs = UP;
+}
+
+void MPU9250Sensor::readRegistersI2C(uint8_t reg, uint8_t count, uint8_t * dest){
+    cs = DOWN;
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);
+    spi.write(MPU9250_I2C_SLV0_REG);
+    spi.write(reg);
+    spi.write(MPU9250_I2C_SLV0_CTRL);
+    spi.write(0x80| count);
+    spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
+    for(int i = 0; i < count; i++) {
+        dest[i] = spi.write(0x00);
+    }
+    cs = UP;
+}
+
+void MPU9250Sensor::writeRegister( uint8_t reg,  uint8_t data, uint8_t mask,uint8_t pos){
+    cs = DOWN;
+    spi.write(reg| MPU9250_READ_FLAG);
+    uint8_t val = spi.write(0x00);
+    spi.write(reg);
+    spi.write(val & ~(mask<<pos) | (data<<pos));
+    cs = UP;
+}
+
+void MPU9250Sensor::writeRegisterI2C( uint8_t reg,  uint8_t data, uint8_t mask,uint8_t pos){
+    uint8_t val;
+    cs = DOWN;
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR|MPU9250_READ_FLAG);    
+    spi.write(MPU9250_I2C_SLV0_REG);
+    spi.write(reg);
+    spi.write(MPU9250_I2C_SLV0_CTRL);
+    spi.write(0x81);
+    spi.write(MPU9250_EXT_SENS_DATA_00| MPU9250_READ_FLAG);
+    val = spi.write(0x00);
+    spi.write(MPU9250_I2C_SLV0_ADDR);
+    spi.write(AK8963_I2C_ADDR);
+    spi.write(MPU9250_I2C_SLV0_DO);
+    val = spi.write(val & ~(mask<<pos) | (data<<pos));
+    cs = UP;
+}
\ No newline at end of file