原谅我又发一次贴，不懂为什么，不懂，不懂~~求助

小P

问题是这样的：我用的是MPU6050，然后我的航向角YAW很奇怪，向左转的时候从0转到25度之后就又变为0了，然后如果从0又右转动的话，又瞬间变成25度左右 纳闷了好久，其他两个角度倒是挺稳定的 用的是IMU四元数，   Q_ANGLE.Y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch   Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll   Q_ANGLE.Z  +=  GYRO_AVG.Z* Gyro_G * 0.002f;//YAW，0.002是周期，GYRO_AVG.Z是陀螺仪的Z轴经过滑窗滤波之后的Z轴输出，Gyro_G 为常数，根据量程确定 然后还有一个问题就是计算航向角YAW还有这条公式， Q_ANGLE.Z  = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw 我之前用这条之后，发现随着时间推移，那个角度会慢慢增加，然后到差不多25度的时候也是变为从0开始 求大神帮忙解决一下，或者提供下你们的思路 PS：为什么把问题放在四轴飞行器的板块好像没人去看过的样子~~~~ 对了，还有一个问题就是我在看匿名的程序时候，发现他的PID控制好奇怪呀，或者说根本就没用到PID运算呀，不知道这份代码为什么这样子~~~ 程序是这样子的~~

void CONTROL(float rol, float pit, float yaw)
{
        u16 moto1=0,moto2=0,moto3=0,moto4=0;
       
        PID_ROL.pout = PID_ROL.P * rol;//比例
        PID_ROL.dout = PID_ROL.D * MPU6050_GYRO_LAST.X;//微分
       
        PID_PIT.pout = PID_PIT.P * pit;//比例
        PID_PIT.dout = PID_PIT.D * MPU6050_GYRO_LAST.Y;//微分
       
        PID_YAW.dout = PID_YAW.D * MPU6050_GYRO_LAST.Z;
       
        PID_ROL.OUT = PID_ROL.pout + PID_ROL.iout + PID_ROL.dout;
        PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
        PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
       
        if(Rc_Get.THROTTLE>1200)
        {
                moto1 = Rc_Get.THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
                moto2 = Rc_Get.THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
                moto3 = Rc_Get.THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
                moto4 = Rc_Get.THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
                //现在遥控数据将会影响到飞机的飞行状态了
        }
        else
        {
                moto1 = 10;
                moto2 = 10;
                moto3 = 10;
                moto4 = 10;
        }
        if(ARMED)            Moto_PwmRflash(moto1,moto2,moto3,moto4);
        else                        Moto_PwmRflash(0,0,0,0);
}

然后再中断中每隔4ms控制一次姿态，然后控制的输入值是姿态的结算值，当场懵逼，这是PID？


求各位大大帮忙看看

xuande

不懂，
这问题比较高端。