chen ada
基于MPU9250的IMU库，可以测量pitch，roll，yaw，compass
Diff: imu.cpp
- Revision:
- 0:35bba382318b
- Child:
- 1:7cf70724bdb0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/imu.cpp Thu Jun 21 07:00:40 2018 +0000
@@ -0,0 +1,403 @@
+#include "imu.h"
+
+I2C i2c(I2C_SDA,I2C_SCL);
+
+/**
+ * @brief invSqrt
+ * @param
+ * @retval
+ */
+float IMU::invSqrt(float x)
+{
+ float halfx = 0.5f * x;
+ float y = x;
+
+ long i = *(long*)&y; //get bits for floating value
+ i = 0x5f3759df - (i >> 1); //gives initial guss you
+ y = *(float*)&i; //convert bits back to float
+ y = y * (1.5f - (halfx * y * y)); //newtop step, repeating increases accuracy
+
+ return y;
+}
+
+/**
+ * @brief initializes IMU
+ * @param None
+ * @retval None
+ */
+void IMU::IMU_Init()
+{
+ MPU9250_Init();
+ //BMP180_Init();
+
+ q0 = 1.0f;
+ q1 = 0.0f;
+ q2 = 0.0f;
+ q3 = 0.0f;
+}
+
+/**
+ * @brief Updata attitude and heading
+ * @param ax: accelerometer X
+ * @param ay: accelerometer Y
+ * @param az: accelerometer Z
+ * @param gx: gyroscopes X
+ * @param gy: gyroscopes Y
+ * @param gz: gyroscopes Z
+ * @param mx: magnetometer X
+ * @param my: magnetometer Y
+ * @param mz: magnetometer Z
+ * @retval None
+ */
+void IMU::IMU_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
+{
+ float norm;
+ float hx, hy, hz, bx, bz;
+ float vx, vy, vz, wx, wy, wz;
+ float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;
+ float ex, ey, ez, halfT = 0.024f;
+
+ float q0q0 = q0 * q0;
+ float q0q1 = q0 * q1;
+ float q0q2 = q0 * q2;
+ float q0q3 = q0 * q3;
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q3q3 = q3 * q3;
+
+ norm = invSqrt(ax * ax + ay * ay + az * az);
+ ax = ax * norm;
+ ay = ay * norm;
+ az = az * norm;
+
+ norm = invSqrt(mx * mx + my * my + mz * mz);
+ mx = mx * norm;
+ my = my * norm;
+ mz = mz * norm;
+
+ // compute reference direction of flux
+ hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
+ hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
+ hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
+ bx = sqrt((hx * hx) + (hy * hy));
+ bz = hz;
+
+ // estimated direction of gravity and flux (v and w)
+ vx = 2 * (q1q3 - q0q2);
+ vy = 2 * (q0q1 + q2q3);
+ vz = q0q0 - q1q1 - q2q2 + q3q3;
+ wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
+ wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
+ wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);
+
+ // error is sum of cross product between reference direction of fields and direction measured by sensors
+ ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+ ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+ ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+
+ if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
+ {
+ exInt = exInt + ex * Ki * halfT;
+ eyInt = eyInt + ey * Ki * halfT;
+ ezInt = ezInt + ez * Ki * halfT;
+
+ gx = gx + Kp * ex + exInt;
+ gy = gy + Kp * ey + eyInt;
+ gz = gz + Kp * ez + ezInt;
+ }
+
+ q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
+ q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
+ q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
+ q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
+
+ norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 = q0 * norm;
+ q1 = q1 * norm;
+ q2 = q2 * norm;
+ q3 = q3 * norm;
+}
+
+/**
+ * @brief Get quaters
+ * @param None
+ * @retval None
+ */
+void IMU::IMU_GetQuater(void)
+{
+ float MotionVal[9];
+
+ MPU9250_READ_ACCEL();
+ MPU9250_READ_GYRO();
+ MPU9250_READ_MAG();
+
+ MotionVal[0]=gyro[0]/32.8;
+ MotionVal[1]=gyro[1]/32.8;
+ MotionVal[2]=gyro[2]/32.8;
+ MotionVal[3]=accel[0];
+ MotionVal[4]=accel[1];
+ MotionVal[5]=accel[2];
+ MotionVal[6]=magn[0];
+ MotionVal[7]=magn[1];
+ MotionVal[8]=magn[2];
+
+ IMU_AHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175,
+ (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5], (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);
+}
+
+/**
+ * @brief Get Yaw Pitch Roll
+ * @param None
+ * @retval None
+ */
+void IMU::IMU_GetYawPitchRoll(float *Angles)
+{
+
+ int x_tmp,y_tmp;
+
+ IMU_GetQuater();
+
+ x_tmp = magn[0];
+ y_tmp = magn[1];
+ if(x_tmp>0x7fff)x_tmp-=0xffff;
+ if(y_tmp>0x7fff)y_tmp-=0xffff;
+
+ Angles[1] = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
+ Angles[2] = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
+ Angles[0] = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3; //Yaw
+ Angles[3] = atan2((double)(y_tmp) , (double)(x_tmp)) * (180 / 3.14159265) + 180; // Compass angle in degrees
+
+
+}
+
+/************************************************************/
+//下面的代码是MPU9250的各种操作
+/************************************************************/
+void IMU::I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t Data)
+{
+ char data_write[2];
+
+ data_write[0] = RegAddr;
+ data_write[1] = Data;
+
+ i2c.write(DevAddr,data_write,2,0);
+}
+
+uint8_t IMU::I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr)
+{
+ char data_write[2];
+ char data_read[2];
+ char data;
+
+ data_write[0] = RegAddr;
+
+ i2c.write(DevAddr,data_write,1,1);
+ i2c.read(DevAddr,data_read,1,0);
+
+ data = data_read[0];
+
+ return data;
+}
+/**
+ * @brief Initializes MPU9250
+ * @param None
+ * @retval None
+ */
+void IMU::MPU9250_Init()
+{
+ I2C_WriteOneByte(GYRO_ADDRESS,PWR_MGMT_1, 0x00);
+ I2C_WriteOneByte(GYRO_ADDRESS,SMPLRT_DIV, 0x07);
+ I2C_WriteOneByte(GYRO_ADDRESS,CONFIG, 0x06);
+ I2C_WriteOneByte(GYRO_ADDRESS,GYRO_CONFIG, 0x10);
+ I2C_WriteOneByte(GYRO_ADDRESS,ACCEL_CONFIG, 0x01);
+
+ Thread::wait(10);
+
+ if(MPU9250_Check())
+ {
+ printf("\r\n MPU9255 Ready!\n");
+ }
+ else
+ {
+ printf("\r\n MPU9255 Erro!\n");
+ }
+
+ MPU9250_InitGyrOffset();
+}
+
+/**
+ * @brief Digital filter
+ * @param *pIndex:
+ * @param *pAvgBuffer:
+ * @param InVal:
+ * @param pOutVal:
+ *
+ * @retval None
+ *
+ */
+void IMU::MPU9250_CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_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 Get accelerometer datas
+ * @param None
+ * @retval None
+ */
+void IMU::MPU9250_READ_ACCEL(void)
+{
+ uint8_t i;
+ int16_t InBuffer[3] = {0};
+ static int32_t OutBuffer[3] = {0};
+ static MPU9250_AvgTypeDef MPU9250_Filter[3];
+
+ BUF[0]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_L);
+ BUF[1]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_XOUT_H);
+ InBuffer[0]= (BUF[1]<<8)|BUF[0];
+
+ BUF[2]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_L);
+ BUF[3]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_YOUT_H);
+ InBuffer[1]= (BUF[3]<<8)|BUF[2];
+
+ BUF[4]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_L);
+ BUF[5]=I2C_ReadOneByte(ACCEL_ADDRESS,ACCEL_ZOUT_H);
+ InBuffer[2]= (BUF[5]<<8)|BUF[4];
+
+ for(i = 0; i < 3; i ++)
+ {
+ MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
+ }
+ accel[0] = *(OutBuffer + 0);
+ accel[1] = *(OutBuffer + 1);
+ accel[2] = *(OutBuffer + 2);
+}
+
+/**
+ * @brief Get gyroscopes datas
+ * @param None
+ * @retval None
+ */
+void IMU::MPU9250_READ_GYRO(void)
+{
+ uint8_t i;
+ int16_t InBuffer[3] = {0};
+ static int32_t OutBuffer[3] = {0};
+ static MPU9250_AvgTypeDef MPU9250_Filter[3];
+
+ BUF[0]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_L);
+ BUF[1]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_XOUT_H);
+ InBuffer[0]= (BUF[1]<<8)|BUF[0];
+
+ BUF[2]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_L);
+ BUF[3]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_YOUT_H);
+ InBuffer[1] = (BUF[3]<<8)|BUF[2];
+
+ BUF[4]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_L);
+ BUF[5]=I2C_ReadOneByte(GYRO_ADDRESS,GYRO_ZOUT_H);
+ InBuffer[2] = (BUF[5]<<8)|BUF[4];
+
+ for(i = 0; i < 3; i ++)
+ {
+ MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
+ }
+ gyro[0] = *(OutBuffer + 0) - MPU9250_Offset.X;
+ gyro[1] = *(OutBuffer + 1) - MPU9250_Offset.Y;
+ gyro[2] = *(OutBuffer + 2) - MPU9250_Offset.Z;
+}
+
+/**
+ * @brief Get compass datas
+ * @param None
+ * @retval None
+ */
+void IMU::MPU9250_READ_MAG(void)
+{
+ uint8_t i;
+ int16_t InBuffer[3] = {0};
+ static int32_t OutBuffer[3] = {0};
+ static MPU9250_AvgTypeDef MPU9250_Filter[3];
+
+ I2C_WriteOneByte(GYRO_ADDRESS,0x37,0x02);//turn on Bypass Mode
+ Thread::wait(10);
+ I2C_WriteOneByte(MAG_ADDRESS,0x0A,0x01);
+ Thread::wait(10);
+ BUF[0]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_L);
+ BUF[1]=I2C_ReadOneByte (MAG_ADDRESS,MAG_XOUT_H);
+ InBuffer[1] =(BUF[1]<<8)|BUF[0];
+
+ BUF[2]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_L);
+ BUF[3]=I2C_ReadOneByte(MAG_ADDRESS,MAG_YOUT_H);
+ InBuffer[0] = (BUF[3]<<8)|BUF[2];
+
+ BUF[4]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_L);
+ BUF[5]=I2C_ReadOneByte(MAG_ADDRESS,MAG_ZOUT_H);
+ InBuffer[2] = (BUF[5]<<8)|BUF[4];
+ InBuffer[2] = -InBuffer[2];
+
+ for(i = 0; i < 3; i ++)
+ {
+ MPU9250_CalAvgValue(&MPU9250_Filter[i].Index, MPU9250_Filter[i].AvgBuffer, InBuffer[i], OutBuffer + i);
+ }
+ magn[0] = *(OutBuffer + 0)-MPU9250_Magn_Offset.X_Off_Err;
+ magn[1] = *(OutBuffer + 1)-MPU9250_Magn_Offset.Y_Off_Err;
+ magn[2] = *(OutBuffer + 2)-MPU9250_Magn_Offset.Z_Off_Err;
+}
+
+/**
+ * @brief Check MPU9250,ensure communication succeed
+ * @param None
+ * @retval true: communicate succeed
+ * false: communicate fualt
+ */
+bool IMU::MPU9250_Check(void)
+{
+ if(WHO_AM_I_VAL == I2C_ReadOneByte(DEFAULT_ADDRESS, WHO_AM_I))
+ {
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+/**
+ * @brief Initializes gyroscopes offset
+ * @param None
+ * @retval None
+ */
+void IMU::MPU9250_InitGyrOffset(void)
+{
+ uint8_t i;
+ int32_t TempGx = 0, TempGy = 0, TempGz = 0;
+
+ for(i = 0; i < 32; i ++)
+ {
+ MPU9250_READ_GYRO();
+
+ TempGx += gyro[0];
+ TempGy += gyro[1];
+ TempGz += gyro[2];
+
+ Thread::wait(1);
+ }
+
+ MPU9250_Offset.X = TempGx >> 5;
+ MPU9250_Offset.Y = TempGy >> 5;
+ MPU9250_Offset.Z = TempGz >> 5;
+
+}
\ No newline at end of file