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

Committer:
adaphoto
Date:
Thu Jun 21 08:46:30 2018 +0000
Revision:
1:7cf70724bdb0
Parent:
0:35bba382318b
???BMP180????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adaphoto 0:35bba382318b 1 #include "imu.h"
adaphoto 0:35bba382318b 2
adaphoto 0:35bba382318b 3 I2C i2c(I2C_SDA,I2C_SCL);
adaphoto 0:35bba382318b 4
adaphoto 0:35bba382318b 5 /**
adaphoto 0:35bba382318b 6 * @brief invSqrt
adaphoto 0:35bba382318b 7 * @param
adaphoto 0:35bba382318b 8 * @retval
adaphoto 0:35bba382318b 9 */
adaphoto 0:35bba382318b 10 float IMU::invSqrt(float x)
adaphoto 0:35bba382318b 11 {
adaphoto 0:35bba382318b 12 float halfx = 0.5f * x;
adaphoto 0:35bba382318b 13 float y = x;
adaphoto 0:35bba382318b 14
adaphoto 0:35bba382318b 15 long i = *(long*)&y; //get bits for floating value
adaphoto 0:35bba382318b 16 i = 0x5f3759df - (i >> 1); //gives initial guss you
adaphoto 0:35bba382318b 17 y = *(float*)&i; //convert bits back to float
adaphoto 0:35bba382318b 18 y = y * (1.5f - (halfx * y * y)); //newtop step, repeating increases accuracy
adaphoto 0:35bba382318b 19
adaphoto 0:35bba382318b 20 return y;
adaphoto 0:35bba382318b 21 }
adaphoto 0:35bba382318b 22
adaphoto 0:35bba382318b 23 /**
adaphoto 0:35bba382318b 24 * @brief initializes IMU
adaphoto 0:35bba382318b 25 * @param None
adaphoto 0:35bba382318b 26 * @retval None
adaphoto 0:35bba382318b 27 */
adaphoto 0:35bba382318b 28 void IMU::IMU_Init()
adaphoto 0:35bba382318b 29 {
adaphoto 0:35bba382318b 30 MPU9250_Init();
adaphoto 1:7cf70724bdb0 31 BMP180_Init();
adaphoto 0:35bba382318b 32
adaphoto 0:35bba382318b 33 q0 = 1.0f;
adaphoto 0:35bba382318b 34 q1 = 0.0f;
adaphoto 0:35bba382318b 35 q2 = 0.0f;
adaphoto 0:35bba382318b 36 q3 = 0.0f;
adaphoto 0:35bba382318b 37 }
adaphoto 0:35bba382318b 38
adaphoto 0:35bba382318b 39 /**
adaphoto 0:35bba382318b 40 * @brief Updata attitude and heading
adaphoto 0:35bba382318b 41 * @param ax: accelerometer X
adaphoto 0:35bba382318b 42 * @param ay: accelerometer Y
adaphoto 0:35bba382318b 43 * @param az: accelerometer Z
adaphoto 0:35bba382318b 44 * @param gx: gyroscopes X
adaphoto 0:35bba382318b 45 * @param gy: gyroscopes Y
adaphoto 0:35bba382318b 46 * @param gz: gyroscopes Z
adaphoto 0:35bba382318b 47 * @param mx: magnetometer X
adaphoto 0:35bba382318b 48 * @param my: magnetometer Y
adaphoto 0:35bba382318b 49 * @param mz: magnetometer Z
adaphoto 0:35bba382318b 50 * @retval None
adaphoto 0:35bba382318b 51 */
adaphoto 0:35bba382318b 52 void IMU::IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
adaphoto 0:35bba382318b 53 {
adaphoto 0:35bba382318b 54 float norm;
adaphoto 0:35bba382318b 55 float hx, hy, hz, bx, bz;
adaphoto 0:35bba382318b 56 float vx, vy, vz, wx, wy, wz;
adaphoto 0:35bba382318b 57 float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;
adaphoto 0:35bba382318b 58 float ex, ey, ez, halfT = 0.024f;
adaphoto 0:35bba382318b 59
adaphoto 0:35bba382318b 60 float q0q0 = q0 * q0;
adaphoto 0:35bba382318b 61 float q0q1 = q0 * q1;
adaphoto 0:35bba382318b 62 float q0q2 = q0 * q2;
adaphoto 0:35bba382318b 63 float q0q3 = q0 * q3;
adaphoto 0:35bba382318b 64 float q1q1 = q1 * q1;
adaphoto 0:35bba382318b 65 float q1q2 = q1 * q2;
adaphoto 0:35bba382318b 66 float q1q3 = q1 * q3;
adaphoto 0:35bba382318b 67 float q2q2 = q2 * q2;
adaphoto 0:35bba382318b 68 float q2q3 = q2 * q3;
adaphoto 0:35bba382318b 69 float q3q3 = q3 * q3;
adaphoto 0:35bba382318b 70
adaphoto 0:35bba382318b 71 norm = invSqrt(ax * ax + ay * ay + az * az);
adaphoto 0:35bba382318b 72 ax = ax * norm;
adaphoto 0:35bba382318b 73 ay = ay * norm;
adaphoto 0:35bba382318b 74 az = az * norm;
adaphoto 0:35bba382318b 75
adaphoto 0:35bba382318b 76 norm = invSqrt(mx * mx + my * my + mz * mz);
adaphoto 0:35bba382318b 77 mx = mx * norm;
adaphoto 0:35bba382318b 78 my = my * norm;
adaphoto 0:35bba382318b 79 mz = mz * norm;
adaphoto 0:35bba382318b 80
adaphoto 0:35bba382318b 81 // compute reference direction of flux
adaphoto 0:35bba382318b 82 hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
adaphoto 0:35bba382318b 83 hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
adaphoto 0:35bba382318b 84 hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
adaphoto 0:35bba382318b 85 bx = sqrt((hx * hx) + (hy * hy));
adaphoto 0:35bba382318b 86 bz = hz;
adaphoto 0:35bba382318b 87
adaphoto 0:35bba382318b 88 // estimated direction of gravity and flux (v and w)
adaphoto 0:35bba382318b 89 vx = 2 * (q1q3 - q0q2);
adaphoto 0:35bba382318b 90 vy = 2 * (q0q1 + q2q3);
adaphoto 0:35bba382318b 91 vz = q0q0 - q1q1 - q2q2 + q3q3;
adaphoto 0:35bba382318b 92 wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
adaphoto 0:35bba382318b 93 wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
adaphoto 0:35bba382318b 94 wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);
adaphoto 0:35bba382318b 95
adaphoto 0:35bba382318b 96 // error is sum of cross product between reference direction of fields and direction measured by sensors
adaphoto 0:35bba382318b 97 ex = (ay * vz - az * vy) + (my * wz - mz * wy);
adaphoto 0:35bba382318b 98 ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
adaphoto 0:35bba382318b 99 ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
adaphoto 0:35bba382318b 100
adaphoto 0:35bba382318b 101 if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
adaphoto 0:35bba382318b 102 {
adaphoto 0:35bba382318b 103 exInt = exInt + ex * Ki * halfT;
adaphoto 0:35bba382318b 104 eyInt = eyInt + ey * Ki * halfT;
adaphoto 0:35bba382318b 105 ezInt = ezInt + ez * Ki * halfT;
adaphoto 0:35bba382318b 106
adaphoto 0:35bba382318b 107 gx = gx + Kp * ex + exInt;
adaphoto 0:35bba382318b 108 gy = gy + Kp * ey + eyInt;
adaphoto 0:35bba382318b 109 gz = gz + Kp * ez + ezInt;
adaphoto 0:35bba382318b 110 }
adaphoto 0:35bba382318b 111
adaphoto 0:35bba382318b 112 q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
adaphoto 0:35bba382318b 113 q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
adaphoto 0:35bba382318b 114 q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
adaphoto 0:35bba382318b 115 q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
adaphoto 0:35bba382318b 116
adaphoto 0:35bba382318b 117 norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
adaphoto 0:35bba382318b 118 q0 = q0 * norm;
adaphoto 0:35bba382318b 119 q1 = q1 * norm;
adaphoto 0:35bba382318b 120 q2 = q2 * norm;
adaphoto 0:35bba382318b 121 q3 = q3 * norm;
adaphoto 0:35bba382318b 122 }
adaphoto 0:35bba382318b 123
adaphoto 0:35bba382318b 124 /**
adaphoto 0:35bba382318b 125 * @brief Get quaters
adaphoto 0:35bba382318b 126 * @param None
adaphoto 0:35bba382318b 127 * @retval None
adaphoto 0:35bba382318b 128 */
adaphoto 0:35bba382318b 129 void IMU::IMU_GetQuater(void)
adaphoto 0:35bba382318b 130 {
adaphoto 0:35bba382318b 131 float MotionVal[9];
adaphoto 0:35bba382318b 132
adaphoto 0:35bba382318b 133 MPU9250_READ_ACCEL();
adaphoto 0:35bba382318b 134 MPU9250_READ_GYRO();
adaphoto 0:35bba382318b 135 MPU9250_READ_MAG();
adaphoto 0:35bba382318b 136
adaphoto 0:35bba382318b 137 MotionVal[0]=gyro[0]/32.8;
adaphoto 0:35bba382318b 138 MotionVal[1]=gyro[1]/32.8;
adaphoto 0:35bba382318b 139 MotionVal[2]=gyro[2]/32.8;
adaphoto 0:35bba382318b 140 MotionVal[3]=accel[0];
adaphoto 0:35bba382318b 141 MotionVal[4]=accel[1];
adaphoto 0:35bba382318b 142 MotionVal[5]=accel[2];
adaphoto 0:35bba382318b 143 MotionVal[6]=magn[0];
adaphoto 0:35bba382318b 144 MotionVal[7]=magn[1];
adaphoto 0:35bba382318b 145 MotionVal[8]=magn[2];
adaphoto 0:35bba382318b 146
adaphoto 0:35bba382318b 147 IMU_AHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175,
adaphoto 0:35bba382318b 148 (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
adaphoto 0:35bba382318b 149 }
adaphoto 0:35bba382318b 150
adaphoto 0:35bba382318b 151 /**
adaphoto 0:35bba382318b 152 * @brief Get Yaw Pitch Roll
adaphoto 0:35bba382318b 153 * @param None
adaphoto 0:35bba382318b 154 * @retval None
adaphoto 0:35bba382318b 155 */
adaphoto 0:35bba382318b 156 void IMU::IMU_GetYawPitchRoll(float *Angles)
adaphoto 0:35bba382318b 157 {
adaphoto 0:35bba382318b 158
adaphoto 0:35bba382318b 159 int x_tmp,y_tmp;
adaphoto 0:35bba382318b 160
adaphoto 0:35bba382318b 161 IMU_GetQuater();
adaphoto 0:35bba382318b 162
adaphoto 0:35bba382318b 163 x_tmp = magn[0];
adaphoto 0:35bba382318b 164 y_tmp = magn[1];
adaphoto 0:35bba382318b 165 if(x_tmp>0x7fff)x_tmp-=0xffff;
adaphoto 0:35bba382318b 166 if(y_tmp>0x7fff)y_tmp-=0xffff;
adaphoto 0:35bba382318b 167
adaphoto 0:35bba382318b 168 Angles[1] = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
adaphoto 0:35bba382318b 169 Angles[2] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
adaphoto 0:35bba382318b 170 Angles[0] = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3; //Yaw
adaphoto 0:35bba382318b 171 Angles[3] = atan2((double)(y_tmp) , (double)(x_tmp)) * (180 / 3.14159265) + 180; // Compass angle in degrees
adaphoto 0:35bba382318b 172
adaphoto 0:35bba382318b 173
adaphoto 0:35bba382318b 174 }
adaphoto 0:35bba382318b 175
adaphoto 0:35bba382318b 176 /************************************************************/
adaphoto 0:35bba382318b 177 //下面的代码是MPU9250的各种操作
adaphoto 0:35bba382318b 178 /************************************************************/
adaphoto 0:35bba382318b 179 void IMU::I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data)
adaphoto 0:35bba382318b 180 {
adaphoto 0:35bba382318b 181 char data_write[2];
adaphoto 0:35bba382318b 182
adaphoto 0:35bba382318b 183 data_write[0] = RegAddr;
adaphoto 0:35bba382318b 184 data_write[1] = Data;
adaphoto 0:35bba382318b 185
adaphoto 0:35bba382318b 186 i2c.write(DevAddr,data_write,2,0);
adaphoto 0:35bba382318b 187 }
adaphoto 0:35bba382318b 188
adaphoto 0:35bba382318b 189 uint8_t IMU::I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr)
adaphoto 0:35bba382318b 190 {
adaphoto 0:35bba382318b 191 char data_write[2];
adaphoto 0:35bba382318b 192 char data_read[2];
adaphoto 0:35bba382318b 193 char data;
adaphoto 0:35bba382318b 194
adaphoto 0:35bba382318b 195 data_write[0] = RegAddr;
adaphoto 0:35bba382318b 196
adaphoto 0:35bba382318b 197 i2c.write(DevAddr,data_write,1,1);
adaphoto 0:35bba382318b 198 i2c.read(DevAddr,data_read,1,0);
adaphoto 0:35bba382318b 199
adaphoto 0:35bba382318b 200 data = data_read[0];
adaphoto 0:35bba382318b 201
adaphoto 0:35bba382318b 202 return data;
adaphoto 0:35bba382318b 203 }
adaphoto 1:7cf70724bdb0 204
adaphoto 1:7cf70724bdb0 205 void IMU::BMP180_ReadReg(uint8_t RegAddr, uint8_t Num, char *pBuffer)
adaphoto 1:7cf70724bdb0 206 {
adaphoto 1:7cf70724bdb0 207 char data_write[2];
adaphoto 1:7cf70724bdb0 208 char DevAddr = BMP180_ADDR;
adaphoto 1:7cf70724bdb0 209
adaphoto 1:7cf70724bdb0 210 data_write[0] = RegAddr;
adaphoto 1:7cf70724bdb0 211 i2c.write(DevAddr, data_write, 1, 1);
adaphoto 1:7cf70724bdb0 212 i2c.read(DevAddr, pBuffer, Num, 0);
adaphoto 1:7cf70724bdb0 213 }
adaphoto 0:35bba382318b 214 /**
adaphoto 0:35bba382318b 215 * @brief Initializes MPU9250
adaphoto 0:35bba382318b 216 * @param None
adaphoto 0:35bba382318b 217 * @retval None
adaphoto 0:35bba382318b 218 */
adaphoto 0:35bba382318b 219 void IMU::MPU9250_Init()
adaphoto 0:35bba382318b 220 {
adaphoto 0:35bba382318b 221 I2C_WriteOneByte(GYRO_ADDRESS,PWR_MGMT_1, 0x00);
adaphoto 0:35bba382318b 222 I2C_WriteOneByte(GYRO_ADDRESS,SMPLRT_DIV, 0x07);
adaphoto 0:35bba382318b 223 I2C_WriteOneByte(GYRO_ADDRESS,CONFIG, 0x06);
adaphoto 0:35bba382318b 224 I2C_WriteOneByte(GYRO_ADDRESS,GYRO_CONFIG, 0x10);
adaphoto 0:35bba382318b 225 I2C_WriteOneByte(GYRO_ADDRESS,ACCEL_CONFIG, 0x01);
adaphoto 0:35bba382318b 226
adaphoto 0:35bba382318b 227 Thread::wait(10);
adaphoto 0:35bba382318b 228
adaphoto 0:35bba382318b 229 if(MPU9250_Check())
adaphoto 0:35bba382318b 230 {
adaphoto 0:35bba382318b 231 printf("\r\n MPU9255 Ready!\n");
adaphoto 0:35bba382318b 232 }
adaphoto 0:35bba382318b 233 else
adaphoto 0:35bba382318b 234 {
adaphoto 0:35bba382318b 235 printf("\r\n MPU9255 Erro!\n");
adaphoto 0:35bba382318b 236 }
adaphoto 0:35bba382318b 237
adaphoto 0:35bba382318b 238 MPU9250_InitGyrOffset();
adaphoto 0:35bba382318b 239 }
adaphoto 0:35bba382318b 240
adaphoto 0:35bba382318b 241 /**
adaphoto 0:35bba382318b 242 * @brief Digital filter
adaphoto 0:35bba382318b 243 * @param *pIndex:
adaphoto 0:35bba382318b 244 * @param *pAvgBuffer:
adaphoto 0:35bba382318b 245 * @param InVal:
adaphoto 0:35bba382318b 246 * @param pOutVal:
adaphoto 0:35bba382318b 247 *
adaphoto 0:35bba382318b 248 * @retval None
adaphoto 0:35bba382318b 249 *
adaphoto 0:35bba382318b 250 */
adaphoto 0:35bba382318b 251 void IMU::MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal)
adaphoto 0:35bba382318b 252 {
adaphoto 0:35bba382318b 253 uint8_t i;
adaphoto 0:35bba382318b 254
adaphoto 0:35bba382318b 255 *(pAvgBuffer + ((*pIndex) ++)) = InVal;
adaphoto 0:35bba382318b 256 *pIndex &= 0x07;
adaphoto 0:35bba382318b 257
adaphoto 0:35bba382318b 258 *pOutVal = 0;
adaphoto 0:35bba382318b 259 for(i = 0; i < 8; i ++)
adaphoto 0:35bba382318b 260 {
adaphoto 0:35bba382318b 261 *pOutVal += *(pAvgBuffer + i);
adaphoto 0:35bba382318b 262 }
adaphoto 0:35bba382318b 263 *pOutVal >>= 3;
adaphoto 0:35bba382318b 264 }
adaphoto 0:35bba382318b 265
adaphoto 0:35bba382318b 266 /**
adaphoto 0:35bba382318b 267 * @brief Get accelerometer datas
adaphoto 0:35bba382318b 268 * @param None
adaphoto 0:35bba382318b 269 * @retval None
adaphoto 0:35bba382318b 270 */
adaphoto 0:35bba382318b 271 void IMU::MPU9250_READ_ACCEL(void)
adaphoto 0:35bba382318b 272 {
adaphoto 0:35bba382318b 273 uint8_t i;
adaphoto 0:35bba382318b 274 int16_t InBuffer[3] = {0};
adaphoto 0:35bba382318b 275 static int32_t OutBuffer[3] = {0};
adaphoto 0:35bba382318b 276 static MPU9250_AvgTypeDef MPU9250_Filter[3];
adaphoto 0:35bba382318b 277
adaphoto 0:35bba382318b 278 BUF[0]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_L);
adaphoto 0:35bba382318b 279 BUF[1]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_H);
adaphoto 0:35bba382318b 280 InBuffer[0]= (BUF[1]<<8)|BUF[0];
adaphoto 0:35bba382318b 281
adaphoto 0:35bba382318b 282 BUF[2]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_L);
adaphoto 0:35bba382318b 283 BUF[3]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_H);
adaphoto 0:35bba382318b 284 InBuffer[1]= (BUF[3]<<8)|BUF[2];
adaphoto 0:35bba382318b 285
adaphoto 0:35bba382318b 286 BUF[4]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_L);
adaphoto 0:35bba382318b 287 BUF[5]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_H);
adaphoto 0:35bba382318b 288 InBuffer[2]= (BUF[5]<<8)|BUF[4];
adaphoto 0:35bba382318b 289
adaphoto 0:35bba382318b 290 for(i = 0; i < 3; i ++)
adaphoto 0:35bba382318b 291 {
adaphoto 0:35bba382318b 292 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
adaphoto 0:35bba382318b 293 }
adaphoto 0:35bba382318b 294 accel[0] = *(OutBuffer + 0);
adaphoto 0:35bba382318b 295 accel[1] = *(OutBuffer + 1);
adaphoto 0:35bba382318b 296 accel[2] = *(OutBuffer + 2);
adaphoto 0:35bba382318b 297 }
adaphoto 0:35bba382318b 298
adaphoto 0:35bba382318b 299 /**
adaphoto 0:35bba382318b 300 * @brief Get gyroscopes datas
adaphoto 0:35bba382318b 301 * @param None
adaphoto 0:35bba382318b 302 * @retval None
adaphoto 0:35bba382318b 303 */
adaphoto 0:35bba382318b 304 void IMU::MPU9250_READ_GYRO(void)
adaphoto 0:35bba382318b 305 {
adaphoto 0:35bba382318b 306 uint8_t i;
adaphoto 0:35bba382318b 307 int16_t InBuffer[3] = {0};
adaphoto 0:35bba382318b 308 static int32_t OutBuffer[3] = {0};
adaphoto 0:35bba382318b 309 static MPU9250_AvgTypeDef MPU9250_Filter[3];
adaphoto 0:35bba382318b 310
adaphoto 0:35bba382318b 311 BUF[0]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_L);
adaphoto 0:35bba382318b 312 BUF[1]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_H);
adaphoto 0:35bba382318b 313 InBuffer[0]= (BUF[1]<<8)|BUF[0];
adaphoto 0:35bba382318b 314
adaphoto 0:35bba382318b 315 BUF[2]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_L);
adaphoto 0:35bba382318b 316 BUF[3]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_H);
adaphoto 0:35bba382318b 317 InBuffer[1] = (BUF[3]<<8)|BUF[2];
adaphoto 0:35bba382318b 318
adaphoto 0:35bba382318b 319 BUF[4]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_L);
adaphoto 0:35bba382318b 320 BUF[5]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_H);
adaphoto 0:35bba382318b 321 InBuffer[2] = (BUF[5]<<8)|BUF[4];
adaphoto 0:35bba382318b 322
adaphoto 0:35bba382318b 323 for(i = 0; i < 3; i ++)
adaphoto 0:35bba382318b 324 {
adaphoto 0:35bba382318b 325 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
adaphoto 0:35bba382318b 326 }
adaphoto 0:35bba382318b 327 gyro[0] = *(OutBuffer + 0) - MPU9250_Offset.X;
adaphoto 0:35bba382318b 328 gyro[1] = *(OutBuffer + 1) - MPU9250_Offset.Y;
adaphoto 0:35bba382318b 329 gyro[2] = *(OutBuffer + 2) - MPU9250_Offset.Z;
adaphoto 0:35bba382318b 330 }
adaphoto 0:35bba382318b 331
adaphoto 0:35bba382318b 332 /**
adaphoto 0:35bba382318b 333 * @brief Get compass datas
adaphoto 0:35bba382318b 334 * @param None
adaphoto 0:35bba382318b 335 * @retval None
adaphoto 0:35bba382318b 336 */
adaphoto 0:35bba382318b 337 void IMU::MPU9250_READ_MAG(void)
adaphoto 0:35bba382318b 338 {
adaphoto 0:35bba382318b 339 uint8_t i;
adaphoto 0:35bba382318b 340 int16_t InBuffer[3] = {0};
adaphoto 0:35bba382318b 341 static int32_t OutBuffer[3] = {0};
adaphoto 0:35bba382318b 342 static MPU9250_AvgTypeDef MPU9250_Filter[3];
adaphoto 0:35bba382318b 343
adaphoto 0:35bba382318b 344 I2C_WriteOneByte(GYRO_ADDRESS,0x37,0x02);//turn on Bypass Mode
adaphoto 0:35bba382318b 345 Thread::wait(10);
adaphoto 0:35bba382318b 346 I2C_WriteOneByte(MAG_ADDRESS,0x0A,0x01);
adaphoto 0:35bba382318b 347 Thread::wait(10);
adaphoto 0:35bba382318b 348 BUF[0]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_L);
adaphoto 0:35bba382318b 349 BUF[1]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_H);
adaphoto 0:35bba382318b 350 InBuffer[1] =(BUF[1]<<8)|BUF[0];
adaphoto 0:35bba382318b 351
adaphoto 0:35bba382318b 352 BUF[2]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_L);
adaphoto 0:35bba382318b 353 BUF[3]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_H);
adaphoto 0:35bba382318b 354 InBuffer[0] = (BUF[3]<<8)|BUF[2];
adaphoto 0:35bba382318b 355
adaphoto 0:35bba382318b 356 BUF[4]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_L);
adaphoto 0:35bba382318b 357 BUF[5]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_H);
adaphoto 0:35bba382318b 358 InBuffer[2] = (BUF[5]<<8)|BUF[4];
adaphoto 0:35bba382318b 359 InBuffer[2] = -InBuffer[2];
adaphoto 0:35bba382318b 360
adaphoto 0:35bba382318b 361 for(i = 0; i < 3; i ++)
adaphoto 0:35bba382318b 362 {
adaphoto 0:35bba382318b 363 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
adaphoto 0:35bba382318b 364 }
adaphoto 0:35bba382318b 365 magn[0] = *(OutBuffer + 0)-MPU9250_Magn_Offset.X_Off_Err;
adaphoto 0:35bba382318b 366 magn[1] = *(OutBuffer + 1)-MPU9250_Magn_Offset.Y_Off_Err;
adaphoto 0:35bba382318b 367 magn[2] = *(OutBuffer + 2)-MPU9250_Magn_Offset.Z_Off_Err;
adaphoto 0:35bba382318b 368 }
adaphoto 0:35bba382318b 369
adaphoto 0:35bba382318b 370 /**
adaphoto 0:35bba382318b 371 * @brief Check MPU9250,ensure communication succeed
adaphoto 0:35bba382318b 372 * @param None
adaphoto 0:35bba382318b 373 * @retval true: communicate succeed
adaphoto 0:35bba382318b 374 * false: communicate fualt
adaphoto 0:35bba382318b 375 */
adaphoto 0:35bba382318b 376 bool IMU::MPU9250_Check(void)
adaphoto 0:35bba382318b 377 {
adaphoto 0:35bba382318b 378 if(WHO_AM_I_VAL == I2C_ReadOneByte(DEFAULT_ADDRESS, WHO_AM_I))
adaphoto 0:35bba382318b 379 {
adaphoto 0:35bba382318b 380 return true;
adaphoto 0:35bba382318b 381 }
adaphoto 0:35bba382318b 382 else
adaphoto 0:35bba382318b 383 {
adaphoto 0:35bba382318b 384 return false;
adaphoto 0:35bba382318b 385 }
adaphoto 0:35bba382318b 386 }
adaphoto 0:35bba382318b 387
adaphoto 0:35bba382318b 388 /**
adaphoto 0:35bba382318b 389 * @brief Initializes gyroscopes offset
adaphoto 0:35bba382318b 390 * @param None
adaphoto 0:35bba382318b 391 * @retval None
adaphoto 0:35bba382318b 392 */
adaphoto 0:35bba382318b 393 void IMU::MPU9250_InitGyrOffset(void)
adaphoto 0:35bba382318b 394 {
adaphoto 0:35bba382318b 395 uint8_t i;
adaphoto 0:35bba382318b 396 int32_t TempGx = 0, TempGy = 0, TempGz = 0;
adaphoto 0:35bba382318b 397
adaphoto 0:35bba382318b 398 for(i = 0; i < 32; i ++)
adaphoto 0:35bba382318b 399 {
adaphoto 0:35bba382318b 400 MPU9250_READ_GYRO();
adaphoto 0:35bba382318b 401
adaphoto 0:35bba382318b 402 TempGx += gyro[0];
adaphoto 0:35bba382318b 403 TempGy += gyro[1];
adaphoto 0:35bba382318b 404 TempGz += gyro[2];
adaphoto 0:35bba382318b 405
adaphoto 0:35bba382318b 406 Thread::wait(1);
adaphoto 0:35bba382318b 407 }
adaphoto 0:35bba382318b 408
adaphoto 0:35bba382318b 409 MPU9250_Offset.X = TempGx >> 5;
adaphoto 0:35bba382318b 410 MPU9250_Offset.Y = TempGy >> 5;
adaphoto 0:35bba382318b 411 MPU9250_Offset.Z = TempGz >> 5;
adaphoto 0:35bba382318b 412
adaphoto 1:7cf70724bdb0 413 }
adaphoto 1:7cf70724bdb0 414
adaphoto 1:7cf70724bdb0 415 //******************************************************
adaphoto 1:7cf70724bdb0 416 //下面的代码是BMP180
adaphoto 1:7cf70724bdb0 417 //******************************************************
adaphoto 1:7cf70724bdb0 418 /**
adaphoto 1:7cf70724bdb0 419 * @brief Digital filter
adaphoto 1:7cf70724bdb0 420 * @param *pIndex:
adaphoto 1:7cf70724bdb0 421 * @param *pAvgBuffer:
adaphoto 1:7cf70724bdb0 422 * @param InVal:
adaphoto 1:7cf70724bdb0 423 * @param pOutVal:
adaphoto 1:7cf70724bdb0 424 *
adaphoto 1:7cf70724bdb0 425 * @retval None
adaphoto 1:7cf70724bdb0 426 *
adaphoto 1:7cf70724bdb0 427 */
adaphoto 1:7cf70724bdb0 428 void IMU::BMP180_CalAvgValue(uint8_t *pIndex, int32_t *pAvgBuffer, int32_t InVal, int32_t *pOutVal)
adaphoto 1:7cf70724bdb0 429 {
adaphoto 1:7cf70724bdb0 430 uint8_t i;
adaphoto 1:7cf70724bdb0 431
adaphoto 1:7cf70724bdb0 432 *(pAvgBuffer + ((*pIndex) ++)) = InVal;
adaphoto 1:7cf70724bdb0 433 *pIndex &= 0x07;
adaphoto 1:7cf70724bdb0 434
adaphoto 1:7cf70724bdb0 435 *pOutVal = 0;
adaphoto 1:7cf70724bdb0 436 for(i = 0; i < 8; i ++)
adaphoto 1:7cf70724bdb0 437 {
adaphoto 1:7cf70724bdb0 438 *pOutVal += *(pAvgBuffer + i);
adaphoto 1:7cf70724bdb0 439 }
adaphoto 1:7cf70724bdb0 440 *pOutVal >>= 3;
adaphoto 1:7cf70724bdb0 441 }
adaphoto 1:7cf70724bdb0 442
adaphoto 1:7cf70724bdb0 443 /**
adaphoto 1:7cf70724bdb0 444 * @brief Start temperature measurement
adaphoto 1:7cf70724bdb0 445 * @param None
adaphoto 1:7cf70724bdb0 446 * @retval None
adaphoto 1:7cf70724bdb0 447 */
adaphoto 1:7cf70724bdb0 448 void IMU::BMP180_StartTemperatureMeasurement(void)
adaphoto 1:7cf70724bdb0 449 {
adaphoto 1:7cf70724bdb0 450 //BMP180_WriteReg(CONTROL, READ_TEMPERATURE);
adaphoto 1:7cf70724bdb0 451 I2C_WriteOneByte(BMP180_ADDR, CONTROL, READ_TEMPERATURE);
adaphoto 1:7cf70724bdb0 452 }
adaphoto 1:7cf70724bdb0 453
adaphoto 1:7cf70724bdb0 454 /**
adaphoto 1:7cf70724bdb0 455 * @brief Start pressure measurement
adaphoto 1:7cf70724bdb0 456 * @param None
adaphoto 1:7cf70724bdb0 457 * @retval None
adaphoto 1:7cf70724bdb0 458 */
adaphoto 1:7cf70724bdb0 459 void IMU::BMP180_StartPressureMeasurement(void)
adaphoto 1:7cf70724bdb0 460 {
adaphoto 1:7cf70724bdb0 461 //BMP180_WriteReg(CONTROL, READ_PRESSURE + (_oss << 6));
adaphoto 1:7cf70724bdb0 462 I2C_WriteOneByte(BMP180_ADDR, CONTROL, READ_PRESSURE + (_oss << 6));
adaphoto 1:7cf70724bdb0 463 }
adaphoto 1:7cf70724bdb0 464
adaphoto 1:7cf70724bdb0 465 /**
adaphoto 1:7cf70724bdb0 466 * @brief Read uncompensated temperature
adaphoto 1:7cf70724bdb0 467 * @param None
adaphoto 1:7cf70724bdb0 468 * @retval None
adaphoto 1:7cf70724bdb0 469 */
adaphoto 1:7cf70724bdb0 470 void IMU::BMP180_ReadUncompensatedTemperature(void)
adaphoto 1:7cf70724bdb0 471 {
adaphoto 1:7cf70724bdb0 472 char RegBuff[2];
adaphoto 1:7cf70724bdb0 473 BMP180_ReadReg(CONTROL_OUTPUT, 2, &RegBuff[0]);
adaphoto 1:7cf70724bdb0 474
adaphoto 1:7cf70724bdb0 475 UT = ((int32_t)RegBuff[0] << 8) + (int32_t)RegBuff[1];
adaphoto 1:7cf70724bdb0 476 }
adaphoto 1:7cf70724bdb0 477
adaphoto 1:7cf70724bdb0 478 /**
adaphoto 1:7cf70724bdb0 479 * @brief Read uncompensated pressure
adaphoto 1:7cf70724bdb0 480 * @param None
adaphoto 1:7cf70724bdb0 481 * @retval None
adaphoto 1:7cf70724bdb0 482 */
adaphoto 1:7cf70724bdb0 483 void IMU::BMP180_ReadUncompensatedPressure(void)
adaphoto 1:7cf70724bdb0 484 {
adaphoto 1:7cf70724bdb0 485 char RegBuff[3];
adaphoto 1:7cf70724bdb0 486
adaphoto 1:7cf70724bdb0 487 BMP180_ReadReg(CONTROL_OUTPUT, 3, &RegBuff[0]);
adaphoto 1:7cf70724bdb0 488
adaphoto 1:7cf70724bdb0 489 UP = (((int32_t)RegBuff[0] << 16) + ((int32_t)RegBuff[1] << 8) + ((int32_t)RegBuff[2])) >> (8 -_oss); // uncompensated pressure value
adaphoto 1:7cf70724bdb0 490 }
adaphoto 1:7cf70724bdb0 491
adaphoto 1:7cf70724bdb0 492 /**
adaphoto 1:7cf70724bdb0 493 * @brief Calculate true temperature
adaphoto 1:7cf70724bdb0 494 * @param *pTrueTemperature: true temperature
adaphoto 1:7cf70724bdb0 495 * @retval None
adaphoto 1:7cf70724bdb0 496 */
adaphoto 1:7cf70724bdb0 497 void IMU::BMP180_CalculateTrueTemperature(int32_t *pTrueTemperature)
adaphoto 1:7cf70724bdb0 498 {
adaphoto 1:7cf70724bdb0 499 int32_t X1, X2;
adaphoto 1:7cf70724bdb0 500
adaphoto 1:7cf70724bdb0 501 X1 = ((UT - AC6) * AC5) >> 15;
adaphoto 1:7cf70724bdb0 502 X2 = (MC << 11) / (X1 + MD);
adaphoto 1:7cf70724bdb0 503 B5 = X1 + X2;
adaphoto 1:7cf70724bdb0 504 *pTrueTemperature = (B5 + 8) >> 4;
adaphoto 1:7cf70724bdb0 505 }
adaphoto 1:7cf70724bdb0 506
adaphoto 1:7cf70724bdb0 507 /**
adaphoto 1:7cf70724bdb0 508 * @brief Calculate true pressure
adaphoto 1:7cf70724bdb0 509 * @param *pTruePressure: true pressure
adaphoto 1:7cf70724bdb0 510 * @retval None
adaphoto 1:7cf70724bdb0 511 */
adaphoto 1:7cf70724bdb0 512 void IMU::BMP180_CalculateTruePressure(int32_t *pTruePressure)
adaphoto 1:7cf70724bdb0 513 {
adaphoto 1:7cf70724bdb0 514 int32_t X1, X2, X3, B3, B6, P, Temp;
adaphoto 1:7cf70724bdb0 515 uint32_t B4, B7;
adaphoto 1:7cf70724bdb0 516
adaphoto 1:7cf70724bdb0 517 B6 = B5 - 4000;
adaphoto 1:7cf70724bdb0 518 X1 = (B2* ((B6 * B6) >> 12)) >> 11;
adaphoto 1:7cf70724bdb0 519 X2 = AC2 * B6 >> 11;
adaphoto 1:7cf70724bdb0 520 X3 = X1 + X2;
adaphoto 1:7cf70724bdb0 521 Temp = (((int32_t)AC1 << 2) + X3) << _oss;
adaphoto 1:7cf70724bdb0 522 B3 = (Temp + 2) >> 2;
adaphoto 1:7cf70724bdb0 523 X1 = (AC3 * B6) >> 13;
adaphoto 1:7cf70724bdb0 524 X2 = (B1 * (B6 * B6 >> 12)) >> 16;
adaphoto 1:7cf70724bdb0 525 X3 = ((X1 + X2) + 2) >> 2;
adaphoto 1:7cf70724bdb0 526 B4 = (AC4 * (uint32_t) (X3 + 32768)) >> 15;
adaphoto 1:7cf70724bdb0 527 B7 = ((uint32_t)UP - B3) * (50000 >> _oss);
adaphoto 1:7cf70724bdb0 528 if(B7 < 0x80000000)
adaphoto 1:7cf70724bdb0 529 {
adaphoto 1:7cf70724bdb0 530 P = (B7 << 1) / B4;
adaphoto 1:7cf70724bdb0 531 }
adaphoto 1:7cf70724bdb0 532 else
adaphoto 1:7cf70724bdb0 533 {
adaphoto 1:7cf70724bdb0 534 P = (B7 / B4) << 1;
adaphoto 1:7cf70724bdb0 535 }
adaphoto 1:7cf70724bdb0 536
adaphoto 1:7cf70724bdb0 537 X1 = (P >> 8) * (P >> 8);
adaphoto 1:7cf70724bdb0 538 X1 = (X1 * 3038) >> 16;
adaphoto 1:7cf70724bdb0 539 X2 = (-7357 * P) >> 16;
adaphoto 1:7cf70724bdb0 540
adaphoto 1:7cf70724bdb0 541 *pTruePressure = P + ((X1 + X2 + 3791) >> 4);
adaphoto 1:7cf70724bdb0 542 }
adaphoto 1:7cf70724bdb0 543
adaphoto 1:7cf70724bdb0 544 /**
adaphoto 1:7cf70724bdb0 545 * @brief Calculating average value of pressure
adaphoto 1:7cf70724bdb0 546 * @param *pVal: the average value of pressure
adaphoto 1:7cf70724bdb0 547 * @retval None
adaphoto 1:7cf70724bdb0 548 */
adaphoto 1:7cf70724bdb0 549 void IMU::BMP180_LocalpressureAvg(int32_t *pVal)
adaphoto 1:7cf70724bdb0 550 {
adaphoto 1:7cf70724bdb0 551 uint8_t i;
adaphoto 1:7cf70724bdb0 552 int32_t Sum = 0;
adaphoto 1:7cf70724bdb0 553
adaphoto 1:7cf70724bdb0 554 for(i = 0; i < 5; i ++)
adaphoto 1:7cf70724bdb0 555 {
adaphoto 1:7cf70724bdb0 556 BMP180_StartTemperatureMeasurement();
adaphoto 1:7cf70724bdb0 557 Thread::wait(5);//delay_ms(5); //4.5ms 324
adaphoto 1:7cf70724bdb0 558 BMP180_ReadUncompensatedTemperature();
adaphoto 1:7cf70724bdb0 559 BMP180_StartPressureMeasurement();
adaphoto 1:7cf70724bdb0 560 Thread::wait(5);//delay_ms(8);//7.5ms 540
adaphoto 1:7cf70724bdb0 561 BMP180_ReadUncompensatedPressure();
adaphoto 1:7cf70724bdb0 562 BMP180_CalculateTruePressure(&PressureVal);
adaphoto 1:7cf70724bdb0 563 BMP180_CalculateTrueTemperature(&TemperatureVal);
adaphoto 1:7cf70724bdb0 564
adaphoto 1:7cf70724bdb0 565 if(i >= 2)
adaphoto 1:7cf70724bdb0 566 {
adaphoto 1:7cf70724bdb0 567 Sum += PressureVal;
adaphoto 1:7cf70724bdb0 568 }
adaphoto 1:7cf70724bdb0 569 }
adaphoto 1:7cf70724bdb0 570 *pVal = Sum / 3;
adaphoto 1:7cf70724bdb0 571 }
adaphoto 1:7cf70724bdb0 572
adaphoto 1:7cf70724bdb0 573 /**
adaphoto 1:7cf70724bdb0 574 * @brief Calculating pressure at sea level
adaphoto 1:7cf70724bdb0 575 * @param None
adaphoto 1:7cf70724bdb0 576 * @retval None
adaphoto 1:7cf70724bdb0 577 */
adaphoto 1:7cf70724bdb0 578 void IMU::BMP180_PressureAtSeaLevel(void)
adaphoto 1:7cf70724bdb0 579 {
adaphoto 1:7cf70724bdb0 580 float Temp = 0.0f;
adaphoto 1:7cf70724bdb0 581
adaphoto 1:7cf70724bdb0 582 BMP180_LocalpressureAvg(&PressureVal);
adaphoto 1:7cf70724bdb0 583
adaphoto 1:7cf70724bdb0 584 Temp = (float)LOCAL_ADS_ALTITUDE / 4433000;
adaphoto 1:7cf70724bdb0 585 Temp = (float)pow((1 - Temp), 5.255f);
adaphoto 1:7cf70724bdb0 586 Pressure0 = (PressureVal - PRESSURE_OFFSET) / Temp;//
adaphoto 1:7cf70724bdb0 587 }
adaphoto 1:7cf70724bdb0 588
adaphoto 1:7cf70724bdb0 589 /**
adaphoto 1:7cf70724bdb0 590 * @brief Calculating absolute altitude
adaphoto 1:7cf70724bdb0 591 * @param *pAltitude: absolute altitude
adaphoto 1:7cf70724bdb0 592 * @param PressureVal: the pressure at the absolute altitude
adaphoto 1:7cf70724bdb0 593 * @retval None
adaphoto 1:7cf70724bdb0 594 */
adaphoto 1:7cf70724bdb0 595 void IMU::BMP180_CalculateAbsoluteAltitude(int32_t *pAltitude, int32_t PressureVal)
adaphoto 1:7cf70724bdb0 596 {
adaphoto 1:7cf70724bdb0 597 *pAltitude = 4433000 * (1 - pow((float)(PressureVal / (float)Pressure0), 0.1903f));
adaphoto 1:7cf70724bdb0 598 }
adaphoto 1:7cf70724bdb0 599
adaphoto 1:7cf70724bdb0 600 /**
adaphoto 1:7cf70724bdb0 601 * @brief Read calibration data from the EEPROM of the BMP180
adaphoto 1:7cf70724bdb0 602 * @param None
adaphoto 1:7cf70724bdb0 603 * @retval None
adaphoto 1:7cf70724bdb0 604 */
adaphoto 1:7cf70724bdb0 605 void IMU::BMP180_ReadCalibrationData(void)
adaphoto 1:7cf70724bdb0 606 {
adaphoto 1:7cf70724bdb0 607 char RegBuff[2];
adaphoto 1:7cf70724bdb0 608
adaphoto 1:7cf70724bdb0 609 BMP180_ReadReg(CAL_AC1, 2, RegBuff);
adaphoto 1:7cf70724bdb0 610 AC1 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 611
adaphoto 1:7cf70724bdb0 612 BMP180_ReadReg(CAL_AC2, 2, RegBuff);
adaphoto 1:7cf70724bdb0 613 AC2 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 614
adaphoto 1:7cf70724bdb0 615 BMP180_ReadReg(CAL_AC3, 2, RegBuff);
adaphoto 1:7cf70724bdb0 616 AC3 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 617
adaphoto 1:7cf70724bdb0 618 BMP180_ReadReg(CAL_AC4, 2, RegBuff);
adaphoto 1:7cf70724bdb0 619 AC4 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 620
adaphoto 1:7cf70724bdb0 621 BMP180_ReadReg(CAL_AC5, 2, RegBuff);
adaphoto 1:7cf70724bdb0 622 AC5 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 623
adaphoto 1:7cf70724bdb0 624 BMP180_ReadReg(CAL_AC6, 2, RegBuff);
adaphoto 1:7cf70724bdb0 625 AC6 = ((uint16_t)RegBuff[0] <<8 | ((uint16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 626
adaphoto 1:7cf70724bdb0 627 BMP180_ReadReg(CAL_B1, 2, RegBuff);
adaphoto 1:7cf70724bdb0 628 B1 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 629
adaphoto 1:7cf70724bdb0 630 BMP180_ReadReg(CAL_B2, 2, RegBuff);
adaphoto 1:7cf70724bdb0 631 B2 = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 632
adaphoto 1:7cf70724bdb0 633 BMP180_ReadReg(CAL_MB, 2, RegBuff);
adaphoto 1:7cf70724bdb0 634 MB = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 635
adaphoto 1:7cf70724bdb0 636 BMP180_ReadReg(CAL_MC, 2, RegBuff);
adaphoto 1:7cf70724bdb0 637 MC = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 638
adaphoto 1:7cf70724bdb0 639 BMP180_ReadReg(CAL_MD, 2, RegBuff);
adaphoto 1:7cf70724bdb0 640 MD = ((int16_t)RegBuff[0] <<8 | ((int16_t)RegBuff[1]));
adaphoto 1:7cf70724bdb0 641 }
adaphoto 1:7cf70724bdb0 642
adaphoto 1:7cf70724bdb0 643 /**
adaphoto 1:7cf70724bdb0 644 * @brief Configures hardware pressure sampling accuracy modes
adaphoto 1:7cf70724bdb0 645 * @param None
adaphoto 1:7cf70724bdb0 646 * @retval None
adaphoto 1:7cf70724bdb0 647 */
adaphoto 1:7cf70724bdb0 648 void IMU::BMP180_SetOversample(void)
adaphoto 1:7cf70724bdb0 649 {
adaphoto 1:7cf70724bdb0 650 _oss = MODE_ULTRA_HIGHRES;
adaphoto 1:7cf70724bdb0 651 }
adaphoto 1:7cf70724bdb0 652
adaphoto 1:7cf70724bdb0 653 /**
adaphoto 1:7cf70724bdb0 654 * @brief initializes BMP180
adaphoto 1:7cf70724bdb0 655 * @param None
adaphoto 1:7cf70724bdb0 656 * @retval None
adaphoto 1:7cf70724bdb0 657 */
adaphoto 1:7cf70724bdb0 658 void IMU::BMP180_Init(void)
adaphoto 1:7cf70724bdb0 659 {
adaphoto 1:7cf70724bdb0 660 BMP180_SetOversample();
adaphoto 1:7cf70724bdb0 661 BMP180_ReadCalibrationData();
adaphoto 1:7cf70724bdb0 662 //BMP180_PressureAtSeaLevel();
adaphoto 1:7cf70724bdb0 663 }
adaphoto 1:7cf70724bdb0 664
adaphoto 1:7cf70724bdb0 665 /**
adaphoto 1:7cf70724bdb0 666 * @brief Calculation of pressure and temperature and altitude for BMP180
adaphoto 1:7cf70724bdb0 667 * @param None
adaphoto 1:7cf70724bdb0 668 * @retval None
adaphoto 1:7cf70724bdb0 669 */
adaphoto 1:7cf70724bdb0 670 void IMU::CalTemperatureAndPressureAndAltitude(void)
adaphoto 1:7cf70724bdb0 671 {
adaphoto 1:7cf70724bdb0 672 static uint8_t State = START_TEMPERATURE_MEASUREMENT;
adaphoto 1:7cf70724bdb0 673 static BMP180_AvgTypeDef BMP180_Filter[3];
adaphoto 1:7cf70724bdb0 674 int32_t PVal,AVal, TVal;
adaphoto 1:7cf70724bdb0 675
adaphoto 1:7cf70724bdb0 676 switch(State)
adaphoto 1:7cf70724bdb0 677 {
adaphoto 1:7cf70724bdb0 678 case START_TEMPERATURE_MEASUREMENT:
adaphoto 1:7cf70724bdb0 679 BMP180_StartTemperatureMeasurement();
adaphoto 1:7cf70724bdb0 680 Thread::wait(5);//delay_ms(5); //4.5ms
adaphoto 1:7cf70724bdb0 681 State = READ_UT_AND_START_PRESSURE_MEASUREMENT;
adaphoto 1:7cf70724bdb0 682 break;
adaphoto 1:7cf70724bdb0 683
adaphoto 1:7cf70724bdb0 684 case READ_UT_AND_START_PRESSURE_MEASUREMENT:
adaphoto 1:7cf70724bdb0 685 BMP180_ReadUncompensatedTemperature();
adaphoto 1:7cf70724bdb0 686 BMP180_StartPressureMeasurement();
adaphoto 1:7cf70724bdb0 687 Thread::wait(10);//delay_ms(10);//7.5ms
adaphoto 1:7cf70724bdb0 688 State = READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE;
adaphoto 1:7cf70724bdb0 689 break;
adaphoto 1:7cf70724bdb0 690
adaphoto 1:7cf70724bdb0 691 case READ_UP_CAL_TRUE_PRESSURE_TEMPERATURE:
adaphoto 1:7cf70724bdb0 692 BMP180_ReadUncompensatedPressure();
adaphoto 1:7cf70724bdb0 693 BMP180_CalculateTruePressure(&PVal);
adaphoto 1:7cf70724bdb0 694 BMP180_CalAvgValue(&BMP180_Filter[0].Index, BMP180_Filter[0].AvgBuffer, PVal - PRESSURE_OFFSET, &PressureVal);
adaphoto 1:7cf70724bdb0 695
adaphoto 1:7cf70724bdb0 696 BMP180_CalculateAbsoluteAltitude(&AVal, PVal - PRESSURE_OFFSET);
adaphoto 1:7cf70724bdb0 697 BMP180_CalAvgValue(&BMP180_Filter[1].Index, BMP180_Filter[1].AvgBuffer, AVal, &AltitudeVal);
adaphoto 1:7cf70724bdb0 698
adaphoto 1:7cf70724bdb0 699 BMP180_CalculateTrueTemperature(&TVal);
adaphoto 1:7cf70724bdb0 700 BMP180_CalAvgValue(&BMP180_Filter[2].Index, BMP180_Filter[2].AvgBuffer, TVal, &TemperatureVal);
adaphoto 1:7cf70724bdb0 701
adaphoto 1:7cf70724bdb0 702 State = START_TEMPERATURE_MEASUREMENT;
adaphoto 1:7cf70724bdb0 703 break;
adaphoto 1:7cf70724bdb0 704
adaphoto 1:7cf70724bdb0 705 default:
adaphoto 1:7cf70724bdb0 706 break;
adaphoto 1:7cf70724bdb0 707 }
adaphoto 0:35bba382318b 708 }