chen ada
基于MPU9250的IMU库，可以测量pitch，roll，yaw，compass
Diff: imu.cpp
- Revision:
- 1:7cf70724bdb0
- Parent:
- 0:35bba382318b
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