|
|
Dear All ！
本人四轴飞机新人，在此寻求伟大网友的帮助。
目前基本程序架构已搭建完成，用MPU6050计算pitch和roll，在网上看到有人用四元素法求得角度（对这个不太熟悉，直接搬抄过来），出现以下2个问题：
1、在初始化时，求得静止时的ACC_Z的值只有1930，翻转过来是2253，和设定的2048有较大差距；
2、在用四元数法计算角度，发现当ptich小于60度，pitch和roll都正常，但在ptich增大到84度左右时，roll突然增加的特别多，变得好大。
以下是我的code，
char MPU6050_Init(void){
long i;
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,PWR_MGMT_1,0x80);
for(i=0;i<0xFFFFF;i++);
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,0x68,0x07);
for(i=0;i<0xFFFFF;i++);
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,PWR_MGMT_1, 0x00); //open power
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,SMPLRT_DIV, 0x00); //ÍÓÂÝÒDzÉÑùƵÂÊ,1k(Hz)/(0x01+0x00)=1k(Hz)
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,CONFIG, 0x03); //µÍͨÂ˲¨44KHZ
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,I2C_MST_CTRL,0x0D); //I2C clk 400khz
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,GYRO_CONFIG, 0x18); //+-2000/s
MPU6050_I2C_WriteSingleByte(MPU6050_Addr,ACCEL_CONFIG, 0x18); //+-16g 2048
if(MPU6050_I2C_ReadSingleByte(MPU6050_Addr,0x75) != WHO_AM_I)
return 1;
return 0;
}
void IMU_Init(void){
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0f;
eyInt = 0.0f;
ezInt = 0.0f;
}
#define IMU_HALFT 0.001f //1ms
#define PI 3.1415927f
#define IMU_KP 40.0f
#define IMU_KI 0.1f
#define GYRO_R 16.4f
void IMUupdate(void){
float norm;
float vx,vy,vz;
float ex,ey,ez;
float gx = (float)(MPU6050_AngleInfo.GYRO[0] - MPU6050_AngleInfo.GYROOffset[0])/16.384f /GYRO_R;
float gy = (float)(MPU6050_AngleInfo.GYRO[1] - MPU6050_AngleInfo.GYROOffset[1])/16.384f /GYRO_R;
float gz = (float)(MPU6050_AngleInfo.GYRO[2] - MPU6050_AngleInfo.GYROOffset[2])/16.384f /GYRO_R;
float ax = (float)(MPU6050_AngleInfo.ACC[0] - MPU6050_AngleInfo.ACCOffset[0]);
float ay = (float)(MPU6050_AngleInfo.ACC[1] - MPU6050_AngleInfo.ACCOffset[1]);
float az = (float)(MPU6050_AngleInfo.ACC[2] - MPU6050_AngleInfo.ACCOffset[2] + 2048) ;
//µÃµ½x,y,zÈýÖáµÄµ¥Î»ÏòÁ¿Öµ
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax/norm;
ay = ay/norm;
az = az/norm;
vx = 2*(q1*q3-q0*q2);
vy = 2*(q0*q1+q2*q3);
vz = q0*q0-q1*q1-q2*q2+q3*q3;
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
exInt = exInt + ex*IMU_KI;
eyInt = eyInt + ey*IMU_KI;
ezInt = ezInt + ez*IMU_KI;
gx = gx+IMU_KP*ex +exInt;
gy = gy+IMU_KP*ey +eyInt;
gz = gz+IMU_KP*ez +ezInt;
q0 = q0+(-q1*gx -q2 *gy - q3*gz)*IMU_HALFT;
q1 = q1+( q0*gx +q2 *gz - q3*gy)*IMU_HALFT;
q2 = q2+( q0*gy -q1 *gz + q3*gx)*IMU_HALFT;
q3 = q3+( q0*gz +q1 *gy - q2*gx)*IMU_HALFT;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0/norm;
q1 = q1/norm;
q2 = q2/norm;
q3 = q3/norm;
MPU6050_AngleInfo.Pitch = asin(-2*q1*q3 +2*q0*q2)*57.3;
MPU6050_AngleInfo.Roll = atan2(2*q2*q3 + 2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3)*57.3;
// MPU6050_AngleInfo.Yaw = atan2(2*q1 *q2 + 2*q0*q3, q0*q0 + q1*q1 - q2*q2 - q3*q3)*57.3;
}
希望在此，能和大家交流，学习以及给他人一些帮助！！！ |
|
发表于 2016-9-21 16:56:06
楼主
楼主，最后的大字体改下，不懂的话，读下版规
，