姿态解算求助，有大神搭救吗

wxavuqvm5m · 发表于 2018-5-21 19:37:01

#define Kp 1.0F//比例系数
#define Ki 0.001F//积分系数
#define halfT 0.001F
static float q0=0.00,q1=0.00,q2=0.00,q3=0.00;//初始化四元数
float exInt=0,eyInt=0,ezInt=0;//Îó2îμÄ»y·Ö

void Posture_update(float gx,float gy,float gz,float ax,float ay,float az)
{
float norm;
float vx,vy,vz;
float ex,ey,ez;
if(ax*ay*az==0)
{
printf("accelerometer error\r\n");
return;
}

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*Ki);
eyInt=eyInt+(ey*Ki);
ezInt=ezInt+(ez*Ki);

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*gz)*halfT;
q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;
q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;

norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
q0=q0/norm;
q1=q1/norm;
q2=q2/norm;
q3=q3/norm;

}
代码附上，现在能正确读取加速度计和陀螺仪数据，并且加速度换算成m/s2,陀螺仪换算成弧度制，上面这个是四元数更新函数，但是执行以后四元数并没有更新，有大神帮忙看看吗