无人机项目跟踪记录四十七--角度Pid控制模块详解（1）

角度控制模块函数为：void CtrlAttiAng(void)，函数作用是通过Pid算法，完成角度控制与角速度串行控制环的外环控制（角度环控制）。详解如下：

首先初始化：

    static uint32_t tPrev=0; //记录上次执行的时间
    float angTarget[3]= {0};  // 期望的角度值
    float dt=0,t=0;           //调用此函数的时间间隔

下面是计算两次调用此函数的时间间隔：

    t=micros();              //程序执行后到此条语句的时间
    dt=(tPrev>0)?(t-tPrev):0;  //求两次调用的时间间隔
    tPrev=t;           

然后是获取期望角度值，如果是遥控模式就获取遥控器输出值，如果是定高模式就获得计算出来的值：

    if(altCtrlMode==MANUAL) {    //如果手动遥控将遥控器输出的横滚俯仰角赋值给期望角
        angTarget[ROLL]=(float)(RC_DATA.ROOL);
        angTarget[PITCH]=(float)(RC_DATA.PITCH);
    } else {
        angTarget[ROLL]=rollSp;   //如果是定高模式将计算出来的获取过来
        angTarget[PITCH]=pitchSp;
    }

初始水平放置时候，记录下陀螺仪的方位角，将此时候的机体系当做地理系来看。无人机起飞后，经过水平旋转后机体系与之前的地理系有了变换角度，遥控器输出是按照地理输出的横滚、俯仰值，要把它变化到当前机体中。如下代码，不过为何要这样做三角运算，公式怎么得到的，没有想明白！

    if(headFreeMode) {//如果是无头模式
#ifdef YAW_CORRECT
			float radDiff = -(imu.yaw - headHold) * M_PI_F / 180.0f;  //计算当前机体和记录时刻的地理系水平转动的角度
#else
        float radDiff = (imu.yaw - headHold) * M_PI_F / 180.0f;
#endif
        float cosDiff = cosf(radDiff);
        float sinDiff = sinf(radDiff);
			float tarPitFree = angTarget[PITCH] * cosDiff + angTarget[ROLL] * sinDiff;  //经过三角变换将地理系系期望得到的角度转换成机体系中
        angTarget[ROLL] = angTarget[ROLL] * cosDiff - angTarget[PITCH] * sinDiff;  //同上
        angTarget[PITCH] = tarPitFree;

下面是角度环pid自整定算法了：

    PID_Postion_Cal(&pitch_angle_PID,angTarget[PITCH],imu.pitch,dt);  //做俯仰角的pid自整定运算
    PID_Postion_Cal(&roll_angle_PID,angTarget[ROLL],imu.roll,dt);     //做横滚角的pid自整定运算