无人机项目跟踪记录四十九----角速度环的pid控制详解

角速度环的pid控制的函数是void CtrlAttiRate(void)，具体解释如下：

首先初始化变量，获取陀螺仪读取的角速度。

    float yawRateTarget=0;                                   //期望的偏航角速度
	
    static uint32_t tPrev=0;
	  float gryoPitch = imu.gyro[PITCH]-imu.gyroOffset[PITCH];   //当前俯仰值
	  float gryoRoll = imu.gyro[ROLL]-imu.gyroOffset[ROLL];      //当前横滚值
	  float gryoYaw = imu.gyro[YAW]-imu.gyroOffset[YAW];         //当前航向值（也既是航向值）

计算两次调用的间隔时间：

    float dt=0,t=0;                    //两次调用间隔时间
    t=micros();
    dt=(tPrev>0)?(t-tPrev):0;
    tPrev=t;

获取希望的偏航角：为何取负值没有搞清楚，估计与机体系和地理系变换有关系。如果陀螺仪读取的角度小于某个阈值就置为零。

    yawRateTarget=-(float)RC_DATA.YAW;  //希望的偏航角
    //注意，原来的pid参数，对应的是 ad值,故转之
#ifdef IMU_SW
	if(fabs(gryoPitch) < 0.001)
	{
		gryoPitch = 0;
	}
	if(fabs(gryoRoll) < 0.001)
	{
		gryoRoll = 0;
	}
	if(fabs(gryoYaw) < 0.001)
	{
		gryoYaw = 0;
	}

做角速度环的PID自整定，与角度环相似。注意一点就是，角度环是大调整，角速度环是内环细调，所以角度环条用的间隔时间长，角速度环的间隔时间短。

    PID_Postion_Cal(&pitch_rate_PID,pitch_angle_PID.Output,gryoPitch*180.0f/M_PI_F,dt);   //做角速度环的PID自整定，俯仰角的自整定
    PID_Postion_Cal(&roll_rate_PID,roll_angle_PID.Output,gryoRoll*180.0f/M_PI_F,dt);//gyroxGloble    //做角速度环的PID自整定，横滚角的自整定
    PID_Postion_Cal(&yaw_rate_PID,yawRateTarget,gryoYaw*180.0f/M_PI_F,dt);//DMP_DATA.GYROz   //做角速度环的PID自整定，航向角的自整定
#else

    //原参数对应于 DMP的直接输出gyro , 是deg.  且原DMP之后的处理运算是错误的
    PID_Postion_Cal(&pitch_rate_PID,pitch_angle_PID.Output,imu.gyro[PITCH]*DMP_GYRO_SCALE,0);
    PID_Postion_Cal(&roll_rate_PID,roll_angle_PID.Output,imu.gyro[ROLL]*DMP_GYRO_SCALE,0);//gyroxGloble
    PID_Postion_Cal(&yaw_rate_PID,yawRateTarget,imu.gyro[YAW]*DMP_GYRO_SCALE,0);          //DMP_DATA.GYROz
#endif

    Pitch = pitch_rate_PID.Output;
    Roll  = roll_rate_PID.Output;
    Yaw   = yaw_rate_PID.Output;