无名飞控的姿态解算和控制（二）

于 2017-12-17 10:02:51 发布
阅读量3.5k 收藏 21
点赞数 4
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/wang2012113132/article/details/78823621
本文详细介绍了四旋翼飞行器控制系统的实现原理及关键算法。从PPM信号解析到飞行模式选择，再到姿态环控制的具体实现，包括角度控制、角速度控制等环节，并深入分析了油门补偿及PWM输出的原理。

摘要生成于 C知道 ，由 DeepSeek-R1 满血版支持， 前往体验 >

接上篇继续码函数

Total_Control();//总控制器
查找定义： 

void Total_Control(void)
{
  static uint16_t Mode_Check_Cnt=0;
  /*************控制器模式选着******************/
  Mode_Check_Cnt++;  
  if(Mode_Check_Cnt>=5)//每20ms检测一次，PPM信号刷新周期为20ms
  {
    Controler_Mode_Select();
    Mode_Check_Cnt=0;
  }
  /*************主导控制器******************/
  Main_Leading_Control();
  /*************姿态环控制器*****************/
  Altitude_Control();
}
先进入Controler_Mode_Select 

/*****************遥控器行程设置**********************/
#define  Deadband       300//油门中位死区
#define  Deadzone_Min   350//油门杆给定期望速度时，下行程临界值
#define  Deadzone_Max   650//油门杆给定期望速度时，上行程临界值
#define  Thr_Top 1000//油门最大上行程
#define  Thr_Buttom 0//油门最大下行程
#define  Climb_Up_Speed_Max    400//向上最大攀爬速度
#define  Climb_Down_Speed_Max  150//向下最大下降速度
#define  Thr_Start  1100//起转油门量
#define  Thr_Min 1000
#define  Thr_Idle 1100//油门怠速
uint16 Motor_PWM_1,Motor_PWM_2,Motor_PWM_3,Motor_PWM_4;//四个电机输出PWM
uint8 MotorTest=0xff;//电机序号测试


float Yaw_Infront_Feadback=0.35;//偏航角前馈控制;
uint8_t Controler_Mode=1,Last_Controler_Mode=1;
#define Self_Balance_Mode 1//自稳、纯姿态加油门补偿
#define High_Hold_Mode    2//定高模式
#define Pos_Hold_Mode 3//定点模式

uint8_t Control_Mode_Change=0;
#define Self_To_High  1//自稳切定高
#define High_To_Pos   2//定高切定点

//偏航模式  
uint8 Yaw_Control_Mode=0;
#define No_Head_Mode 0
#define Guide_Direction_Mode 1

float  Position_Hold_Pitch=0,Position_Hold_Roll=0;
float  Speed_Hold_Pitch=0,Speed_Hold_Roll=0;
uint16_t High_Hold_Throttle=0; 
uint8_t  High_Hold_SetFlag=0;
uint8_t  Pos_Hold_SetFlag=0;
uint16_t  HomePoint_Is_Okay=0;
void Controler_Mode_Select()
{
   Last_Controler_Mode=Controler_Mode;//上次控制器模式
   if(PPM_Databuf[6]>=1900)      Controler_Mode=3;//GPS定点
   else if(PPM_Databuf[6]>=1400) Controler_Mode=2;//气压计定高
   else if(PPM_Databuf[6]>=900)  Controler_Mode=1;//纯姿态自稳
   
   
   if(Controler_Mode!=Last_Controler_Mode)  
   {  
     if(Controler_Mode==3)  {Control_Mode_Change=2;Pos_Hold_SetFlag=0;}//定高切定点
     if(Controler_Mode==2)  {Control_Mode_Change=1;High_Hold_SetFlag=0;}//自稳切定高
   }
   else Control_Mode_Change=0;//无模式切换
   
 
   if(Controler_Mode==High_Hold_Mode//本次为定高模式
      &&Last_Controler_Mode==Self_Balance_Mode//上次为自稳模式
      &&High_Hold_SetFlag==0)//高度只设置一次
   {  
      High_Hold_Throttle=1360;//保存当前油门值，只存一次
    /*******************将当前惯导竖直位置估计作为目标高度***************************/   
      Total_Controler.High_Position_Control.Expect
        =NamelessQuad.Position[_YAW];//将开关拨动瞬间的惯导高度设置为期望高度
      High_Hold_SetFlag=1;
   }
   
   
   if(Controler_Mode==Pos_Hold_Mode//本次为定点模式
      &&Last_Controler_Mode==High_Hold_Mode//上次为定高模式
       &&Pos_Hold_SetFlag==0
        &&GPS_Sate_Num>=6//定位卫星超过6个
         &&GPS_Quality<=5)//水平精度因子大于6不可用,水平位置期望只设置一次
   {  
     /*******************将当前惯导水平位置估计作为目标悬停点************************/
      Total_Controler.Latitude_Position_Control.Expect
        =NamelessQuad.Position[_ROLL];
      Total_Controler.Longitude_Position_Control.Expect
        =NamelessQuad.Position[_PITCH];
      Pos_Hold_SetFlag=1;
   }
/******当前档位为定点模式，但显示悬停点未设置，说明之前未满足设置定点条件有三种情况********
   1、初始通过开关切定点模式时，GPS状态未满足悬停条件；
   2、初始通过开关切定点模式时，GPS状态未满足悬停条件，之后持续检测仍然未满足GPS定点条件；
   3、之前GPS状态满足悬停条件，但由于GPS信号质量变差，自动切换至不满足GPS定点条件；
*******重新判断当下是否满足定点条件，如满足条件更新悬停点，允许进入定点模式******/  
   if(Controler_Mode==Pos_Hold_Mode
      &&Pos_Hold_SetFlag==0)
   {
      if(GPS_Sate_Num>=6//定位卫星超过6个
         &&GPS_Quality<=5)//水平精度因子大于6不可用
      {
      /*******************将当前惯导水平位置估计作为目标悬停点************************/
        Total_Controler.Latitude_Position_Control.Expect=NamelessQuad.Position[_ROLL];
        Total_Controler.Longitude_Position_Control.Expect=NamelessQuad.Position[_PITCH];
        Pos_Hold_SetFlag=1;
      } 
   }
/******若满足GPS定点模式，对Pos_Hold_SetFlag置1，允许进入定点模式*****************/
}
对于PPM的理解可以看这篇博客 PPM信号编码格式和 PPM信号介绍

首先通过ppm信号脉宽解析飞控模式

void Controler_Mode_Select()
{
   Last_Controler_Mode=Controler_Mode;//上次控制器模式
   if(PPM_Databuf[6]>=1900)      Controler_Mode=3;//GPS定点
   else if(PPM_Databuf[6]>=1400) Controler_Mode=2;//气压计定高
   else if(PPM_Databuf[6]>=900)  Controler_Mode=1;//纯姿态自稳
模式选择： 

  if(Controler_Mode!=Last_Controler_Mode)  
   {  
     if(Controler_Mode==3)  {Control_Mode_Change=2;Pos_Hold_SetFlag=0;}//定高切定点
     if(Controler_Mode==2)  {Control_Mode_Change=1;High_Hold_SetFlag=0;}//自稳切定高
   }
   else Control_Mode_Change=0;//无模式切换

假如切换到定高模式 
   if(Controler_Mode==High_Hold_Mode//本次为定高模式
      &&Last_Controler_Mode==Self_Balance_Mode//上次为自稳模式
      &&High_Hold_SetFlag==0)//高度只设置一次
   {  
      High_Hold_Throttle=1360;//保存当前油门值，只存一次
    /*******************将当前惯导竖直位置估计作为目标高度***************************/   
      Total_Controler.High_Position_Control.Expect
        =NamelessQuad.Position[_YAW];//将开关拨动瞬间的惯导高度设置为期望高度
      High_Hold_SetFlag=1;
   }

假如切换到定点模式（需要GPS） 

   if(Controler_Mode==Pos_Hold_Mode//本次为定点模式
      &&Last_Controler_Mode==High_Hold_Mode//上次为定高模式
       &&Pos_Hold_SetFlag==0
        &&GPS_Sate_Num>=6//定位卫星超过6个
         &&GPS_Quality<=5)//水平精度因子大于6不可用,水平位置期望只设置一次
   {  
     /*******************将当前惯导水平位置估计作为目标悬停点************************/
      Total_Controler.Latitude_Position_Control.Expect
        =NamelessQuad.Position[_ROLL];
      Total_Controler.Longitude_Position_Control.Expect
        =NamelessQuad.Position[_PITCH];
      Pos_Hold_SetFlag=1;
   }
考虑意外情况： 

/******当前档位为定点模式，但显示悬停点未设置，说明之前未满足设置定点条件有三种情况********
   1、初始通过开关切定点模式时，GPS状态未满足悬停条件；
   2、初始通过开关切定点模式时，GPS状态未满足悬停条件，之后持续检测仍然未满足GPS定点条件；
   3、之前GPS状态满足悬停条件，但由于GPS信号质量变差，自动切换至不满足GPS定点条件；
*******重新判断当下是否满足定点条件，如满足条件更新悬停点，允许进入定点模式******/  
   if(Controler_Mode==Pos_Hold_Mode
      &&Pos_Hold_SetFlag==0)
   {
      if(GPS_Sate_Num>=6//定位卫星超过6个
         &&GPS_Quality<=5)//水平精度因子大于6不可用
      {
      /*******************将当前惯导水平位置估计作为目标悬停点************************/
        Total_Controler.Latitude_Position_Control.Expect=NamelessQuad.Position[_ROLL];
        Total_Controler.Longitude_Position_Control.Expect=NamelessQuad.Position[_PITCH];
        Pos_Hold_SetFlag=1;
      } 
   }
/******若满足GPS定点模式，对Pos_Hold_SetFlag置1，允许进入定点模式*****************/
}

接着回到Total_Control进入Main_Leading_Control();

uint16 Throttle=0,Throttle_Hover=1350;
void Main_Leading_Control(void)
{
/*********************根据遥控器切换档位，飞控进入不同模式****************************/  
    if(Controler_Mode==1)//姿态自稳定模式
  {
     Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
     Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];

     if(Throttle_Control<=1000)   Throttle=1000;
     else Throttle=Throttle_Control;//油门直接来源于遥控器油门给定
  }
  else if(Controler_Mode==2)//定高模式
  {
/**************************定高模式，水平姿态期望角来源于遥控器******************************************/     
     Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
     Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];   
     
/*高度控制器第1步*/    
/********
    **
    **
    **
    **
    **
 ********/ 
/*******************************高度控制器开始****************************************/ 
/****************定高：高度位置环+速度环+加速度环，控制周期分别为8ms、4ms、4ms*******************/     
      if(RC_NewData[0]>=Deadzone_Min&&RC_NewData[0]<=Deadzone_Max)
     {
          static int16_t High_Hold_Cnt=0;//定高控制周期计数器
          //高度位置环输出给定速度期望
          if(Total_Controler.High_Position_Control.Expect==0)//油门回中后，只设置一次
          {
            Total_Controler.High_Position_Control.Expect=NamelessQuad.Position[_YAW];//油门回中后，更新高度期望
          }
          High_Hold_Cnt++;
          if(High_Hold_Cnt>=2)//竖直高度控制周期2*4=8ms
          {
              High_Hold_Cnt=0;
              Total_Controler.High_Position_Control.FeedBack=NamelessQuad.Position[_YAW];//反馈 
              PID_Control_High(&Total_Controler.High_Position_Control);//海拔高度位置控制器
              //内环速度期望
              Total_Controler.High_Speed_Control.Expect=Total_Controler.High_Position_Control.Control_OutPut;
          }
     }
     else if(RC_NewData[0]>Deadzone_Max)//给定上升速度期望
     {
            //油门杆上推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Up_Speed_Max*(RC_NewData[0]-Deadzone_Max)/(Thr_Top-Deadzone_Max);//最大上升速度50cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     else if(RC_NewData[0]<Deadzone_Min)//给定下降速度期望
     {
            //油门杆下推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Down_Speed_Max*(RC_NewData[0]-Deadzone_Min)/(Deadzone_Min-Thr_Buttom);//最大下降速度40cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     
/*高度控制器第2步*/ 
/********
        *
        *
   ****
 *
 *
 ********/  
     
/*******************************竖直速度控制器开始*******************************************************************/     
     Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
     PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制
/*******************************竖直速度控制器结束******************************************************************/ 
     
/*高度控制器第3步*/ 
/********
       **
     **
   **
     **
       **
 ********/      
     
/*******************************竖直加速度控制器开始******************************************************************/
      Total_Controler.High_Acce_Control.Expect=Total_Controler.High_Speed_Control.Control_OutPut;//加速度期望
      Total_Controler.High_Acce_Control.FeedBack=FilterAfter_NamelessQuad.Acceleration[_YAW];//加速度反馈
      PID_Control_High(&Total_Controler.High_Acce_Control);//海拔高度加速度控制
/*******************************竖直加速度控制器结束******************************************************************/ 
     Throttle=Int_Sort(Throttle_Hover//悬停油门
              +Total_Controler.High_Acce_Control.Control_OutPut);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/   
  }
  else if(Controler_Mode==3)//定高+定点模式
  {       
/*******************************高度控制器开始****************************************/ 
/****************定高：高度位置环+速度环+加速度环，控制周期分别为8ms、4ms、4ms*******************/     
      if(RC_NewData[0]>=Deadzone_Min&&RC_NewData[0]<=Deadzone_Max)
     {
          static int16_t High_Hold_Cnt=0;//定高控制周期计数器
          //高度位置环输出给定速度期望
          if(Total_Controler.High_Position_Control.Expect==0)//油门回中后，只设置一次
          {
            Total_Controler.High_Position_Control.Expect=NamelessQuad.Position[_YAW];//油门回中后，更新高度期望
          }
          High_Hold_Cnt++;
          if(High_Hold_Cnt>=2)//竖直高度控制周期2*4=8ms
          {
              High_Hold_Cnt=0;
              Total_Controler.High_Position_Control.FeedBack=NamelessQuad.Position[_YAW];//反馈 
              PID_Control_High(&Total_Controler.High_Position_Control);//海拔高度位置控制器
              //内环速度期望
              Total_Controler.High_Speed_Control.Expect=Total_Controler.High_Position_Control.Control_OutPut;
          }
     }
     else if(RC_NewData[0]>Deadzone_Max)//给定上升速度期望
     {
            //油门杆上推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Up_Speed_Max*(RC_NewData[0]-Deadzone_Max)/(Thr_Top-Deadzone_Max);//最大上升速度50cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     else if(RC_NewData[0]<Deadzone_Min)//给定下降速度期望
     {
            //油门杆下推、给定速度期望
           Total_Controler.High_Speed_Control.Expect=Climb_Down_Speed_Max*(RC_NewData[0]-Deadzone_Min)/(Deadzone_Min-Thr_Buttom);//最大下降速度40cm/s
           Total_Controler.High_Position_Control.Expect=0;//位置环期望置0
     }
     
/*高度控制器第2步*/ 
/********
        *
        *
   ****
 *
 *
 ********/  
     
/*******************************竖直速度控制器开始*******************************************************************/     
     Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
     PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制
/*******************************竖直速度控制器结束******************************************************************/ 
     
/*高度控制器第3步*/ 
/********
       **
     **
   **
     **
       **
 ********/      
     
/*******************************竖直加速度控制器开始******************************************************************/
      Total_Controler.High_Acce_Control.Expect=Total_Controler.High_Speed_Control.Control_OutPut;//加速度期望
      Total_Controler.High_Acce_Control.FeedBack=FilterAfter_NamelessQuad.Acceleration[_YAW];//加速度反馈
      PID_Control_High(&Total_Controler.High_Acce_Control);//海拔高度加速度控制
/*******************************竖直加速度控制器结束******************************************************************/ 
     Throttle=Int_Sort(Throttle_Hover//悬停油门
              +Total_Controler.High_Acce_Control.Control_OutPut);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/   

     
/*******************************水平位置控制器开始***********************************************************/   
  if(GPS_Sate_Num>=6//定位卫星超过6个
       &&GPS_Quality<=5//水平精度因子大于6不可用
         )
   {
     if(Roll_Control==0&&Pitch_Control==0)//无水平遥控量给定 
    {
    //位置期望,经纬、航行速度、高度          
    if(Total_Controler.Latitude_Position_Control.Expect==0&&Total_Controler.Longitude_Position_Control.Expect==0)//方向杆回中后，只设置一次
    {
      Total_Controler.Latitude_Position_Control.Expect=NamelessQuad.Position[_ROLL];
      Total_Controler.Longitude_Position_Control.Expect=NamelessQuad.Position[_PITCH];
    }
   
    //位置反馈，来源于当前惯导的位置估计
      Total_Controler.Latitude_Position_Control.FeedBack=NamelessQuad.Position[_ROLL];
      Total_Controler.Longitude_Position_Control.FeedBack=NamelessQuad.Position[_PITCH];


      PID_Control(&Total_Controler.Latitude_Position_Control);//水平位置控制 
      PID_Control(&Total_Controler.Longitude_Position_Control);

      Total_Controler.Latitude_Speed_Control.Expect =Total_Controler.Latitude_Position_Control.Control_OutPut;// 南北方向、俯仰、纬度 
      Total_Controler.Longitude_Speed_Control.Expect=Total_Controler.Longitude_Position_Control.Control_OutPut;//东西方向、恒滚、经度
      
      
      //Total_Controler.Latitude_Speed_Control.Expect =0;// 南北方向、俯仰、纬度 
      //Total_Controler.Longitude_Speed_Control.Expect=0;//东西方向、恒滚、经度
    
        
         //水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=NamelessQuad.Speed[_ROLL];
      Total_Controler.Longitude_Speed_Control.FeedBack=NamelessQuad.Speed[_PITCH];
   
      PID_Control(&Total_Controler.Latitude_Speed_Control); 
      PID_Control(&Total_Controler.Longitude_Speed_Control);
      /***************水平经纬度方向上的角度期望，机头指向正北的时候*******
      ***********旋转到对应航向的Pitch,Roll方向上的期望角度****************
      V_Roll*Cos_Yaw+V_Pitch*Sin_Yaw=V_Latitude;
      -V_Roll*Sin_Yaw+V_Pitch*Cos_Yaw=V_Longitude; 
      ==============》
      V_Roll =V_Latitude*Cos_Yaw-V_Longitude*Sin_Yaw;
      V_Pitch=V_Latitude*Sin_Yaw+V_Longitude*Cos_Yaw;   
      也可以通过旋转矩阵来推导
       Z轴旋转矩阵
       R（θ）
      {cosΨ      sinΨ      0}
      {-sinΨ     cosΨ      0}
     { 0          0          1}   
     转置即可
**********************************************************************************************/ 
   Position_Hold_Pitch=Total_Controler.Latitude_Speed_Control.Control_OutPut*Cos_Yaw
                        -Total_Controler.Longitude_Speed_Control.Control_OutPut*Sin_Yaw;
   
   Position_Hold_Roll=Total_Controler.Latitude_Speed_Control.Control_OutPut*Sin_Yaw
                        +Total_Controler.Longitude_Speed_Control.Control_OutPut*Cos_Yaw;
/**********************************************************************************************/     
   //定点模式，水平角度期望来源于水平速度环控制器输出
      Total_Controler.Pitch_Angle_Control.Expect=-Position_Hold_Pitch;
      Total_Controler.Roll_Angle_Control.Expect=Position_Hold_Roll;
   
 /*******************************水平位置控制器结束***********************************************************/
    }
    else //只进行水平速度控制，无水平位置控制
   {
      //分两种情况，1、导航坐标系的航向速度控制；
     //            2、载体坐标系方向上的速度控制
     if(Yaw_Control_Mode==Guide_Direction_Mode)//推动方向杆，对应导航系正东、正北的期望速度
     {
      Total_Controler.Latitude_Speed_Control.Expect =-Pitch_Control*4.0;// 最大期望速度50*3/100=1.5m/s
      Total_Controler.Longitude_Speed_Control.Expect=Roll_Control*4.0;
      
         //沿正东、正北水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=NamelessQuad.Speed[_ROLL];
      Total_Controler.Longitude_Speed_Control.FeedBack=NamelessQuad.Speed[_PITCH];
   
      PID_Control(&Total_Controler.Latitude_Speed_Control); 
      PID_Control(&Total_Controler.Longitude_Speed_Control);
 
      Position_Hold_Pitch=Total_Controler.Latitude_Speed_Control.Control_OutPut*Cos_Yaw
                        -Total_Controler.Longitude_Speed_Control.Control_OutPut*Sin_Yaw;
   
      Position_Hold_Roll =Total_Controler.Latitude_Speed_Control.Control_OutPut*Sin_Yaw
                        +Total_Controler.Longitude_Speed_Control.Control_OutPut*Cos_Yaw;         
    //水平角度期望来源于水平速度环控制器输出
      Total_Controler.Pitch_Angle_Control.Expect=-Position_Hold_Pitch;
      Total_Controler.Roll_Angle_Control.Expect=Position_Hold_Roll;    
     }
     else//推动方向杆，对应给定载体坐标系的沿Pitch,Roll方向运动速度
     {
      Total_Controler.Latitude_Speed_Control.Expect =-Pitch_Control*4.0;// 最大期望速度50*3/100=1.5m/s
      Total_Controler.Longitude_Speed_Control.Expect=Roll_Control*4.0;  
       //导航系的水平速度，转化到机体坐标系X-Y方向上
      Speed_Hold_Pitch=NamelessQuad.Speed[_ROLL]*Cos_Yaw
                        -NamelessQuad.Speed[_PITCH]*Sin_Yaw;
      Speed_Hold_Roll =NamelessQuad.Speed[_ROLL]*Sin_Yaw
                        +NamelessQuad.Speed[_PITCH]*Cos_Yaw;
       //沿载体Pitch、Roll方向水平速度控制
      Total_Controler.Latitude_Speed_Control.FeedBack=Speed_Hold_Pitch;
      Total_Controler.Longitude_Speed_Control.FeedBack=Speed_Hold_Roll;
      
      PID_Control(&Total_Controler.Latitude_Speed_Control);
      PID_Control(&Total_Controler.Longitude_Speed_Control);
     
      Total_Controler.Pitch_Angle_Control.Expect=-Total_Controler.Latitude_Speed_Control.Control_OutPut;
      Total_Controler.Roll_Angle_Control.Expect=Total_Controler.Longitude_Speed_Control.Control_OutPut;       
     }
     
      Total_Controler.Latitude_Position_Control.Expect=0;
      Total_Controler.Longitude_Position_Control.Expect=0;
   }
   }
  else//不满足定点条件，控制量给水平姿态
  {
/********对GPS定点模式位置0，直接进入姿态模式，等待GPS信号再次满足条件时，***********
*********自动切换至GPS定点模式，结合Controler_Mode_Select函数理解运行过程**********/
     Pos_Hold_SetFlag=0;//置0，当开关档位仍为定点模式时，
                        //在控制模式里面自检是否允许再次进入GPS定点模式
     Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
     Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];
  }
  }
  
}
其他的先不管，只追自稳的代码 

if(Controler_Mode==1)//姿态自稳定模式
  {
     Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
     Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];

     if(Throttle_Control<=1000)   Throttle=1000;
     else Throttle=Throttle_Control;//油门直接来源于遥控器油门给定
  }
首先Total_Controler是一个AllControler 的结构体，结构体的定义如下： 

typedef struct
{
 PID_Controler Pitch_Angle_Control;
 PID_Controler Pitch_Gyro_Control;
 PID_Controler Roll_Angle_Control;
 PID_Controler Roll_Gyro_Control;
 PID_Controler Yaw_Angle_Control;
 PID_Controler Yaw_Gyro_Control;
 PID_Controler High_Position_Control;
 PID_Controler High_Speed_Control;
 PID_Controler Longitude_Position_Control;
 PID_Controler Longitude_Speed_Control;
 PID_Controler Latitude_Position_Control;
 PID_Controler Latitude_Speed_Control;
 /*************加速度控制器，新加****************/
 PID_Controler High_Acce_Control;
 PID_Controler Longitude_Acce_Control;
 PID_Controler Latitude_Acce_Control;
}AllControler;

其中PID_Controler也是一个结构体

typedef struct
{
uint8 Err_Limit_Flag :1;//偏差限幅标志
uint8 Integrate_Limit_Flag :1;//积分限幅标志
uint8 Integrate_Separation_Flag :1;//积分分离标志
float Expect;//期望
float FeedBack;//反馈值
float Err;//偏差
float Last_Err;//上次偏差
float Err_Max;//偏差限幅值
float Integrate_Separation_Err;//积分分离偏差值
float Integrate;//积分值
float Integrate_Max;//积分限幅值
float Kp;//控制参数Kp
float Ki;//控制参数Ki
float Kd;//控制参数Kd
float Control_OutPut;//控制器总输出
float Last_Control_OutPut;//上次控制器总输出
float Control_OutPut_Limit;//输出限幅
/***************************************/
float Last_FeedBack;//上次反馈值
float Dis_Err;//微分量
float Dis_Error_History[5];//历史微分量
Butter_BufferData Control_Device_LPF_Buffer;//控制器低通输入输出缓冲
}PID_Controler;

Target_Angle是目标姿态，是一个数组 

int16 Target_Angle[2]={0};
来自于遥控器输入 
 if(PPM_Databuf[0]<=1480)  Roll_Control=(1450-PPM_Databuf[0])*50/400;
 else if(PPM_Databuf[0]>=1520)  Roll_Control=(1550-PPM_Databuf[0])*50/400;
 else Roll_Control=0;
 
 
 if(PPM_Databuf[1]<=1450)  Pitch_Control=(1450-PPM_Databuf[1])*50/400;
 else if(PPM_Databuf[1]>=1550)  Pitch_Control=(1550-PPM_Databuf[1])*50/400;
 else Pitch_Control=0;
 
 Target_Angle[0]=-Pitch_Control;
 Target_Angle[1]=-Roll_Control;
然后进入 Altitude_Control 
void Altitude_Control(void)
{
  Angle_Control();//角度控制
  Gyro_Control();//角速度控制  
}

首先是  Angle_Control();//角度控制 

void Angle_Control()//角度环节
{
  //角度反馈
 Total_Controler.Pitch_Angle_Control.FeedBack=Pitch;
 PID_Control(&Total_Controler.Pitch_Angle_Control); 
 Total_Controler.Roll_Angle_Control.FeedBack =Roll;
 PID_Control(&Total_Controler.Roll_Angle_Control);
 
 
 if(Yaw_Control==0)//偏航杆置于中位
 {
      if(Yaw_Cnt<=500)//无头模式、飞机上电后一段时间锁定偏航角，磁力计、陀螺仪融合需要一段时间，这里取500
      {
        Yaw_Cnt++;
      }
      if(Total_Controler.Yaw_Angle_Control.Expect==0||Yaw_Cnt<=500)
      {
        Total_Controler.Yaw_Angle_Control.Expect=Yaw;
      }
      Total_Controler.Yaw_Angle_Control.FeedBack=Yaw;//偏航角反馈
      
/*0deg  360deg附近特殊处理，使得最快转置期望角度1*/     
      if(Total_Controler.Yaw_Angle_Control.Expect>=0
         &&Total_Controler.Yaw_Angle_Control.Expect<=90//期望在第1象限
           &&Total_Controler.Yaw_Angle_Control.FeedBack<360
             &&Total_Controler.Yaw_Angle_Control.FeedBack>270)//期望在第4象限
      {
        Total_Controler.Yaw_Angle_Control.FeedBack=Yaw-360;//重置偏航角反馈
        PID_Integrate_Reset(&Total_Controler.Yaw_Gyro_Control);//清空内环控制
      }
/*0deg  360deg附近特殊处理，使得最快转置期望角度2*/      
      else if(Total_Controler.Yaw_Angle_Control.Expect>270
               &&Total_Controler.Yaw_Angle_Control.Expect<360//期望在第1象限
                &&Total_Controler.Yaw_Angle_Control.FeedBack<=90
                 &&Total_Controler.Yaw_Angle_Control.FeedBack>0)//期望在第4象限
      {
        Total_Controler.Yaw_Angle_Control.FeedBack=Yaw+360;//重置偏航角反馈
        PID_Integrate_Reset(&Total_Controler.Yaw_Gyro_Control);//清空内环控制
      }
      PID_Control(&Total_Controler.Yaw_Angle_Control);//偏航角度控制
      Total_Controler.Yaw_Gyro_Control.Expect=Total_Controler.Yaw_Angle_Control.Control_OutPut;//偏航角速度环期望，来源于偏航角度控制器输出
 }
 else//波动偏航方向杆后，只进行内环角速度控制
 {
    Total_Controler.Yaw_Angle_Control.Expect=0;//偏航角期望给0,不进行角度控制
    Total_Controler.Yaw_Gyro_Control.Expect=Yaw_Control;//偏航角速度环期望，直接来源于遥控器打杆量
 }
 
}
首先是获取当前姿态解算的欧拉角，以便后来得到误差：

  //角度反馈

 Total_Controler.Pitch_Angle_Control.FeedBack=Pitch;
 PID_Control(&Total_Controler.Pitch_Angle_Control); 
 Total_Controler.Roll_Angle_Control.FeedBack =Roll;
 PID_Control(&Total_Controler.Roll_Angle_Control);
对于PID_Control函数，得到对于俯仰角和偏航角的控制量 

float PID_Control(PID_Controler *Controler)
{
/*******偏差计算*********************/
  Controler->Last_Err=Controler->Err;//保存上次偏差
  Controler->Err=Controler->Expect-Controler->FeedBack;//期望减去反馈得到偏差
  if(Controler->Err_Limit_Flag==1)//偏差限幅度标志位
  {
  if(Controler->Err>=Controler->Err_Max)   Controler->Err= Controler->Err_Max;
  if(Controler->Err<=-Controler->Err_Max)  Controler->Err=-Controler->Err_Max;
  }
/*******积分计算*********************/
  if(Controler->Integrate_Separation_Flag==1)//积分分离标志位
  {
    if(ABS(Controler->Err)<=Controler->Integrate_Separation_Err)//ABS绝对值，#define ABS(X)  (((X)>0)?(X):-(X))
    Controler->Integrate+=Controler->Ki*Controler->Err;
  }
  else
  {
    Controler->Integrate+=Controler->Ki*Controler->Err;
  }
/*******积分限幅*********************/
 if(Controler->Integrate_Limit_Flag==1)//积分限制幅度标志
 {
  if(Controler->Integrate>=Controler->Integrate_Max)
    Controler->Integrate=Controler->Integrate_Max;
  if(Controler->Integrate<=-Controler->Integrate_Max)
    Controler->Integrate=-Controler->Integrate_Max ;
 }
/*******总输出计算*********************/
  Controler->Last_Control_OutPut=Controler->Control_OutPut;//输出值递推
  Controler->Control_OutPut=Controler->Kp*Controler->Err//比例
                         +Controler->Integrate//积分
                         +Controler->Kd*(Controler->Err-Controler->Last_Err);//微分
/*******总输出限幅*********************/
  if(Controler->Control_OutPut>=Controler->Control_OutPut_Limit)
  Controler->Control_OutPut=Controler->Control_OutPut_Limit;
  if(Controler->Control_OutPut<=-Controler->Control_OutPut_Limit)
  Controler->Control_OutPut=-Controler->Control_OutPut_Limit;
/*******返回总输出*********************/
  return Controler->Control_OutPut;
}
返回Total_Controler.Roll_Angle_Control.Control_OutPut（一 横滚角为例， 俯仰角同理）
剩下对偏航角处理

目标偏航角

 if(PPM_Databuf[3]<=1450)  Yaw_Control=-(PPM_Databuf[3]-1450)*150/400;
 else if(PPM_Databuf[3]>=1550)  Yaw_Control=-(PPM_Databuf[3]-1550)*150/400;
 else Yaw_Control=0;
 
 if(Yaw_Control>=150) Yaw_Control=150;
 else if(Yaw_Control<=-150) Yaw_Control=-150;
Yaw实际偏航角

若是不打杆

 if(Yaw_Control==0)//偏航杆置于中位
 {
      if(Yaw_Cnt<=500)//无头模式、飞机上电后一段时间锁定偏航角，磁力计、陀螺仪融合需要一段时间，这里取500
      {
        Yaw_Cnt++;
      }
      if(Total_Controler.Yaw_Angle_Control.Expect==0||Yaw_Cnt<=500)
      {
        Total_Controler.Yaw_Angle_Control.Expect=Yaw;
      }
      Total_Controler.Yaw_Angle_Control.FeedBack=Yaw;//偏航角反馈
      
/*0deg  360deg附近特殊处理，使得最快转置期望角度1*/ // Controler->Err=Controler->Expect-Controler->FeedBack;//期望减去反馈得到偏差
      {    
      if(Total_Controler.Yaw_Angle_Control.Expect>=0
         &&Total_Controler.Yaw_Angle_Control.Expect<=90//期望在第1象限
           &&Total_Controler.Yaw_Angle_Control.FeedBack<360
             &&Total_Controler.Yaw_Angle_Control.FeedBack>270)//期望在第4象限
      {
        Total_Controler.Yaw_Angle_Control.FeedBack=Yaw-360;//重置偏航角反馈
        PID_Integrate_Reset(&Total_Controler.Yaw_Gyro_Control);//清空内环控制
      }
/*0deg  360deg附近特殊处理，使得最快转置期望角度2*/      
      else if(Total_Controler.Yaw_Angle_Control.Expect>270
               &&Total_Controler.Yaw_Angle_Control.Expect<360//期望在第1象限
                &&Total_Controler.Yaw_Angle_Control.FeedBack<=90
                 &&Total_Controler.Yaw_Angle_Control.FeedBack>0)//期望在第4象限
      {
        Total_Controler.Yaw_Angle_Control.FeedBack=Yaw+360;//重置偏航角反馈
        PID_Integrate_Reset(&Total_Controler.Yaw_Gyro_Control);//清空内环控制
      }
      PID_Control(&Total_Controler.Yaw_Angle_Control);//偏航角度控制
      Total_Controler.Yaw_Gyro_Control.Expect=Total_Controler.Yaw_Angle_Control.Control_OutPut;//偏航角速度环期望，来源于偏航角度控制器输出
 }
 else//波动偏航方向杆后，只进行内环角速度控制
 {
    Total_Controler.Yaw_Angle_Control.Expect=0;//偏航角期望给0,不进行角度控制
    Total_Controler.Yaw_Gyro_Control.Expect=Yaw_Control;//偏航角速度环期望，直接来源于遥控器打杆量
 }

上面这段代码是根据偏航角不受遥控的情况分情况处理

若是没有遥控控的话，保持偏航角不变

若是期望偏航角在第一象限，实际偏航角度在第四象限

若是实际偏航角度在第一象限，期望偏航角在第四象限

分别怎么处理

PID_Integrate_Reset//清空内环控制

void  PID_Integrate_Reset(PID_Controler *Controler)  {Controler->Integrate=0.0f;}

积分置0

接下来对偏航角进行控制

PID_Control(&Total_Controler.Yaw_Angle_Control);//偏航角度控制
Total_Controler.Yaw_Gyro_Control.Expect=Total_Controler.Yaw_Angle_Control.Control_OutPut;//偏航角速度环期望，来源于偏航角度控制器输出
当对偏航控制的时候 
 Total_Controler.Yaw_Angle_Control.Expect=0;//偏航角期望给0,不进行角度控制
 Total_Controler.Yaw_Gyro_Control.Expect=Yaw_Control;//偏航角速度环期望，直接来源于遥控器打杆量
void Angle_Control()//角度环节结束进入void Gyro_Control()//角速度环 

void Gyro_Control()//角速度环
{
 /***************内环角速度期望****************/ 
 Total_Controler.Pitch_Gyro_Control.Expect=Total_Controler.Pitch_Angle_Control.Control_OutPut;  
 Total_Controler.Roll_Gyro_Control.Expect=Total_Controler.Roll_Angle_Control.Control_OutPut;
 /***************内环角速度反馈****************/ 
 Total_Controler.Pitch_Gyro_Control.FeedBack=Pitch_Gyro;  
 Total_Controler.Roll_Gyro_Control.FeedBack=Roll_Gyro;
 Total_Controler.Yaw_Gyro_Control.FeedBack=Yaw_Gyro;
 /***************内环角速度控制****************/ 
 PID_Control_LPF(&Total_Controler.Pitch_Gyro_Control);
 PID_Control_LPF(&Total_Controler.Roll_Gyro_Control);
 PID_Control_LPF(&Total_Controler.Yaw_Gyro_Control);
 //**************************偏航角前馈控制**********************************
 Total_Controler.Yaw_Gyro_Control.Control_OutPut=Total_Controler.Yaw_Gyro_Control.Control_OutPut
   +Yaw_Infront_Feadback*Total_Controler.Yaw_Gyro_Control.Expect;//偏航角前馈控制
 if(Total_Controler.Yaw_Gyro_Control.Control_OutPut>=Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit)
   Total_Controler.Yaw_Gyro_Control.Control_OutPut=Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit;
 if(Total_Controler.Yaw_Gyro_Control.Control_OutPut<=-Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit)
   Total_Controler.Yaw_Gyro_Control.Control_OutPut=-Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit;

}
首先是内环的角速度期望，来自于 void Angle_Control()的Controler->Control_OutPut 

/***************内环角速度期望****************/ 
 Total_Controler.Pitch_Gyro_Control.Expect=Total_Controler.Pitch_Angle_Control.Control_OutPut;  
 Total_Controler.Roll_Gyro_Control.Expect=Total_Controler.Roll_Angle_Control.Control_OutPut;
然后是现在的角速度： 

 Total_Controler.Pitch_Gyro_Control.FeedBack=Pitch_Gyro;  
 Total_Controler.Roll_Gyro_Control.FeedBack=Roll_Gyro;
 Total_Controler.Yaw_Gyro_Control.FeedBack=Yaw_Gyro;

其中：Pitch_Gyro;  Roll_Gyro;Yaw_Gyro来自于姿态解算AHRSUpdate_GraDes_Delayback函数

      Yaw_Gyro=gz;
	Pitch_Gyro=gx;
	Roll_Gyro=gy;

其中gz;gx;gy;对应Z_w_av,X_w_av,Y_w_av

对应于GET_MPU_DATA()中的

        X_w_av=GYRO_LPF(X_w,
                        &Gyro_BufferData[0],
                        &Gyro_Parameter   
                        );
        Y_w_av=GYRO_LPF(Y_w,
                        &Gyro_BufferData[1],
                        &Gyro_Parameter   
                        );
        Z_w_av=GYRO_LPF(Z_w,
                        &Gyro_BufferData[2],
                        &Gyro_Parameter   
                        );
角速度内环控制 

/***************内环角速度控制****************/ 
 PID_Control_LPF(&Total_Controler.Pitch_Gyro_Control);
 PID_Control_LPF(&Total_Controler.Roll_Gyro_Control);
 PID_Control_LPF(&Total_Controler.Yaw_Gyro_Control);
打开定义： 

Butter_Parameter Control_Device_LPF_Parameter={
 //500---20hz
  1,   -1.307285,   0.491812,
  0.046132,  0.092264,  0.046132
};
float PID_Control_LPF(PID_Controler *Controler)
{
  int16  i=0;
/*******偏差计算*********************/
  Controler->Last_Err=Controler->Err;//保存上次偏差
  Controler->Err=Controler->Expect-Controler->FeedBack;//期望减去反馈得到偏差
  Controler->Dis_Err=Controler->Err-Controler->Last_Err;//原始微分
  for(i=4;i>0;i--)//数字低通后微分项保存
  {
  Controler->Dis_Error_History[i]=Controler->Dis_Error_History[i-1];
  }
  Controler->Dis_Error_History[0]=Control_Device_LPF(Controler->Dis_Err,
                                  &Controler->Control_Device_LPF_Buffer,
                                  &Control_Device_LPF_Parameter);//巴特沃斯低通后得到的微分项,20hz

  if(Controler->Err_Limit_Flag==1)//偏差限幅度标志位
  {
  if(Controler->Err>=Controler->Err_Max)   Controler->Err= Controler->Err_Max;
  if(Controler->Err<=-Controler->Err_Max)  Controler->Err=-Controler->Err_Max;
  }
/*******积分计算*********************/
  if(Controler->Integrate_Separation_Flag==1)//积分分离标志位
  {
    if(ABS(Controler->Err)<=Controler->Integrate_Separation_Err)
    Controler->Integrate+=Controler->Ki*Controler->Err;
  }
  else
  {
    Controler->Integrate+=Controler->Ki*Controler->Err;
  }
/*******积分限幅*********************/
 if(Controler->Integrate_Limit_Flag==1)//积分限制幅度标志
 {
  if(Controler->Integrate>=Controler->Integrate_Max)
    Controler->Integrate=Controler->Integrate_Max;
  if(Controler->Integrate<=-Controler->Integrate_Max)
    Controler->Integrate=-Controler->Integrate_Max ;
 }
/*******总输出计算*********************/
  Controler->Last_Control_OutPut=Controler->Control_OutPut;//输出值递推
  Controler->Control_OutPut=Controler->Kp*Controler->Err//比例
                         +Controler->Integrate//积分
                         //+Controler->Kd*Controler->Dis_Err;//微分
                         +Controler->Kd*Controler->Dis_Error_History[0];//微分项来源于巴特沃斯低通滤波器
/*******总输出限幅*********************/
  if(Controler->Control_OutPut>=Controler->Control_OutPut_Limit)
  Controler->Control_OutPut=Controler->Control_OutPut_Limit;
  if(Controler->Control_OutPut<=-Controler->Control_OutPut_Limit)
  Controler->Control_OutPut=-Controler->Control_OutPut_Limit;
/*******返回总输出*********************/
  return Controler->Control_OutPut;
}
和外环角度处理类似，只是微分项加了 20hz的巴特沃斯低通后得到的微分项

输出

 return Controler->Control_OutPut;
回到void Gyro_Control()//角速度环函数的前馈处理 

Total_Controler.Yaw_Gyro_Control.Control_OutPut=Total_Controler.Yaw_Gyro_Control.Control_OutPut
   +Yaw_Infront_Feadback*Total_Controler.Yaw_Gyro_Control.Expect;//偏航角前馈控制
其中float Yaw_Infront_Feadback=0.35;//偏航角前馈控制;

限幅

if(Total_Controler.Yaw_Gyro_Control.Control_OutPut>=Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit)
   Total_Controler.Yaw_Gyro_Control.Control_OutPut=Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit;
 if(Total_Controler.Yaw_Gyro_Control.Control_OutPut<=-Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit)
   Total_Controler.Yaw_Gyro_Control.Control_OutPut=-Total_Controler.Yaw_Angle_Control.Control_OutPut_Limit;
结束void Gyro_Control()//角速度环

结束void Altitude_Control(void)

结束void Total_Control(void)

回到TIME.c 里的Control_Output();//控制量总输出

void Control_Output()
{
Throttle_Angle_Compensate();  
//Landon_Earth_Check();//着陆条件自检  
if(Controler_State==Unlock_Controler)//解锁
{
  if(Landon_Earth_Flag==1)//检测到着陆条件
  {
      Motor_PWM_1=Thr_Idle;//油门怠速//Thr_Idle 1100//油门怠速
      Motor_PWM_2=Thr_Idle;
      Motor_PWM_3=Thr_Idle;
      Motor_PWM_4=Thr_Idle;
  }
  else
  {
    
     if(Throttle>=Thr_Start)////Thr_Start  1100//起转油门量
     {
      Motor_PWM_1=Int_Sort(Throttle_Output
                        -Total_Controler.Roll_Gyro_Control.Control_OutPut
                        -Total_Controler.Pitch_Gyro_Control.Control_OutPut
                        +Total_Controler.Yaw_Gyro_Control.Control_OutPut);
      Motor_PWM_2=Int_Sort(Throttle_Output
                        +Total_Controler.Roll_Gyro_Control.Control_OutPut
                        +Total_Controler.Pitch_Gyro_Control.Control_OutPut
                        +Total_Controler.Yaw_Gyro_Control.Control_OutPut);
      Motor_PWM_3=Int_Sort(Throttle_Output
                        +Total_Controler.Roll_Gyro_Control.Control_OutPut
                        -Total_Controler.Pitch_Gyro_Control.Control_OutPut
                        -Total_Controler.Yaw_Gyro_Control.Control_OutPut);
      Motor_PWM_4=Int_Sort(Throttle_Output
                        -Total_Controler.Roll_Gyro_Control.Control_OutPut
                        +Total_Controler.Pitch_Gyro_Control.Control_OutPut
                        -Total_Controler.Yaw_Gyro_Control.Control_OutPut);
     }
     else
     {
      Motor_PWM_1=Int_Sort(Throttle_Output);
      Motor_PWM_2=Int_Sort(Throttle_Output);
      Motor_PWM_3=Int_Sort(Throttle_Output);
      Motor_PWM_4=Int_Sort(Throttle_Output);
      Take_Off_Reset();
     }
  }
}
else//未解锁，油门置于最低位
{
  Motor_PWM_1=Thr_Min;
  Motor_PWM_2=Thr_Min;
  Motor_PWM_3=Thr_Min;
  Motor_PWM_4=Thr_Min;
}
  

  Motor_PWM_1=Value_Limit(0,2000,Motor_PWM_1);//总输出限幅
  Motor_PWM_2=Value_Limit(0,2000,Motor_PWM_2);
  Motor_PWM_3=Value_Limit(0,2000,Motor_PWM_3);
  Motor_PWM_4=Value_Limit(0,2000,Motor_PWM_4);
  if(MotorTest==0x00)  
  {
    PWM_Set(1000,1000,1000,1000);
  }
  else
  {    
    PWM_Set((0x01&MotorTest) ? Motor_PWM_1:0,
            (0x02&MotorTest) ? Motor_PWM_2:0,
            (0x04&MotorTest) ? Motor_PWM_3:0,
            (0x08&MotorTest) ? Motor_PWM_4:0);
  }
}
首先对于Throttle_Angle_Compensate()定义 
uint16_t Throttle_Output=0;
void Throttle_Angle_Compensate()//油门倾角补偿
{
  float CosPitch_CosRoll=Cos_Pitch*Cos_Roll;
  float Throttle_Makeup=0;
  if(CosPitch_CosRoll>=0.999999)  CosPitch_CosRoll=0.999999;
  if(CosPitch_CosRoll<=0.000001)  CosPitch_CosRoll=0.000001;
  if(CosPitch_CosRoll<=0.75)  CosPitch_CosRoll=0.75;//Pitch,Roll约等于30度
  if(Throttle>=Thr_Start)//大于起转油门量
  {
      Throttle_Makeup=(Throttle-Thr_Start);//油门倾角补偿
      Throttle_Output=Thr_Start+(uint16_t)(Throttle_Makeup/CosPitch_CosRoll);  
  }
  else Throttle_Output=Throttle;
}
其中：#define  Thr_Start  1100//起转油门量，是常数；

Throttle_Control来源于遥控器输入或者定高，定点后赋值给Throttle，在处理后输出Throttle_Output

其中Take_Off_Reset将内外环的积分项全部置0；

void Take_Off_Reset(void)
{
  PID_Integrate_Reset(&Total_Controler.Roll_Gyro_Control);//起飞前屏蔽积分
  PID_Integrate_Reset(&Total_Controler.Pitch_Gyro_Control);
  PID_Integrate_Reset(&Total_Controler.Yaw_Gyro_Control);
  PID_Integrate_Reset(&Total_Controler.Pitch_Angle_Control);
  PID_Integrate_Reset(&Total_Controler.Roll_Angle_Control);
  PID_Integrate_Reset(&Total_Controler.Yaw_Angle_Control);
}
Value_Limit对输出限幅 

uint16 Value_Limit(uint16 Min,uint16 Max,uint16 Data)
{
 if(Data>=Max) Data=Max;
 else if(Data<=Min) Data=Min;
 return Data;
}
另外有一些变量定义： 

uint16 Motor_PWM_1,Motor_PWM_2,Motor_PWM_3,Motor_PWM_4;//四个电机输出PWM
uint8 MotorTest=0xff;//电机序号测试

后pwm输出（单片机这一块是软肋，不懂）

函数 Control_Output();//控制量总输出结束！

为什么要油门补偿？

PWM_Set干什么的？

Motor_PWM_i计算公式怎么来的？

内环为什么低通butterworth滤波？

剩下定高和定点控制另开贴再更贴！











JRlipton
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值