为什么姿态解算代码中陀螺仪换算成弧度制后四元数并没有更新？

#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   errorrn");
                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,陀螺仪换算成弧度制，上面这个是四元数更新函数，但是执行以后四元数并没有更新，有大神帮忙看看吗