chen ada
基于MPU9250的IMU库，可以测量pitch，roll，yaw，compass
Embed:
(wiki syntax)
Show/hide line numbers
imu.cpp
00001 #include "imu.h" 00002 00003 I2C i2c(I2C_SDA,I2C_SCL); 00004 00005 /** 00006 * @brief invSqrt 00007 * @param 00008 * @retval 00009 */ 00010 float IMU::invSqrt(float x) 00011 { 00012 float halfx = 0.5f * x; 00013 float y = x; 00014 00015 long i = *(long*)&y; //get bits for floating value 00016 i = 0x5f3759df - (i >> 1); //gives initial guss you 00017 y = *(float*)&i; //convert bits back to float 00018 y = y * (1.5f - (halfx * y * y)); //newtop step, repeating increases accuracy 00019 00020 return y; 00021 } 00022 00023 /** 00024 * @brief initializes IMU 00025 * @param None 00026 * @retval None 00027 */ 00028 void IMU::IMU_Init() 00029 { 00030 MPU9250_Init(); 00031 BMP180_Init(); 00032 00033 q0 = 1.0f; 00034 q1 = 0.0f; 00035 q2 = 0.0f; 00036 q3 = 0.0f; 00037 } 00038 00039 /** 00040 * @brief Updata attitude and heading 00041 * @param ax: accelerometer X 00042 * @param ay: accelerometer Y 00043 * @param az: accelerometer Z 00044 * @param gx: gyroscopes X 00045 * @param gy: gyroscopes Y 00046 * @param gz: gyroscopes Z 00047 * @param mx: magnetometer X 00048 * @param my: magnetometer Y 00049 * @param mz: magnetometer Z 00050 * @retval None 00051 */ 00052 void IMU::IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 00053 { 00054 float norm; 00055 float hx, hy, hz, bx, bz; 00056 float vx, vy, vz, wx, wy, wz; 00057 float exInt = 0.0, eyInt = 0.0, ezInt = 0.0; 00058 float ex, ey, ez, halfT = 0.024f; 00059 00060 float q0q0 = q0 * q0; 00061 float q0q1 = q0 * q1; 00062 float q0q2 = q0 * q2; 00063 float q0q3 = q0 * q3; 00064 float q1q1 = q1 * q1; 00065 float q1q2 = q1 * q2; 00066 float q1q3 = q1 * q3; 00067 float q2q2 = q2 * q2; 00068 float q2q3 = q2 * q3; 00069 float q3q3 = q3 * q3; 00070 00071 norm = invSqrt(ax * ax + ay * ay + az * az); 00072 ax = ax * norm; 00073 ay = ay * norm; 00074 az = az * norm; 00075 00076 norm = invSqrt(mx * mx + my * my + mz * mz); 00077 mx = mx * norm; 00078 my = my * norm; 00079 mz = mz * norm; 00080 00081 // compute reference direction of flux 00082 hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2); 00083 hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1); 00084 hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2); 00085 bx = sqrt((hx * hx) + (hy * hy)); 00086 bz = hz; 00087 00088 // estimated direction of gravity and flux (v and w) 00089 vx = 2 * (q1q3 - q0q2); 00090 vy = 2 * (q0q1 + q2q3); 00091 vz = q0q0 - q1q1 - q2q2 + q3q3; 00092 wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2); 00093 wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3); 00094 wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2); 00095 00096 // error is sum of cross product between reference direction of fields and direction measured by sensors 00097 ex = (ay * vz - az * vy) + (my * wz - mz * wy); 00098 ey = (az * vx - ax * vz) + (mz * wx - mx * wz); 00099 ez = (ax * vy - ay * vx) + (mx * wy - my * wx); 00100 00101 if(ex != 0.0f && ey != 0.0f && ez != 0.0f) 00102 { 00103 exInt = exInt + ex * Ki * halfT; 00104 eyInt = eyInt + ey * Ki * halfT; 00105 ezInt = ezInt + ez * Ki * halfT; 00106 00107 gx = gx + Kp * ex + exInt; 00108 gy = gy + Kp * ey + eyInt; 00109 gz = gz + Kp * ez + ezInt; 00110 } 00111 00112 q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT; 00113 q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT; 00114 q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT; 00115 q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT; 00116 00117 norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 00118 q0 = q0 * norm; 00119 q1 = q1 * norm; 00120 q2 = q2 * norm; 00121 q3 = q3 * norm; 00122 } 00123 00124 /** 00125 * @brief Get quaters 00126 * @param None 00127 * @retval None 00128 */ 00129 void IMU::IMU_GetQuater(void) 00130 { 00131 float MotionVal[9]; 00132 00133 MPU9250_READ_ACCEL(); 00134 MPU9250_READ_GYRO(); 00135 MPU9250_READ_MAG(); 00136 00137 MotionVal[0]=gyro[0]/32.8; 00138 MotionVal[1]=gyro[1]/32.8; 00139 MotionVal[2]=gyro[2]/32.8; 00140 MotionVal[3]=accel[0]; 00141 MotionVal[4]=accel[1]; 00142 MotionVal[5]=accel[2]; 00143 MotionVal[6]=magn[0]; 00144 MotionVal[7]=magn[1]; 00145 MotionVal[8]=magn[2]; 00146 00147 IMU_AHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175, 00148 (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]); 00149 } 00150 00151 /** 00152 * @brief Get Yaw Pitch Roll 00153 * @param None 00154 * @retval None 00155 */ 00156 void IMU::IMU_GetYawPitchRoll(float *Angles) 00157 { 00158 00159 int x_tmp,y_tmp; 00160 00161 IMU_GetQuater(); 00162 00163 x_tmp = magn[0]; 00164 y_tmp = magn[1]; 00165 if(x_tmp>0x7fff)x_tmp-=0xffff; 00166 if(y_tmp>0x7fff)y_tmp-=0xffff; 00167 00168 Angles[1] = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch 00169 Angles[2] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll 00170 Angles[0] = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3; //Yaw 00171 Angles[3] = atan2((double)(y_tmp) , (double)(x_tmp)) * (180 / 3.14159265) + 180; // Compass angle in degrees 00172 00173 00174 } 00175 00176 /************************************************************/ 00177 //下面的代码是MPU9250的各种操作 00178 /************************************************************/ 00179 void IMU::I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data) 00180 { 00181 char data_write[2]; 00182 00183 data_write[0] = RegAddr; 00184 data_write[1] = Data; 00185 00186 i2c.write(DevAddr,data_write,2,0); 00187 } 00188 00189 uint8_t IMU::I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr) 00190 { 00191 char data_write[2]; 00192 char data_read[2]; 00193 char data; 00194 00195 data_write[0] = RegAddr; 00196 00197 i2c.write(DevAddr,data_write,1,1); 00198 i2c.read(DevAddr,data_read,1,0); 00199 00200 data = data_read[0]; 00201 00202 return data; 00203 } 00204 00205 void IMU::BMP180_ReadReg(uint8_t RegAddr, uint8_t Num, char *pBuffer) 00206 { 00207 char data_write[2]; 00208 char DevAddr = BMP180_ADDR; 00209 00210 data_write[0] = RegAddr; 00211 i2c.write(DevAddr, data_write, 1, 1); 00212 i2c.read(DevAddr, pBuffer, Num, 0); 00213 } 00214 /** 00215 * @brief Initializes MPU9250 00216 * @param None 00217 * @retval None 00218 */ 00219 void IMU::MPU9250_Init() 00220 { 00221 I2C_WriteOneByte(GYRO_ADDRESS,PWR_MGMT_1, 0x00); 00222 I2C_WriteOneByte(GYRO_ADDRESS,SMPLRT_DIV, 0x07); 00223 I2C_WriteOneByte(GYRO_ADDRESS,CONFIG, 0x06); 00224 I2C_WriteOneByte(GYRO_ADDRESS,GYRO_CONFIG, 0x10); 00225 I2C_WriteOneByte(GYRO_ADDRESS,ACCEL_CONFIG, 0x01); 00226 00227 Thread::wait(10); 00228 00229 if(MPU9250_Check()) 00230 { 00231 printf("\r\n MPU9255 Ready!\n"); 00232 } 00233 else 00234 { 00235 printf("\r\n MPU9255 Erro!\n"); 00236 } 00237 00238 MPU9250_InitGyrOffset(); 00239 } 00240 00241 /** 00242 * @brief Digital filter 00243 * @param *pIndex: 00244 * @param *pAvgBuffer: 00245 * @param InVal: 00246 * @param pOutVal: 00247 * 00248 * @retval None 00249 * 00250 */ 00251 void IMU::MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal) 00252 { 00253 uint8_t i; 00254 00255 *(pAvgBuffer + ((*pIndex) ++)) = InVal; 00256 *pIndex &= 0x07; 00257 00258 *pOutVal = 0; 00259 for(i = 0; i < 8; i ++) 00260 { 00261 *pOutVal += *(pAvgBuffer + i); 00262 } 00263 *pOutVal >>= 3; 00264 } 00265 00266 /** 00267 * @brief Get accelerometer datas 00268 * @param None 00269 * @retval None 00270 */ 00271 void IMU::MPU9250_READ_ACCEL(void) 00272 { 00273 uint8_t i; 00274 int16_t InBuffer[3] = {0}; 00275 static int32_t OutBuffer[3] = {0}; 00276 static MPU9250_AvgTypeDef MPU9250_Filter[3]; 00277 00278 BUF[0]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_L); 00279 BUF[1]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_H); 00280 InBuffer[0]= (BUF[1]<<8)|BUF[0]; 00281 00282 BUF[2]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_L); 00283 BUF[3]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_H); 00284 InBuffer[1]= (BUF[3]<<8)|BUF[2]; 00285 00286 BUF[4]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_L); 00287 BUF[5]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_H); 00288 InBuffer[2]= (BUF[5]<<8)|BUF[4]; 00289 00290 for(i = 0; i < 3; i ++) 00291 { 00292 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i); 00293 } 00294 accel[0] = *(OutBuffer + 0); 00295 accel[1] = *(OutBuffer + 1); 00296 accel[2] = *(OutBuffer + 2); 00297 } 00298 00299 /** 00300 * @brief Get gyroscopes datas 00301 * @param None 00302 * @retval None 00303 */ 00304 void IMU::MPU9250_READ_GYRO(void) 00305 { 00306 uint8_t i; 00307 int16_t InBuffer[3] = {0}; 00308 static int32_t OutBuffer[3] = {0}; 00309 static MPU9250_AvgTypeDef MPU9250_Filter[3]; 00310 00311 BUF[0]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_L); 00312 BUF[1]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_H); 00313 InBuffer[0]= (BUF[1]<<8)|BUF[0]; 00314 00315 BUF[2]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_L); 00316 BUF[3]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_H); 00317 InBuffer[1] = (BUF[3]<<8)|BUF[2]; 00318 00319 BUF[4]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_L); 00320 BUF[5]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_H); 00321 InBuffer[2] = (BUF[5]<<8)|BUF[4]; 00322 00323 for(i = 0; i < 3; i ++) 00324 { 00325 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i); 00326 } 00327 gyro[0] = *(OutBuffer + 0) - MPU9250_Offset.X; 00328 gyro[1] = *(OutBuffer + 1) - MPU9250_Offset.Y; 00329 gyro[2] = *(OutBuffer + 2) - MPU9250_Offset.Z; 00330 } 00331 00332 /** 00333 * @brief Get compass datas 00334 * @param None 00335 * @retval None 00336 */ 00337 void IMU::MPU9250_READ_MAG(void) 00338 { 00339 uint8_t i; 00340 int16_t InBuffer[3] = {0}; 00341 static int32_t OutBuffer[3] = {0}; 00342 static MPU9250_AvgTypeDef MPU9250_Filter[3]; 00343 00344 I2C_WriteOneByte(GYRO_ADDRESS,0x37,0x02);//turn on Bypass Mode 00345 Thread::wait(10); 00346 I2C_WriteOneByte(MAG_ADDRESS,0x0A,0x01); 00347 Thread::wait(10); 00348 BUF[0]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_L); 00349 BUF[1]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_H); 00350 InBuffer[1] =(BUF[1]<<8)|BUF[0]; 00351 00352 BUF[2]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_L); 00353 BUF[3]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_H); 00354 InBuffer[0] = (BUF[3]<<8)|BUF[2]; 00355 00356 BUF[4]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_L); 00357 BUF[5]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_H); 00358 InBuffer[2] = (BUF[5]<<8)|BUF[4]; 00359 InBuffer[2] = -InBuffer[2]; 00360 00361 for(i = 0; i < 3; i ++) 00362 { 00363 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i); 00364 } 00365 magn[0] = *(OutBuffer + 0)-MPU9250_Magn_Offset.X_Off_Err; 00366 magn[1] = *(OutBuffer + 1)-MPU9250_Magn_Offset.Y_Off_Err; 00367 magn[2] = *(OutBuffer + 2)-MPU9250_Magn_Offset.Z_Off_Err; 00368 } 00369 00370 /** 00371 * @brief Check MPU9250,ensure communication succeed 00372 * @param None 00373 * @retval true: communicate succeed 00374 * false: communicate fualt 00375 */ 00376 bool IMU::MPU9250_Check(void) 00377 { 00378 if(WHO_AM_I_VAL == I2C_ReadOneByte(DEFAULT_ADDRESS, WHO_AM_I)) 00379 { 00380 return true; 00381 } 00382 else 00383 { 00384 return false; 00385 } 00386 } 00387 00388 /** 00389 * @brief Initializes gyroscopes offset 00390 * @param None 00391 * @retval None 00392 */ 00393 void IMU::MPU9250_InitGyrOffset(void) 00394 { 00395 uint8_t i; 00396 int32_t TempGx = 0, TempGy = 0, TempGz = 0; 00397 00398 for(i = 0; i < 32; i ++) 00399 { 00400 MPU9250_READ_GYRO(); 00401 00402 TempGx += gyro[0]; 00403 TempGy += gyro[1]; 00404 TempGz += gyro[2]; 00405 00406 Thread::wait(1); 00407 } 00408 00409 MPU9250_Offset.X = TempGx >> 5; 00410 MPU9250_Offset.Y = TempGy >> 5; 00411 MPU9250_Offset.Z = TempGz >> 5; 00412 00413 } 00414 00415 //****************************************************** 00416 //下面的代码是BMP180 00417 //****************************************************** 00418 /** 00419 * @brief Digital filter 00420 * @param *pIndex: 00421 * @param *pAvgBuffer: 00422 * @param InVal: 00423 * @param pOutVal: 00424 * 00425 * @retval None 00426 * 00427 */ 00428 void IMU::BMP180_CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_t InVal, int32_t *pOutVal) 00429 { 00430 uint8_t i; 00431 00432 *(pAvgBuffer + ((*pIndex) ++)) = InVal; 00433 *pIndex &= 0x07; 00434 00435 *pOutVal = 0; 00436 for(i = 0; i < 8; i ++) 00437 { 00438 *pOutVal += *(pAvgBuffer + i); 00439 } 00440 *pOutVal >>= 3; 00441 } 00442 00443 /** 00444 * @brief Start temperature measurement 00445 * @param None 00446 * @retval None 00447 */ 00448 void IMU::BMP180_StartTemperatureMeasurement(void) 00449 { 00450 //BMP180_WriteReg(CONTROL, READ_TEMPERATURE); 00451 I2C_WriteOneByte(BMP180_ADDR, CONTROL, READ_TEMPERATURE); 00452 } 00453 00454 /** 00455 * @brief Start pressure measurement 00456 * @param None 00457 * @retval None 00458 */ 00459 void IMU::BMP180_StartPressureMeasurement(void) 00460 { 00461 //BMP180_WriteReg(CONTROL, READ_PRESSURE + (_oss << 6)); 00462 I2C_WriteOneByte(BMP180_ADDR, CONTROL, READ_PRESSURE + (_oss << 6)); 00463 } 00464 00465 /** 00466 * @brief Read uncompensated temperature 00467 * @param None 00468 * @retval None 00469 */ 00470 void IMU::BMP180_ReadUncompensatedTemperature(void) 00471 { 00472 char RegBuff[2]; 00473 BMP180_ReadReg(CONTROL_OUTPUT, 2, &RegBuff[0]); 00474 00475 UT = ((int32_t)RegBuff[0] << 8) + (int32_t)RegBuff[1]; 00476 } 00477 00478 /** 00479 * @brief Read uncompensated pressure 00480 * @param None 00481 * @retval None 00482 */ 00483 void IMU::BMP180_ReadUncompensatedPressure(void) 00484 { 00485 char RegBuff[3]; 00486 00487 BMP180_ReadReg(CONTROL_OUTPUT, 3, &RegBuff[0]); 00488 00489 UP = (((int32_t)RegBuff[0] << 16) + ((int32_t)RegBuff[1] << 8) + ((int32_t)RegBuff[2])) >> (8 -_oss); // uncompensated pressure value 00490 } 00491 00492 /** 00493 * @brief Calculate true temperature 00494 * @param *pTrueTemperature: true temperature 00495 * @retval None 00496 */ 00497 void IMU::BMP180_CalculateTrueTemperature(int32_t *pTrueTemperature) 00498 { 00499 int32_t X1, X2; 00500 00501 X1 = ((UT - AC6) * AC5) >> 15; 00502 X2 = (MC << 11) / (X1 + MD); 00503 B5 = X1 + X2; 00504 *pTrueTemperature = (B5 + 8) >> 4; 00505 } 00506 00507 /** 00508 * @brief Calculate true pressure 00509 * @param *pTruePressure: true pressure 00510 * @retval None 00511 */ 00512 void IMU::BMP180_CalculateTruePressure(int32_t *pTruePressure) 00513 { 00514 int32_t X1, X2, X3, B3, B6, P, Temp; 00515 uint32_t B4, B7; 00516 00517 B6 = B5 - 4000; 00518 X1 = (B2* ((B6 * B6) >> 12)) >> 11; 00519 X2 = AC2 * B6 >> 11; 00520 X3 = X1 + X2; 00521 Temp = (((int32_t)AC1 << 2) + X3) << _oss; 00522 B3 = (Temp + 2) >> 2; 00523 X1 = (AC3 * B6) >> 13; 00524 X2 = (B1 * (B6 * B6 >> 12)) >> 16; 00525 X3 = ((X1 + X2) + 2) >> 2; 00526 B4 = (AC4 * (uint32_t) (X3 + 32768)) >> 15; 00527 B7 = ((uint32_t)UP - B3) * (50000 >> _oss); 00528 if(B7 < 0x80000000) 00529 { 00530 P = (B7 << 1) / B4; 00531 } 00532 else 00533 { 00534 P = (B7 / B4) << 1; 00535 } 00536 00537 X1 = (P >> 8) * (P >> 8); 00538 X1 = (X1 * 3038) >> 16; 00539 X2 = (-7357 * P) >> 16; 00540 00541 *pTruePressure = P + ((X1 + X2 + 3791) >> 4); 00542 } 00543 00544 /** 00545 * @brief Calculating average value of pressure 00546 * @param *pVal: the average value of pressure 00547 * @retval None 00548 */ 00549 void IMU::BMP180_LocalpressureAvg(int32_t *pVal) 00550 { 00551 uint8_t i; 00552 int32_t Sum = 0; 00553 00554 for(i = 0; i < 5; i ++) 00555 { 00556 BMP180_StartTemperatureMeasurement(); 00557 Thread::wait(5);//delay_ms(5); //4.5ms 324 00558 BMP180_ReadUncompensatedTemperature(); 00559 BMP180_StartPressureMeasurement(); 00560 Thread::wait(5);//delay_ms(8);//7.5ms 540 00561 BMP180_ReadUncompensatedPressure(); 00562 BMP180_CalculateTruePressure(&PressureVal); 00563 BMP180_CalculateTrueTemperature(&TemperatureVal); 00564 00565 if(i >= 2) 00566 { 00567 Sum += PressureVal; 00568 } 00569 } 00570 *pVal = Sum / 3; 00571 } 00572 00573 /** 00574 * @brief Calculating pressure at sea level 00575 * @param None 00576 * @retval None 00577 */ 00578 void IMU::BMP180_PressureAtSeaLevel(void) 00579 { 00580 float Temp = 0.0f; 00581 00582 BMP180_LocalpressureAvg(&PressureVal); 00583 00584 Temp = (float)LOCAL_ADS_ALTITUDE / 4433000; 00585 Temp = (float)pow((1 - Temp), 5.255f); 00586 Pressure0 = (PressureVal - PRESSURE_OFFSET) / Temp;// 00587 } 00588 00589 /** 00590 * @brief Calculating absolute altitude 00591 * @param *pAltitude: absolute altitude 00592 * @param PressureVal: the pressure at the absolute altitude 00593 * @retval None 00594 */ 00595 void IMU::BMP180_CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal) 00596 { 00597 *pAltitude = 4433000 * (1 - pow((float)(PressureVal / (float)Pressure0), 0.1903f)); 00598 } 00599 00600 /** 00601 * @brief Read calibration data from the EEPROM of the BMP180 00602 * @param None 00603 * @retval None 00604 */ 00605 void IMU::BMP180_ReadCalibrationData(void) 00606 { 00607 char RegBuff[2]; 00608 00609 BMP180_ReadReg(CAL_AC1, 2, RegBuff); 00610 AC1 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00611 00612 BMP180_ReadReg(CAL_AC2, 2, RegBuff); 00613 AC2 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00614 00615 BMP180_ReadReg(CAL_AC3, 2, RegBuff); 00616 AC3 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00617 00618 BMP180_ReadReg(CAL_AC4, 2, RegBuff); 00619 AC4 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1])); 00620 00621 BMP180_ReadReg(CAL_AC5, 2, RegBuff); 00622 AC5 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1])); 00623 00624 BMP180_ReadReg(CAL_AC6, 2, RegBuff); 00625 AC6 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1])); 00626 00627 BMP180_ReadReg(CAL_B1, 2, RegBuff); 00628 B1 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00629 00630 BMP180_ReadReg(CAL_B2, 2, RegBuff); 00631 B2 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00632 00633 BMP180_ReadReg(CAL_MB, 2, RegBuff); 00634 MB = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00635 00636 BMP180_ReadReg(CAL_MC, 2, RegBuff); 00637 MC = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00638 00639 BMP180_ReadReg(CAL_MD, 2, RegBuff); 00640 MD = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1])); 00641 } 00642 00643 /** 00644 * @brief Configures hardware pressure sampling accuracy modes 00645 * @param None 00646 * @retval None 00647 */ 00648 void IMU::BMP180_SetOversample(void) 00649 { 00650 _oss = MODE_ULTRA_HIGHRES; 00651 } 00652 00653 /** 00654 * @brief initializes BMP180 00655 * @param None 00656 * @retval None 00657 */ 00658 void IMU::BMP180_Init(void) 00659 { 00660 BMP180_SetOversample(); 00661 BMP180_ReadCalibrationData(); 00662 //BMP180_PressureAtSeaLevel(); 00663 } 00664 00665 /** 00666 * @brief Calculation of pressure and temperature and altitude for BMP180 00667 * @param None 00668 * @retval None 00669 */ 00670 void IMU::CalTemperatureAndPressureAndAltitude(void) 00671 { 00672 static uint8_t State = START_TEMPERATURE_MEASUREMENT; 00673 static BMP180_AvgTypeDef BMP180_Filter[3]; 00674 int32_t PVal,AVal, TVal; 00675 00676 switch(State) 00677 { 00678 case START_TEMPERATURE_MEASUREMENT: 00679 BMP180_StartTemperatureMeasurement(); 00680 Thread::wait(5);//delay_ms(5); //4.5ms 00681 State = READ_UT_AND_START_PRESSURE_MEASUREMENT; 00682 break; 00683 00684 case READ_UT_AND_START_PRESSURE_MEASUREMENT: 00685 BMP180_ReadUncompensatedTemperature(); 00686 BMP180_StartPressureMeasurement(); 00687 Thread::wait(10);//delay_ms(10);//7.5ms 00688 State = READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE; 00689 break; 00690 00691 case READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE: 00692 BMP180_ReadUncompensatedPressure(); 00693 BMP180_CalculateTruePressure(&PVal); 00694 BMP180_CalAvgValue(&BMP180_Filter[0].Index, BMP180_Filter[0].AvgBuffer, PVal - PRESSURE_OFFSET, &PressureVal); 00695 00696 BMP180_CalculateAbsoluteAltitude(&AVal, PVal - PRESSURE_OFFSET); 00697 BMP180_CalAvgValue(&BMP180_Filter[1].Index, BMP180_Filter[1].AvgBuffer, AVal, &AltitudeVal); 00698 00699 BMP180_CalculateTrueTemperature(&TVal); 00700 BMP180_CalAvgValue(&BMP180_Filter[2].Index, BMP180_Filter[2].AvgBuffer, TVal, &TemperatureVal); 00701 00702 State = START_TEMPERATURE_MEASUREMENT; 00703 break; 00704 00705 default: 00706 break; 00707 } 00708 }
Generated on Thu Jul 14 2022 18:58:55 by
1.7.2