File content as of revision 14:cb369746225d:

#include "MPU9250Sensor.h"
#include "mbed.h"

#ifndef LOG
#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() {
    return "MPU9250";
}


uint32_t MPU9250Sensor::verifyIntegrity(uint32_t* errorResult) {
    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\r\n",sensorId);
    }
    

    //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);
    }
    
    //perform self test
    errors+=selfTest(&errorResult[errors]); 
    
    return errors;
}

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:
    
    // 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);
    uint8_t val = spi.write(0x00);
    cs = UP;
    return val;
}

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;
}