|
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;
}
希望在此,能和大家交流,学习以及给他人一些帮助!!! |
阿莫论坛20周年了!感谢大家的支持与爱护!!
如果天空是黑暗的,那就摸黑生存;
如果发出声音是危险的,那就保持沉默;
如果自觉无力发光,那就蜷伏于牆角。
但是,不要习惯了黑暗就为黑暗辩护;
也不要为自己的苟且而得意;
不要嘲讽那些比自己更勇敢的人。
我们可以卑微如尘土,但不可扭曲如蛆虫。
|