File content as of revision 0:35bba382318b:

#include "imu.h"

I2C i2c(I2C_SDA,I2C_SCL);

/**
  * @brief  invSqrt
  * @param  
  * @retval 
  */
float IMU::invSqrt(float x) 
{
    float halfx = 0.5f * x;
    float y = x;
    
    long i = *(long*)&y;                //get bits for floating value
    i = 0x5f3759df - (i >> 1);          //gives initial guss you
    y = *(float*)&i;                    //convert bits back to float
    y = y * (1.5f - (halfx * y * y));   //newtop step, repeating increases accuracy
    
    return y;
}

/**
  * @brief  initializes IMU
  * @param  None
  * @retval None
  */
void IMU::IMU_Init()
{    
    MPU9250_Init();
    //BMP180_Init();
    
    q0 = 1.0f;  
    q1 = 0.0f;
    q2 = 0.0f;
    q3 = 0.0f;
}

/**
  * @brief  Updata attitude and heading 
  * @param  ax: accelerometer X
  * @param  ay: accelerometer Y
  * @param  az: accelerometer Z
  * @param  gx: gyroscopes X
  * @param  gy: gyroscopes Y
  * @param  gz: gyroscopes Z
  * @param  mx: magnetometer X
  * @param  my: magnetometer Y
  * @param  mz: magnetometer Z
  * @retval None
  */
void IMU::IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
{
    float norm;
    float hx, hy, hz, bx, bz;
    float vx, vy, vz, wx, wy, wz;
    float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;
    float ex, ey, ez, halfT = 0.024f;

    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;   
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;          

    norm = invSqrt(ax * ax + ay * ay + az * az);       
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

    norm = invSqrt(mx * mx + my * my + mz * mz);          
    mx = mx * norm;
    my = my * norm;
    mz = mz * norm;

    // compute reference direction of flux
    hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
    hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
    hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);         
    bx = sqrt((hx * hx) + (hy * hy));
    bz = hz;     

    // estimated direction of gravity and flux (v and w)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;
    wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
    wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
    wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);  

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay * vz - az * vy) + (my * wz - mz * wy);
    ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
    ez = (ax * vy - ay * vx) + (mx * wy - my * wx);

    if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
    {
        exInt = exInt + ex * Ki * halfT;
        eyInt = eyInt + ey * Ki * halfT;    
        ezInt = ezInt + ez * Ki * halfT;

        gx = gx + Kp * ex + exInt;
        gy = gy + Kp * ey + eyInt;
        gz = gz + Kp * ez + ezInt;
    }

    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;  

    norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;
}

/**
  * @brief  Get quaters
  * @param  None
  * @retval None
  */
void IMU::IMU_GetQuater(void)
{
    float MotionVal[9];
            
    MPU9250_READ_ACCEL();
    MPU9250_READ_GYRO();
    MPU9250_READ_MAG();

    MotionVal[0]=gyro[0]/32.8;
    MotionVal[1]=gyro[1]/32.8;
    MotionVal[2]=gyro[2]/32.8;
    MotionVal[3]=accel[0];
    MotionVal[4]=accel[1];
    MotionVal[5]=accel[2];
    MotionVal[6]=magn[0];
    MotionVal[7]=magn[1];
    MotionVal[8]=magn[2];
    
    IMU_AHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175,
    (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
}

/**
  * @brief  Get Yaw Pitch Roll
  * @param  None
  * @retval None
  */
void IMU::IMU_GetYawPitchRoll(float *Angles) 
{
    
    int x_tmp,y_tmp;
    
    IMU_GetQuater(); 
    
    x_tmp = magn[0];
    y_tmp = magn[1];
    if(x_tmp>0x7fff)x_tmp-=0xffff;    
    if(y_tmp>0x7fff)y_tmp-=0xffff;  
      
    Angles[1] = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
    Angles[2] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
    Angles[0] = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3;   //Yaw
    Angles[3] = atan2((double)(y_tmp) , (double)(x_tmp)) * (180 / 3.14159265) + 180; // Compass angle in degrees
    
    
}

/************************************************************/
//下面的代码是MPU9250的各种操作
/************************************************************/
void IMU::I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data)
{
    char data_write[2];
    
    data_write[0] = RegAddr;
    data_write[1] = Data;
    
    i2c.write(DevAddr,data_write,2,0);
}

uint8_t IMU::I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr)
{
    char data_write[2];
    char data_read[2];
    char data;
    
    data_write[0] = RegAddr;
    
    i2c.write(DevAddr,data_write,1,1);
    i2c.read(DevAddr,data_read,1,0);
    
    data = data_read[0];
    
    return data;
}
/**
  * @brief  Initializes MPU9250
  * @param  None
  * @retval None
  */
void IMU::MPU9250_Init()
{
    I2C_WriteOneByte(GYRO_ADDRESS,PWR_MGMT_1, 0x00);
    I2C_WriteOneByte(GYRO_ADDRESS,SMPLRT_DIV, 0x07);
    I2C_WriteOneByte(GYRO_ADDRESS,CONFIG, 0x06);
    I2C_WriteOneByte(GYRO_ADDRESS,GYRO_CONFIG, 0x10);
    I2C_WriteOneByte(GYRO_ADDRESS,ACCEL_CONFIG, 0x01);
    
    Thread::wait(10);
    
    if(MPU9250_Check())
    {
        printf("\r\n MPU9255 Ready!\n");
    }
    else
    {
        printf("\r\n MPU9255 Erro!\n");
    }
    
    MPU9250_InitGyrOffset();
}

/**
  * @brief  Digital filter
  * @param *pIndex:
  * @param *pAvgBuffer:
  * @param InVal:
  * @param pOutVal:
  *
  * @retval None
  *               
  */
void IMU::MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal)
{   
    uint8_t i;
    
    *(pAvgBuffer + ((*pIndex) ++)) = InVal;
    *pIndex &= 0x07;
    
    *pOutVal = 0;
    for(i = 0; i < 8; i ++) 
    {
        *pOutVal += *(pAvgBuffer + i);
    }
    *pOutVal >>= 3;
}

/**
  * @brief Get accelerometer datas
  * @param  None
  * @retval None
  */
void IMU::MPU9250_READ_ACCEL(void)
{ 
    uint8_t i;
    int16_t InBuffer[3] = {0}; 
    static int32_t OutBuffer[3] = {0};
    static MPU9250_AvgTypeDef MPU9250_Filter[3];
    
   BUF[0]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_L); 
   BUF[1]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_H);
   InBuffer[0]= (BUF[1]<<8)|BUF[0];

   BUF[2]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_L);
   BUF[3]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_H);
   InBuffer[1]= (BUF[3]<<8)|BUF[2];
                       
   BUF[4]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_L);
   BUF[5]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_H);
   InBuffer[2]= (BUF[5]<<8)|BUF[4];                
   
   for(i = 0; i < 3; i ++)  
   {
      MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
   }
   accel[0] = *(OutBuffer + 0);
   accel[1] = *(OutBuffer + 1);
   accel[2] = *(OutBuffer + 2); 
}

/**
  * @brief Get gyroscopes datas
  * @param  None
  * @retval None
  */
void IMU::MPU9250_READ_GYRO(void)
{ 
     uint8_t i;
     int16_t InBuffer[3] = {0}; 
     static int32_t OutBuffer[3] = {0};
     static MPU9250_AvgTypeDef MPU9250_Filter[3];

   BUF[0]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_L); 
   BUF[1]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_H);
   InBuffer[0]= (BUF[1]<<8)|BUF[0];
   
   BUF[2]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_L);
   BUF[3]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_H);
   InBuffer[1] = (BUF[3]<<8)|BUF[2];
    
   BUF[4]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_L);
   BUF[5]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_H);
   InBuffer[2] = (BUF[5]<<8)|BUF[4];    

   for(i = 0; i < 3; i ++)  
   {
      MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
   }
   gyro[0] = *(OutBuffer + 0) - MPU9250_Offset.X;
   gyro[1] = *(OutBuffer + 1) - MPU9250_Offset.Y;
   gyro[2] = *(OutBuffer + 2) - MPU9250_Offset.Z;
}

/**
  * @brief Get compass datas
  * @param  None
  * @retval None
  */
void IMU::MPU9250_READ_MAG(void)
{ 
     uint8_t i;
     int16_t InBuffer[3] = {0}; 
     static int32_t OutBuffer[3] = {0};
     static MPU9250_AvgTypeDef MPU9250_Filter[3];

    I2C_WriteOneByte(GYRO_ADDRESS,0x37,0x02);//turn on Bypass Mode 
    Thread::wait(10);
    I2C_WriteOneByte(MAG_ADDRESS,0x0A,0x01);    
    Thread::wait(10);
    BUF[0]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_L);
    BUF[1]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_H);
    InBuffer[1] =(BUF[1]<<8)|BUF[0];

    BUF[2]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_L);
    BUF[3]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_H);
    InBuffer[0] =   (BUF[3]<<8)|BUF[2];
    
    BUF[4]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_L);
    BUF[5]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_H);
    InBuffer[2] =   (BUF[5]<<8)|BUF[4]; 
    InBuffer[2] = -InBuffer[2];
     
   for(i = 0; i < 3; i ++)  
   {
      MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
   } 
        magn[0] = *(OutBuffer + 0)-MPU9250_Magn_Offset.X_Off_Err;
        magn[1] = *(OutBuffer + 1)-MPU9250_Magn_Offset.Y_Off_Err;
        magn[2] = *(OutBuffer + 2)-MPU9250_Magn_Offset.Z_Off_Err;
}

/**
  * @brief  Check MPU9250,ensure communication succeed
  * @param  None
  * @retval true: communicate succeed
  *               false: communicate fualt 
  */
bool IMU::MPU9250_Check(void) 
{
    if(WHO_AM_I_VAL == I2C_ReadOneByte(DEFAULT_ADDRESS, WHO_AM_I))  
    {
        return true;
    }
    else 
    {
        return false;
    }   
}

/**
  * @brief  Initializes gyroscopes offset
  * @param  None
  * @retval None
  */
void IMU::MPU9250_InitGyrOffset(void)
{
    uint8_t i;
    int32_t TempGx = 0, TempGy = 0, TempGz = 0;
    
    for(i = 0; i < 32; i ++)
    {
        MPU9250_READ_GYRO();
        
        TempGx += gyro[0];
        TempGy += gyro[1];
        TempGz += gyro[2];

        Thread::wait(1);
    }

    MPU9250_Offset.X = TempGx >> 5;
    MPU9250_Offset.Y = TempGy >> 5;
    MPU9250_Offset.Z = TempGz >> 5;

}