chen ada / IMU

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

imu.cpp@0:35bba382318b, 2018-06-21 (annotated)

Committer:
adaphoto
Date:
Thu Jun 21 07:00:40 2018 +0000
Revision:
0:35bba382318b
Child:
1:7cf70724bdb0
??MPU9250?IMU??????pith?roll?yaw?compass

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 0:35bba382318b 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 0:35bba382318b 204 /**
adaphoto 0:35bba382318b 205 * @brief Initializes MPU9250
adaphoto 0:35bba382318b 206 * @param None
adaphoto 0:35bba382318b 207 * @retval None
adaphoto 0:35bba382318b 208 */
adaphoto 0:35bba382318b 209 void IMU::MPU9250_Init()
adaphoto 0:35bba382318b 210 {
adaphoto 0:35bba382318b 211 I2C_WriteOneByte(GYRO_ADDRESS,PWR_MGMT_1, 0x00);
adaphoto 0:35bba382318b 212 I2C_WriteOneByte(GYRO_ADDRESS,SMPLRT_DIV, 0x07);
adaphoto 0:35bba382318b 213 I2C_WriteOneByte(GYRO_ADDRESS,CONFIG, 0x06);
adaphoto 0:35bba382318b 214 I2C_WriteOneByte(GYRO_ADDRESS,GYRO_CONFIG, 0x10);
adaphoto 0:35bba382318b 215 I2C_WriteOneByte(GYRO_ADDRESS,ACCEL_CONFIG, 0x01);
adaphoto 0:35bba382318b 216
adaphoto 0:35bba382318b 217 Thread::wait(10);
adaphoto 0:35bba382318b 218
adaphoto 0:35bba382318b 219 if(MPU9250_Check())
adaphoto 0:35bba382318b 220 {
adaphoto 0:35bba382318b 221 printf("\r\n MPU9255 Ready!\n");
adaphoto 0:35bba382318b 222 }
adaphoto 0:35bba382318b 223 else
adaphoto 0:35bba382318b 224 {
adaphoto 0:35bba382318b 225 printf("\r\n MPU9255 Erro!\n");
adaphoto 0:35bba382318b 226 }
adaphoto 0:35bba382318b 227
adaphoto 0:35bba382318b 228 MPU9250_InitGyrOffset();
adaphoto 0:35bba382318b 229 }
adaphoto 0:35bba382318b 230
adaphoto 0:35bba382318b 231 /**
adaphoto 0:35bba382318b 232 * @brief Digital filter
adaphoto 0:35bba382318b 233 * @param *pIndex:
adaphoto 0:35bba382318b 234 * @param *pAvgBuffer:
adaphoto 0:35bba382318b 235 * @param InVal:
adaphoto 0:35bba382318b 236 * @param pOutVal:
adaphoto 0:35bba382318b 237 *
adaphoto 0:35bba382318b 238 * @retval None
adaphoto 0:35bba382318b 239 *
adaphoto 0:35bba382318b 240 */
adaphoto 0:35bba382318b 241 void IMU::MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal)
adaphoto 0:35bba382318b 242 {
adaphoto 0:35bba382318b 243 uint8_t i;
adaphoto 0:35bba382318b 244
adaphoto 0:35bba382318b 245 *(pAvgBuffer + ((*pIndex) ++)) = InVal;
adaphoto 0:35bba382318b 246 *pIndex &= 0x07;
adaphoto 0:35bba382318b 247
adaphoto 0:35bba382318b 248 *pOutVal = 0;
adaphoto 0:35bba382318b 249 for(i = 0; i < 8; i ++)
adaphoto 0:35bba382318b 250 {
adaphoto 0:35bba382318b 251 *pOutVal += *(pAvgBuffer + i);
adaphoto 0:35bba382318b 252 }
adaphoto 0:35bba382318b 253 *pOutVal >>= 3;
adaphoto 0:35bba382318b 254 }
adaphoto 0:35bba382318b 255
adaphoto 0:35bba382318b 256 /**
adaphoto 0:35bba382318b 257 * @brief Get accelerometer datas
adaphoto 0:35bba382318b 258 * @param None
adaphoto 0:35bba382318b 259 * @retval None
adaphoto 0:35bba382318b 260 */
adaphoto 0:35bba382318b 261 void IMU::MPU9250_READ_ACCEL(void)
adaphoto 0:35bba382318b 262 {
adaphoto 0:35bba382318b 263 uint8_t i;
adaphoto 0:35bba382318b 264 int16_t InBuffer[3] = {0};
adaphoto 0:35bba382318b 265 static int32_t OutBuffer[3] = {0};
adaphoto 0:35bba382318b 266 static MPU9250_AvgTypeDef MPU9250_Filter[3];
adaphoto 0:35bba382318b 267
adaphoto 0:35bba382318b 268 BUF[0]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_L);
adaphoto 0:35bba382318b 269 BUF[1]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_H);
adaphoto 0:35bba382318b 270 InBuffer[0]= (BUF[1]<<8)|BUF[0];
adaphoto 0:35bba382318b 271
adaphoto 0:35bba382318b 272 BUF[2]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_L);
adaphoto 0:35bba382318b 273 BUF[3]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_H);
adaphoto 0:35bba382318b 274 InBuffer[1]= (BUF[3]<<8)|BUF[2];
adaphoto 0:35bba382318b 275
adaphoto 0:35bba382318b 276 BUF[4]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_L);
adaphoto 0:35bba382318b 277 BUF[5]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_H);
adaphoto 0:35bba382318b 278 InBuffer[2]= (BUF[5]<<8)|BUF[4];
adaphoto 0:35bba382318b 279
adaphoto 0:35bba382318b 280 for(i = 0; i < 3; i ++)
adaphoto 0:35bba382318b 281 {
adaphoto 0:35bba382318b 282 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
adaphoto 0:35bba382318b 283 }
adaphoto 0:35bba382318b 284 accel[0] = *(OutBuffer + 0);
adaphoto 0:35bba382318b 285 accel[1] = *(OutBuffer + 1);
adaphoto 0:35bba382318b 286 accel[2] = *(OutBuffer + 2);
adaphoto 0:35bba382318b 287 }
adaphoto 0:35bba382318b 288
adaphoto 0:35bba382318b 289 /**
adaphoto 0:35bba382318b 290 * @brief Get gyroscopes datas
adaphoto 0:35bba382318b 291 * @param None
adaphoto 0:35bba382318b 292 * @retval None
adaphoto 0:35bba382318b 293 */
adaphoto 0:35bba382318b 294 void IMU::MPU9250_READ_GYRO(void)
adaphoto 0:35bba382318b 295 {
adaphoto 0:35bba382318b 296 uint8_t i;
adaphoto 0:35bba382318b 297 int16_t InBuffer[3] = {0};
adaphoto 0:35bba382318b 298 static int32_t OutBuffer[3] = {0};
adaphoto 0:35bba382318b 299 static MPU9250_AvgTypeDef MPU9250_Filter[3];
adaphoto 0:35bba382318b 300
adaphoto 0:35bba382318b 301 BUF[0]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_L);
adaphoto 0:35bba382318b 302 BUF[1]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_H);
adaphoto 0:35bba382318b 303 InBuffer[0]= (BUF[1]<<8)|BUF[0];
adaphoto 0:35bba382318b 304
adaphoto 0:35bba382318b 305 BUF[2]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_L);
adaphoto 0:35bba382318b 306 BUF[3]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_H);
adaphoto 0:35bba382318b 307 InBuffer[1] = (BUF[3]<<8)|BUF[2];
adaphoto 0:35bba382318b 308
adaphoto 0:35bba382318b 309 BUF[4]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_L);
adaphoto 0:35bba382318b 310 BUF[5]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_H);
adaphoto 0:35bba382318b 311 InBuffer[2] = (BUF[5]<<8)|BUF[4];
adaphoto 0:35bba382318b 312
adaphoto 0:35bba382318b 313 for(i = 0; i < 3; i ++)
adaphoto 0:35bba382318b 314 {
adaphoto 0:35bba382318b 315 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
adaphoto 0:35bba382318b 316 }
adaphoto 0:35bba382318b 317 gyro[0] = *(OutBuffer + 0) - MPU9250_Offset.X;
adaphoto 0:35bba382318b 318 gyro[1] = *(OutBuffer + 1) - MPU9250_Offset.Y;
adaphoto 0:35bba382318b 319 gyro[2] = *(OutBuffer + 2) - MPU9250_Offset.Z;
adaphoto 0:35bba382318b 320 }
adaphoto 0:35bba382318b 321
adaphoto 0:35bba382318b 322 /**
adaphoto 0:35bba382318b 323 * @brief Get compass datas
adaphoto 0:35bba382318b 324 * @param None
adaphoto 0:35bba382318b 325 * @retval None
adaphoto 0:35bba382318b 326 */
adaphoto 0:35bba382318b 327 void IMU::MPU9250_READ_MAG(void)
adaphoto 0:35bba382318b 328 {
adaphoto 0:35bba382318b 329 uint8_t i;
adaphoto 0:35bba382318b 330 int16_t InBuffer[3] = {0};
adaphoto 0:35bba382318b 331 static int32_t OutBuffer[3] = {0};
adaphoto 0:35bba382318b 332 static MPU9250_AvgTypeDef MPU9250_Filter[3];
adaphoto 0:35bba382318b 333
adaphoto 0:35bba382318b 334 I2C_WriteOneByte(GYRO_ADDRESS,0x37,0x02);//turn on Bypass Mode
adaphoto 0:35bba382318b 335 Thread::wait(10);
adaphoto 0:35bba382318b 336 I2C_WriteOneByte(MAG_ADDRESS,0x0A,0x01);
adaphoto 0:35bba382318b 337 Thread::wait(10);
adaphoto 0:35bba382318b 338 BUF[0]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_L);
adaphoto 0:35bba382318b 339 BUF[1]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_H);
adaphoto 0:35bba382318b 340 InBuffer[1] =(BUF[1]<<8)|BUF[0];
adaphoto 0:35bba382318b 341
adaphoto 0:35bba382318b 342 BUF[2]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_L);
adaphoto 0:35bba382318b 343 BUF[3]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_H);
adaphoto 0:35bba382318b 344 InBuffer[0] = (BUF[3]<<8)|BUF[2];
adaphoto 0:35bba382318b 345
adaphoto 0:35bba382318b 346 BUF[4]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_L);
adaphoto 0:35bba382318b 347 BUF[5]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_H);
adaphoto 0:35bba382318b 348 InBuffer[2] = (BUF[5]<<8)|BUF[4];
adaphoto 0:35bba382318b 349 InBuffer[2] = -InBuffer[2];
adaphoto 0:35bba382318b 350
adaphoto 0:35bba382318b 351 for(i = 0; i < 3; i ++)
adaphoto 0:35bba382318b 352 {
adaphoto 0:35bba382318b 353 MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
adaphoto 0:35bba382318b 354 }
adaphoto 0:35bba382318b 355 magn[0] = *(OutBuffer + 0)-MPU9250_Magn_Offset.X_Off_Err;
adaphoto 0:35bba382318b 356 magn[1] = *(OutBuffer + 1)-MPU9250_Magn_Offset.Y_Off_Err;
adaphoto 0:35bba382318b 357 magn[2] = *(OutBuffer + 2)-MPU9250_Magn_Offset.Z_Off_Err;
adaphoto 0:35bba382318b 358 }
adaphoto 0:35bba382318b 359
adaphoto 0:35bba382318b 360 /**
adaphoto 0:35bba382318b 361 * @brief Check MPU9250,ensure communication succeed
adaphoto 0:35bba382318b 362 * @param None
adaphoto 0:35bba382318b 363 * @retval true: communicate succeed
adaphoto 0:35bba382318b 364 * false: communicate fualt
adaphoto 0:35bba382318b 365 */
adaphoto 0:35bba382318b 366 bool IMU::MPU9250_Check(void)
adaphoto 0:35bba382318b 367 {
adaphoto 0:35bba382318b 368 if(WHO_AM_I_VAL == I2C_ReadOneByte(DEFAULT_ADDRESS, WHO_AM_I))
adaphoto 0:35bba382318b 369 {
adaphoto 0:35bba382318b 370 return true;
adaphoto 0:35bba382318b 371 }
adaphoto 0:35bba382318b 372 else
adaphoto 0:35bba382318b 373 {
adaphoto 0:35bba382318b 374 return false;
adaphoto 0:35bba382318b 375 }
adaphoto 0:35bba382318b 376 }
adaphoto 0:35bba382318b 377
adaphoto 0:35bba382318b 378 /**
adaphoto 0:35bba382318b 379 * @brief Initializes gyroscopes offset
adaphoto 0:35bba382318b 380 * @param None
adaphoto 0:35bba382318b 381 * @retval None
adaphoto 0:35bba382318b 382 */
adaphoto 0:35bba382318b 383 void IMU::MPU9250_InitGyrOffset(void)
adaphoto 0:35bba382318b 384 {
adaphoto 0:35bba382318b 385 uint8_t i;
adaphoto 0:35bba382318b 386 int32_t TempGx = 0, TempGy = 0, TempGz = 0;
adaphoto 0:35bba382318b 387
adaphoto 0:35bba382318b 388 for(i = 0; i < 32; i ++)
adaphoto 0:35bba382318b 389 {
adaphoto 0:35bba382318b 390 MPU9250_READ_GYRO();
adaphoto 0:35bba382318b 391
adaphoto 0:35bba382318b 392 TempGx += gyro[0];
adaphoto 0:35bba382318b 393 TempGy += gyro[1];
adaphoto 0:35bba382318b 394 TempGz += gyro[2];
adaphoto 0:35bba382318b 395
adaphoto 0:35bba382318b 396 Thread::wait(1);
adaphoto 0:35bba382318b 397 }
adaphoto 0:35bba382318b 398
adaphoto 0:35bba382318b 399 MPU9250_Offset.X = TempGx >> 5;
adaphoto 0:35bba382318b 400 MPU9250_Offset.Y = TempGy >> 5;
adaphoto 0:35bba382318b 401 MPU9250_Offset.Z = TempGz >> 5;
adaphoto 0:35bba382318b 402
adaphoto 0:35bba382318b 403 }