Revision 1:7cf70724bdb0, committed 2018-06-21

Comitter:

adaphoto

Date:

Thu Jun 21 08:46:30 2018 +0000

Parent:

0:35bba382318b

Commit message:

???BMP180????????????????

Changed in this revision

diff -r 35bba382318b -r 7cf70724bdb0 imu.cpp
--- a/imu.cpp	Thu Jun 21 07:00:40 2018 +0000
+++ b/imu.cpp	Thu Jun 21 08:46:30 2018 +0000
@@ -28,7 +28,7 @@
 void IMU::IMU_Init()
 {    
     MPU9250_Init();
-    //BMP180_Init();
+    BMP180_Init();
     
     q0 = 1.0f;  
     q1 = 0.0f;
@@ -201,6 +201,16 @@
     
     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
@@ -400,4 +410,299 @@
     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;
+    }   
 }
\ No newline at end of file

diff -r 35bba382318b -r 7cf70724bdb0 imu.h
--- a/imu.h	Thu Jun 21 07:00:40 2018 +0000
+++ b/imu.h	Thu Jun 21 08:46:30 2018 +0000
@@ -56,32 +56,83 @@
 #define DEFAULT_ADDRESS     GYRO_ADDRESS
 #define WHO_AM_I_VAL        0x73 //identity of MPU9250 is 0x71. identity of MPU9255 is 0x73.
 
-    typedef struct
-    {
-        int16_t X;
-        int16_t Y;
-        int16_t Z;
-    }MPU9250_TypeDef;
-    
-    typedef struct
-    {
-        int16_t X_Off_Err;
-        int16_t Y_Off_Err;
-        int16_t Z_Off_Err;
-    }MPU9250_TypeDef_Off;
-    
-    typedef struct
-    {
-        uint8_t Index;
-        int16_t AvgBuffer[8];
-    }MPU9250_AvgTypeDef;
+
+// define BMP180 register address
+//****************************************
+#define BMP180_ADDR       0xEE     // default I2C address  
+
+/* ---- Registers ---- */
+#define CAL_AC1           0xAA  // R   Calibration data (16 bits)
+#define CAL_AC2           0xAC  // R   Calibration data (16 bits)
+#define CAL_AC3           0xAE  // R   Calibration data (16 bits)    
+#define CAL_AC4           0xB0  // R   Calibration data (16 bits)
+#define CAL_AC5           0xB2  // R   Calibration data (16 bits)
+#define CAL_AC6           0xB4  // R   Calibration data (16 bits)
+#define CAL_B1            0xB6  // R   Calibration data (16 bits)
+#define CAL_B2            0xB8  // R   Calibration data (16 bits)
+#define CAL_MB            0xBA  // R   Calibration data (16 bits)
+#define CAL_MC            0xBC  // R   Calibration data (16 bits)
+#define CAL_MD            0xBE  // R   Calibration data (16 bits)
+#define CONTROL           0xF4  // W   Control register 
+#define CONTROL_OUTPUT    0xF6  // R   Output registers 0xF6=MSB, 0xF7=LSB, 0xF8=XLSB
+
+// BMP180 Modes
+#define MODE_ULTRA_LOW_POWER    0 //oversampling=0, internalsamples=1, maxconvtimepressure=4.5ms, avgcurrent=3uA, RMSnoise_hPA=0.06, RMSnoise_m=0.5
+#define MODE_STANDARD           1 //oversampling=1, internalsamples=2, maxconvtimepressure=7.5ms, avgcurrent=5uA, RMSnoise_hPA=0.05, RMSnoise_m=0.4
+#define MODE_HIGHRES            2 //oversampling=2, internalsamples=4, maxconvtimepressure=13.5ms, avgcurrent=7uA, RMSnoise_hPA=0.04, RMSnoise_m=0.3
+#define MODE_ULTRA_HIGHRES      3 //oversampling=3, internalsamples=8, maxconvtimepressure=25.5ms, avgcurrent=12uA, RMSnoise_hPA=0.03, RMSnoise_m=0.25
+
+// Control register
+#define READ_TEMPERATURE        0x2E 
+#define READ_PRESSURE           0x34 
+//Other
+#define MSLP                    102025          // Mean Sea Level Pressure = 1013.25 hPA (1hPa = 100Pa = 1mbar)
+#define LOCAL_ADS_ALTITUDE      2500            //mm     altitude of your position now
+#define PRESSURE_OFFSET         0               //Pa    Offset
 
+/*********************************************/
+// struct
+typedef struct
+{
+    int16_t X;
+    int16_t Y;
+    int16_t Z;
+}MPU9250_TypeDef;
+
+typedef struct
+{
+    int16_t X_Off_Err;
+    int16_t Y_Off_Err;
+    int16_t Z_Off_Err;
+}MPU9250_TypeDef_Off;
+
+typedef struct
+{
+    uint8_t Index;
+    int16_t AvgBuffer[8];
+}MPU9250_AvgTypeDef;
+
+typedef struct
+{
+    uint8_t Index;
+    int32_t AvgBuffer[8];
+}BMP180_AvgTypeDef;
+
+enum
+{
+    START_TEMPERATURE_MEASUREMENT = 0,
+    READ_UT_AND_START_PRESSURE_MEASUREMENT,
+    READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE,
+};
+/*******************************************************/
 class IMU 
 {
     
     public:
+    //IMU
     void IMU_Init();
     void IMU_GetYawPitchRoll(float*Angles);
+    
     //MPU9255
     void MPU9250_Init();
     void MPU9250_READ_ACCEL();
@@ -91,14 +142,32 @@
     void MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
     void MPU9250_InitGyrOffset();
     
-    int16_t accel[3], gyro[3], magn[3];
+    //BMP180
+    void CalTemperatureAndPressureAndAltitude(void);
+    void BMP180_Init(void);
+    void BMP180_StartTemperatureMeasurement(void);
+    void BMP180_StartPressureMeasurement(void);
+    void BMP180_ReadUncompensatedTemperature(void);
+    void BMP180_ReadUncompensatedPressure(void);
+    void BMP180_CalculateTrueTemperature(int32_t *pTrueTemperature);
+    void BMP180_CalculateTruePressure(int32_t *pTruePressure);
+    void BMP180_LocalpressureAvg(int32_t *pVal);
+    void BMP180_PressureAtSeaLevel(void);
+    void BMP180_CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal);
+    void BMP180_ReadCalibrationData(void);
+    void BMP180_SetOversample(void);
+    
+    int32_t PressureVal, TemperatureVal, AltitudeVal;
     
     private:
+    //IMU
+    float   q0, q1, q2, q3;
     
+    //MPU9250
     char    BUF[10];
-    //int16_t accel[3], gyro[3], magn[3];
+    int16_t accel[3], gyro[3], magn[3];
     float   angles[3];
-    float   q0, q1, q2, q3;
+
     MPU9250_TypeDef     MPU9250_Offset;//={0};
     MPU9250_TypeDef_Off MPU9250_Magn_Offset;//={0};
     
@@ -106,10 +175,18 @@
     void    IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
     void    IMU_GetQuater(void);
     
-    //void    MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
+    //BMP180
+    //int32_t PressureVal, TemperatureVal, AltitudeVal;
+    int16_t AC1, AC2, AC3, B1, B2, MB, MC, MD, _oss;  
+    uint16_t AC4, AC5, AC6;
+    int32_t B5, UT, UP, Pressure0; 
     
+    void BMP180_CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_t InVal, int32_t *pOutVal);
+
+    //I2C
     void    I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data);
     uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr);
+    void    BMP180_ReadReg(uint8_t RegAddr, uint8_t Num, char *pBuffer);
 };
 
 #endif
\ No newline at end of file