基于MPU9250的IMU库,可以测量pitch,roll,yaw,compass

imu.cpp

Committer:
adaphoto
Date:
15 months ago
Revision:
1:7cf70724bdb0
Parent:
0:35bba382318b

File content as of revision 1:7cf70724bdb0:

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

void IMU::BMP180_ReadReg(uint8_t RegAddr, uint8_t Num, char *pBuffer) 
{
    char data_write[2];
    char DevAddr = BMP180_ADDR;
    
    data_write[0] = RegAddr;
    i2c.write(DevAddr, data_write, 1, 1);
    i2c.read(DevAddr, pBuffer, Num, 0);
}
/**
  * @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;

}

//******************************************************
//下面的代码是BMP180
//******************************************************
/**
  * @brief  Digital filter
  * @param *pIndex:
  * @param *pAvgBuffer:
  * @param InVal:
  * @param pOutVal:
  *
  * @retval None
  *               
  */
void IMU::BMP180_CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_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  Start temperature measurement
  * @param  None
  * @retval None
  */
void IMU::BMP180_StartTemperatureMeasurement(void)
{
    //BMP180_WriteReg(CONTROL, READ_TEMPERATURE); 
    I2C_WriteOneByte(BMP180_ADDR, CONTROL, READ_TEMPERATURE);
}

/**
  * @brief  Start pressure measurement
  * @param  None
  * @retval None
  */
void IMU::BMP180_StartPressureMeasurement(void)
{
    //BMP180_WriteReg(CONTROL, READ_PRESSURE + (_oss << 6)); 
    I2C_WriteOneByte(BMP180_ADDR, CONTROL, READ_PRESSURE + (_oss << 6));
}

/**
  * @brief  Read uncompensated temperature
  * @param  None
  * @retval None
  */
void IMU::BMP180_ReadUncompensatedTemperature(void)
{
    char RegBuff[2];
    BMP180_ReadReg(CONTROL_OUTPUT, 2, &RegBuff[0]); 

    UT = ((int32_t)RegBuff[0] << 8) + (int32_t)RegBuff[1]; 
}

/**
  * @brief  Read uncompensated pressure
  * @param  None
  * @retval None
  */
void IMU::BMP180_ReadUncompensatedPressure(void)
{
    char RegBuff[3];
    
    BMP180_ReadReg(CONTROL_OUTPUT, 3, &RegBuff[0]); 
    
    UP = (((int32_t)RegBuff[0] << 16) + ((int32_t)RegBuff[1] << 8) + ((int32_t)RegBuff[2])) >> (8 -_oss); // uncompensated pressure value
}

/**
  * @brief  Calculate true temperature
  * @param  *pTrueTemperature: true temperature 
  * @retval None
  */
void IMU::BMP180_CalculateTrueTemperature(int32_t *pTrueTemperature)
{
    int32_t X1, X2;
    
    X1 = ((UT - AC6) * AC5) >> 15;
    X2 = (MC << 11) / (X1 + MD);
    B5 = X1 + X2;
    *pTrueTemperature = (B5 + 8) >> 4;
}

/**
  * @brief  Calculate true pressure
  * @param  *pTruePressure: true pressure
  * @retval None
  */
void IMU::BMP180_CalculateTruePressure(int32_t *pTruePressure)
{
    int32_t X1, X2, X3, B3, B6, P, Temp;
    uint32_t  B4, B7;
    
    B6 = B5 - 4000;             
    X1 = (B2* ((B6 * B6) >> 12)) >> 11;
    X2 = AC2 * B6 >> 11;
    X3 = X1 + X2;
    Temp = (((int32_t)AC1 << 2) + X3) << _oss;
    B3 = (Temp + 2) >> 2;
    X1 = (AC3 * B6) >> 13;
    X2 = (B1 * (B6 * B6 >> 12)) >> 16;
    X3 = ((X1 + X2) + 2) >> 2;
    B4 = (AC4 * (uint32_t) (X3 + 32768)) >> 15;
    B7 = ((uint32_t)UP - B3) * (50000 >> _oss);
    if(B7 < 0x80000000)
    {
        P = (B7 << 1) / B4;
    }   
    else
    {
        P = (B7 / B4) << 1;
    }

    X1 = (P >> 8) * (P >> 8);
    X1 = (X1 * 3038) >> 16;
    X2 = (-7357 * P) >> 16;
    
    *pTruePressure = P + ((X1 + X2 + 3791) >> 4);
}

/**
  * @brief  Calculating average value of pressure
  * @param  *pVal: the average value of pressure
  * @retval None
  */
void IMU::BMP180_LocalpressureAvg(int32_t *pVal)
{
    uint8_t i;
    int32_t Sum = 0;
    
    for(i = 0; i < 5; i ++)
    {
        BMP180_StartTemperatureMeasurement();
        Thread::wait(5);//delay_ms(5); //4.5ms   324
        BMP180_ReadUncompensatedTemperature();
        BMP180_StartPressureMeasurement();
        Thread::wait(5);//delay_ms(8);//7.5ms    540
        BMP180_ReadUncompensatedPressure();
        BMP180_CalculateTruePressure(&PressureVal);
        BMP180_CalculateTrueTemperature(&TemperatureVal);
        
        if(i >= 2)
        {
            Sum += PressureVal;
        }
    }
    *pVal = Sum / 3;
}

/** 
  * @brief  Calculating pressure at sea level
  * @param  None
  * @retval None
  */
void IMU::BMP180_PressureAtSeaLevel(void)
{  
    float Temp = 0.0f;
    
    BMP180_LocalpressureAvg(&PressureVal);
    
    Temp = (float)LOCAL_ADS_ALTITUDE / 4433000;
    Temp = (float)pow((1 - Temp), 5.255f);
    Pressure0 = (PressureVal - PRESSURE_OFFSET) / Temp;//
}

/** 
  * @brief  Calculating absolute altitude
  * @param  *pAltitude: absolute altitude
  * @param  PressureVal: the pressure at the absolute altitude
  * @retval None
  */
void IMU::BMP180_CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal)
{
    *pAltitude = 4433000 * (1 - pow((float)(PressureVal / (float)Pressure0), 0.1903f)); 
}

/**
  * @brief  Read calibration data from the EEPROM of the BMP180
  * @param  None
  * @retval None
  */
void IMU::BMP180_ReadCalibrationData(void) 
{
    char RegBuff[2];

    BMP180_ReadReg(CAL_AC1, 2, RegBuff);
    AC1 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
    
    BMP180_ReadReg(CAL_AC2, 2, RegBuff);
    AC2 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
    
    BMP180_ReadReg(CAL_AC3, 2, RegBuff);
    AC3 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
    
    BMP180_ReadReg(CAL_AC4, 2, RegBuff);
    AC4 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1]));
    
    BMP180_ReadReg(CAL_AC5, 2, RegBuff);
    AC5 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1]));
    
    BMP180_ReadReg(CAL_AC6, 2, RegBuff);
    AC6 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1])); 
    
    BMP180_ReadReg(CAL_B1, 2, RegBuff);
    B1 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 
    
    BMP180_ReadReg(CAL_B2, 2, RegBuff);
    B2 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
     
    BMP180_ReadReg(CAL_MB, 2, RegBuff);
    MB = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
    
    BMP180_ReadReg(CAL_MC, 2, RegBuff);
    MC = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
    
    BMP180_ReadReg(CAL_MD, 2, RegBuff);
    MD = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 
}

/**
  * @brief  Configures hardware pressure sampling accuracy modes
  * @param  None
  * @retval None
  */
void IMU::BMP180_SetOversample(void)
{
    _oss = MODE_ULTRA_HIGHRES;
}

/**
  * @brief  initializes BMP180
  * @param  None
  * @retval None
  */
void IMU::BMP180_Init(void) 
{  
    BMP180_SetOversample();
    BMP180_ReadCalibrationData();
    //BMP180_PressureAtSeaLevel();
}

/**
  * @brief  Calculation of pressure and temperature and altitude for BMP180
  * @param  None
  * @retval None
  */
void IMU::CalTemperatureAndPressureAndAltitude(void)
{
    static uint8_t State = START_TEMPERATURE_MEASUREMENT;
    static BMP180_AvgTypeDef BMP180_Filter[3];
    int32_t PVal,AVal, TVal;

    switch(State)
    {
        case START_TEMPERATURE_MEASUREMENT:
            BMP180_StartTemperatureMeasurement();
            Thread::wait(5);//delay_ms(5); //4.5ms
            State = READ_UT_AND_START_PRESSURE_MEASUREMENT;
            break;
            
        case READ_UT_AND_START_PRESSURE_MEASUREMENT:
            BMP180_ReadUncompensatedTemperature();
            BMP180_StartPressureMeasurement();
            Thread::wait(10);//delay_ms(10);//7.5ms
            State = READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE;
            break;
            
        case READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE:
            BMP180_ReadUncompensatedPressure();
            BMP180_CalculateTruePressure(&PVal);
            BMP180_CalAvgValue(&BMP180_Filter[0].Index, BMP180_Filter[0].AvgBuffer, PVal - PRESSURE_OFFSET, &PressureVal);

            BMP180_CalculateAbsoluteAltitude(&AVal, PVal - PRESSURE_OFFSET);
            BMP180_CalAvgValue(&BMP180_Filter[1].Index, BMP180_Filter[1].AvgBuffer, AVal, &AltitudeVal);

            BMP180_CalculateTrueTemperature(&TVal);
            BMP180_CalAvgValue(&BMP180_Filter[2].Index, BMP180_Filter[2].AvgBuffer, TVal, &TemperatureVal);

            State = START_TEMPERATURE_MEASUREMENT;
            break;

        default:
            break;
    }   
}