Self test boot program for testing icarus sensors
Dependencies: BLE_API mbed nRF51822
Fork of BLE_UARTConsole by
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