CONTROL.c（参考匿名的）

#include "CONTROL.h"
#include "BSP.H"
#include "UART2.h"
#include "IMU.h"
#include "MPU6050.h"
#include "MOTO.h"
PID PID_ROL,PID_PIT,PID_YAW;
u8 ARMED = 0;
u16 moto1=0,moto2=0,moto3=0,moto4=0;

float Get_MxMi(float num,float max,float min)//将num限制在min与max之间
{
    if(num>max)
        return max;
    else if(numreturn min;
    else
        return num;
}         //解算出来的自我姿态，根据此得出自稳所需改变角度  //遥控发来的所需要改变的角度以进行倾斜的飞行，进行前进后退或转向 
          //Q_ANGLE.X ,         Q_ANGLE.Y,     Q_ANGLE.Z, //RC_Target_ROL, RC_Target_PIT, RC_Target_YAW
void CONTROL(float rol_now, float pit_now, float yaw_now,   float rol_tar, float pit_tar, float yaw_tar)
{

    vs16 throttle;
    vs16 yaw_d;
    float rol = rol_tar + rol_now;//将所有要改变的值加起来
    float pit = pit_tar + pit_now;
    float yaw = yaw_tar + yaw_now;

    throttle = Rc_Get.THROTTLE - 1000;//油门，代表电机的转速
    if(throttle<0)  throttle=0;//消除实际操作误差

    PID_ROL.IMAX = throttle/2;  //积分项限幅=throttle的1/2，为什么和throttle有关？
    Get_MxMi(PID_ROL.IMAX,1000,0);//并将积分项限幅限制在0-1000之间
    PID_PIT.IMAX = PID_ROL.IMAX;


    /****************************  P比例调节   ***********************************/

    PID_ROL.pout = PID_ROL.P * rol;
    PID_PIT.pout = PID_PIT.P * pit;
    PID_YAW.pout = PID_YAW.P * yaw;

    /****************************  I积分调节   **********************************/

    if(rol_tar*rol_tar<0.1 && pit_tar*pit_tar<0.1 && rol_now*rol_now<30 && pit_now*pit_now<30 && throttle>300)
    {//target遥控几乎未动，now当前比较接近水平，油门很大

        PID_ROL.iout += PID_ROL.I * rol;  
        PID_PIT.iout += PID_PIT.I * pit;                                 //积分项容易超调
        PID_ROL.iout = Get_MxMi(PID_ROL.iout,PID_ROL.IMAX,-PID_ROL.IMAX);//积分项限制在积分项限幅内
        PID_PIT.iout = Get_MxMi(PID_PIT.iout,PID_PIT.IMAX,-PID_PIT.IMAX);//防止大角度积累造成积分超调
    }
    else if(throttle<200)//油门很小不需要积分调节，为什么？
    {
        PID_ROL.iout = 0;
        PID_PIT.iout = 0;
    }

  /***************************  D微分调节  ***********************************/ 

    PID_ROL.dout = PID_ROL.D * MPU6050_GYRO_LAST.X;//，飞机头朝X正方向，ROL绕X轴转
    PID_PIT.dout = PID_PIT.D * MPU6050_GYRO_LAST.Y;//6050测得角速度，角度的微分就是角速度

    if(1480>Rc_Get.YAW || Rc_Get.YAW>1520)//YAW的微分调节，1500是什么也不改变的零点值
    {         //自己飘的YAW值       //遥控所需要的改变量，*10提高权重,加快响应
        yaw_d = MPU6050_GYRO_LAST.Z + (Rc_Get.YAW-1500)*10;
        GYRO_I.Z = 0;//假设遥控不动的情况下YAW不可能自己飘，没有地磁作参考直接置零算了              
    }   //在IMU的preparedata()中GYRO_I.Z += MPU6050_GYRO_LAST.Z*Gyro_G*0.001;//0.001为dt
                              //GYRO_I.Z相当于绕Z轴（即yaw）旋转的总角度
        //在IMU的 IMUupdata ()中Q_ANGLE.Z = GYRO_I.Z
    else //为什么要分两种情况//遥控的左转右转功能不要了
    yaw_d = MPU6050_GYRO_LAST.Z;//很小的遥控改变量就不要了
    PID_YAW.dout = PID_YAW.D * yaw_d;

    /**************************************************************************************/


    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(throttle>200)//                                - +                           + +
    {            //两正两负，调整roll的话PID_ROL.OUT要 - +,调整pit的话PID_PIT.OUT 要 - -
        moto3= throttle - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;//赋给电机值
        moto1= throttle + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;//加减号一定两正两负，电机对称
        moto2 = throttle + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;//要改，先注释掉其他三个
        moto4= throttle - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
    }
    else//定义电机的死区，避免误操作
    {
        moto1 = 0;
        moto2 = 0;
        moto3 = 0;
        moto4 = 0;
    }
    if(ARMED)   {PCout(13)=0;Moto_PwmRflash(moto1,moto2,moto3,moto4);}//使能电机
    else            {PCout(13)=1;Moto_PwmRflash(0,0,0,0);}
}