MPU9250_SPI

/*********************************************************************
Function : InitialSensor
Purpose : initialization
Parameter : none
Return value : none
*********************************************************************/
void InitialSensor(int sample_rate_div,int low_pass_filter)
{
  U8 MPU_Init_Data[][3] = {
        //{PWR_MGMT_1,        0x80, 1},     // Reset
        {PWR_MGMT_1,        0x01, 100},     // Clock Source
        {PWR_MGMT_2,        0x00, 100},     // Enable Acc & Gyro
        {CONFIG,            low_pass_filter, 100},
        {GYRO_CONFIG,       0x18, 100},
        {ACCEL_CONFIG,      0x08, 100},
        {ACCEL_CONFIG_2,    0x09, 100},
        {INT_PIN_CFG,       0x22, 100},

        {USER_CTRL,         0x20, 100},
        {I2C_MST_CTRL,      0x0D, 100},

        {I2C_SLV0_ADDR, AK8963_I2C_ADDR, 100},

        {I2C_SLV0_REG, AK8963_CNTL2, 100},
        {I2C_SLV0_DO,       0x01, 100},
        {I2C_SLV0_CTRL,     0x81, 100},

        {I2C_SLV0_REG, AK8963_CNTL1, 100},
        {I2C_SLV0_DO,       0x12, 100},
        {I2C_SLV0_CTRL,     0x81, 100}
};

    U8 i,j,u8InitCnt, id;
    U8 u8len = sizeof(MPU_Init_Data) / 3;
    unsigned char testAcc, testGyro;

    if(g_Main.SysPar.SysID > 0)
    {
        u8InitCnt = 1;
    }
    else
    {
        u8InitCnt = 18;
    }

    SPI_CLK_SLOW();

    for(j=0; j<u8InitCnt; j++)
    {
        for(i=0; i<u8len; i++)
        {
            WriteReg(j,MPU_Init_Data[i][0], MPU_Init_Data[i][1]);
            MPU_Init_Data[i][1] = 0;
            // delay
            Delayms(MPU_Init_Data[i][2]);
        }
        calib_mag(j);
        set_acc_scale(j, BITS_FS_4G);
        set_gyro_scale(j, BITS_FS_2000DPS);
        id = AK8963_whoami(j);   //Only read 0x00
    }
}

/*********************************************************************
Function : set_acc_scale
Purpose : MPU9250's set acc scale
Parameter :  int value
Return value : none
*********************************************************************/
unsigned int set_acc_scale(U8 num, int scale)
{
  unsigned int temp_scale;
    WriteReg(num, ACCEL_CONFIG, scale);

    switch (scale){
        case BITS_FS_2G:
            g_Main.acc_divider=16384;
        break;
        case BITS_FS_4G:
            g_Main.acc_divider=8192;
        break;
        case BITS_FS_8G:
            g_Main.acc_divider=4096;
        break;
        case BITS_FS_16G:
            g_Main.acc_divider=2048;
        break;
    }
    temp_scale = ReadReg(num, ACCEL_CONFIG);

    switch (temp_scale){
        case BITS_FS_2G:
            temp_scale=2;
        break;
        case BITS_FS_4G:
            temp_scale=4;
        break;
        case BITS_FS_8G:
            temp_scale=8;
        break;
        case BITS_FS_16G:
            temp_scale=16;
        break;
    }
    return temp_scale;
}

/*-----------------------------------------------------------------------------------------------
                                GYROSCOPE SCALE
usage: call this function at startup, after initialization, to set the right range for the
gyroscopes. Suitable ranges are:
BITS_FS_250DPS
BITS_FS_500DPS
BITS_FS_1000DPS
BITS_FS_2000DPS
returns the range set (250,500,1000 or 2000)
-----------------------------------------------------------------------------------------------*/
unsigned int set_gyro_scale(U8 num, int scale){
    unsigned int temp_scale;
     WriteReg(num, GYRO_CONFIG, scale);
    switch (scale){
        case BITS_FS_250DPS:
            g_Main.gyro_divider=131;
        break;
        case BITS_FS_500DPS:
            g_Main.gyro_divider=65.5;
        break;
        case BITS_FS_1000DPS:
            g_Main.gyro_divider=32.8;
        break;
        case BITS_FS_2000DPS:
            g_Main.gyro_divider=16.4;
        break;
    }
    //temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);

    temp_scale = ReadReg(num, GYRO_CONFIG);
    switch (temp_scale){
        case BITS_FS_250DPS:
            temp_scale=250;
        break;
        case BITS_FS_500DPS:
            temp_scale=500;
        break;
        case BITS_FS_1000DPS:
            temp_scale=1000;
        break;
        case BITS_FS_2000DPS:
            temp_scale=2000;
        break;
    }
    return temp_scale;
}

/*********************************************************************
Function : MPU9250::AK8963_whoami
Purpose : MPU9250's Calibration
Parameter :  dest1(Acc), dest2(Gyro)
Return value : none
*********************************************************************/
uint8_t AK8963_whoami(U8 num){
    uint8_t response;
    WriteReg(num, I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
    WriteReg(num, I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
    WriteReg(num, I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer

    //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
    Delayms(100);
    WriteReg(num, EXT_SENS_DATA_00|READ_FLAG, 0x00);    //Read I2C
    response = ReadReg(num, EXT_SENS_DATA_00);
    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C

    return response;
}

/*********************************************************************
Function : calib_mag
Purpose : MPU9250's Magnetometer Calibration
Parameter :  none
Return value : none
*********************************************************************/
void calib_mag(U8 num){
    uint8_t response[3];
    float data;
    int i;

    WriteReg(num, I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
    WriteReg(num, I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
    WriteReg(num, I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer

    //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
    //delayMicroseconds(1000);
    //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00);    //Read I2C

    CS_Func(LOW,num);
    if(num == 0)
    {
        UCB1BurstRead(EXT_SENS_DATA_00, 3, &response[0]); // read FIFO sample count
    }
    else
    {
        UCA0BurstRead(EXT_SENS_DATA_00, 3, &response[0]);
    }
    CS_Func(HIGH,num);
    //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);

    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C
    for(i = 0; i < 3; i++) {
        data=response[i];
        g_Main.Magnetometer_ASA[i] = ((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
    }
}