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

最新推荐文章于 2022-05-08 08:21:00 发布

原创

最新推荐文章于 2022-05-08 08:21:00 发布 · 3.6k 阅读

21 ·

CC 4.0 BY-SA版权

本文详细介绍了四旋翼飞行器控制系统的实现原理及关键算法。从PPM信号解析到飞行模式选择，再到姿态环控制的具体实现，包括角度控制、角速度控制等环节，并深入分析了油门补偿及PWM输出的原理。

接上篇继续码函数

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=T

最低0.47元/天解锁文章