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

2023-05-16

继续码代码

上一篇主要写了自稳模式下的代码流程，这次主要是飞控的定高和定点控制流程

首先是定高

控制模式在Main_Leading_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);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/   
  }

首先根据Controler_Mode_Select里的Controler_Mode=2来选择定高

Total_Controler.Pitch_Angle_Control.Expect=Target_Angle[0];
Total_Controler.Roll_Angle_Control.Expect=Target_Angle[1];

遥控器输入作为俯仰角和横滚角的期望值

高度控制器第1步

判断遥控输入是否在死区内

if(RC_NewData[0]>=Deadzone_Min&&RC_NewData[0]<=Deadzone_Max)

RC_NewData[0]=Throttle_Control;//遥感油门原始行程量

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++;///++后此时为1，

以这个量来限制定高控制的周期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;
          }

进入定高控制，High_Hold_Cnt置0；

Total_Controler.High_Position_Control.FeedBack=NamelessQuad.Position[_YAW];//反馈得到现在的高度；

 PID_Control_High(&Total_Controler.High_Position_Control);//海拔高度位置控制器
              //内环速度期望

得到期望速度

看一下PID_Control_High定义

float PID_Control_High(PID_Controler *Controler)
{
/*******偏差计算*********************/
  Controler->Last_Err=Controler->Err;//保存上次偏差
  Controler->Err=Controler->Expect-Controler->FeedBack;//期望减去反馈得到偏差
  //Controler->Err=LPButter_Vel_Error(Controler->Err);
  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->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;
}

对比一下 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.High_Position_Control.Control_OutPut，将其赋给Total_Controler.High_Speed_Control.Expect即期望速度；

如果油门杆不在死区内部，那么有两种情况爬升和下降

 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
     }

对于这两种情况根据油门输入赋值给期望速度

期望高度值置0；

/*高度控制器第2步*/

  Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
  PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制

其中：NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈

这里的NamelessQuad.Speed是来自于姿态解算的高度结算

这里大致记一下：

高度结算大致如下函数：

float pos_correction[3]={0,0,0};
float acc_correction[3]={0,0,0};
float vel_correction[3]={0,0,0};
/****气压计三阶互补滤波方案——参考开源飞控APM****/
//#define TIME_CONTANST_ZER       1.5f
float TIME_CONTANST_ZER=3.0f;
#define K_ACC_ZER 	        (1.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER * TIME_CONTANST_ZER))
#define K_VEL_ZER	        (3.0f / (TIME_CONTANST_ZER * TIME_CONTANST_ZER))//20															// XY????·′à??μêy,3.0
#define K_POS_ZER               (3.0f / TIME_CONTANST_ZER)
//#define High_Delay_Cnt  5 //20ms
uint16 High_Delay_Cnt=1;
float Altitude_Dealt=0;
float Altitude_Estimate=0;
void Strapdown_INS_High()
{
      uint16 Cnt=0;
      static uint16_t Save_Cnt=0;
      Save_Cnt++;//数据存储周期
      #ifdef  IMU_BOARD_GY86
          Altitude_Estimate=AirPresure_Altitude;//高度观测量
      #elsedef IMU_BOARD_NC686
          Altitude_Estimate=SPL06_001_Filter_high;
      #elsedef IMU_BOARD_NC683
          Altitude_Estimate=FBM320_Filter_high;
      #endif
      //由观测量（气压计）得到状态误差
      Altitude_Dealt=Altitude_Estimate-NamelessQuad.Pos_History[_YAW][High_Delay_Cnt];//气压计(超声波)与SINS估计量的差，单位cm
      //三路积分反馈量修正惯导
      acc_correction[_YAW] +=Altitude_Dealt* K_ACC_ZER*CNTLCYCLE ;//加速度矫正量
      vel_correction[_YAW] +=Altitude_Dealt* K_VEL_ZER*CNTLCYCLE ;//速度矫正量
      pos_correction[_YAW] +=Altitude_Dealt* K_POS_ZER*CNTLCYCLE ;//位置矫正量
      //加速度计矫正后更新
      NamelessQuad.Acceleration[_YAW]=Origion_NamelessQuad.Acceleration[_YAW]+acc_correction[_YAW];
      //速度增量矫正后更新，用于更新位置
      SpeedDealt[_YAW]=NamelessQuad.Acceleration[_YAW]*CNTLCYCLE;
      //原始位置更新
      Origion_NamelessQuad.Position[_YAW]+=(NamelessQuad.Speed[_YAW]+0.5*SpeedDealt[_YAW])*CNTLCYCLE;
      //位置矫正后更新
      NamelessQuad.Position[_YAW]=Origion_NamelessQuad.Position[_YAW]+pos_correction[_YAW];
      //原始速度更新
      Origion_NamelessQuad.Speed[_YAW]+=SpeedDealt[_YAW];
      //速度矫正后更新
      NamelessQuad.Speed[_YAW]=Origion_NamelessQuad.Speed[_YAW]+vel_correction[_YAW];

      if(Save_Cnt>=5)//20ms
      {
        for(Cnt=Num-1;Cnt>0;Cnt--)//20ms滑动一次
        {
        NamelessQuad.Pos_History[_YAW][Cnt]=NamelessQuad.Pos_History[_YAW][Cnt-1];
        }
        NamelessQuad.Pos_History[_YAW][0]=NamelessQuad.Position[_YAW];
        Save_Cnt=0;
      }

}

根据TIME.c里的代码，高度结算并没有用卡尔曼滤波

根据定义#define IMU_BOARD_NC686，则有 Altitude_Estimate=SPL06_001_Filter_high;

有函数

void SPL06_001_StateMachine(void)
{
  user_spl0601_get();
  if(SPL06_001_Cnt<=50)  SPL06_001_Cnt++;
  if(SPL06_001_Cnt==49)
  {
    SPL06_001_Offset_Okay=1;
    High_Okay_flag=1;
    SPL06_001_Offset=pressure;
  }
    
  if(SPL06_001_Offset_Okay==1)//初始气压零点设置完毕
  {
    SPL06_001_Filter_P=pressure;      
    SPL06_001_Filter_high=Get_Altitude_SPL06_001(SPL06_001_Filter_P);
  }
   
}

有气压计得到高度

float SPL06_001_Offset=0;
float Get_Altitude_SPL06_001(float baroPress)
{
    float Tempbaro=(float)(baroPress/SPL06_001_Offset)*1.0;
    True_Altitude = 4433000.0f * (1 - powf((float)(Tempbaro),0.190295f));   
    return True_Altitude;
}

回到高度控制：

 Total_Controler.High_Speed_Control.FeedBack=NamelessQuad.Speed[_YAW];//惯导速度估计给速度反馈
 PID_Control_High(&Total_Controler.High_Speed_Control);//海拔高度速度控制

将现在竖直速度和期望速度对比产生误差控制输出Total_Controler.High_Speed_Control.Control_OutPut

/*高度控制器第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);//海拔高度加速度控制

类似第二步，总共3阶pid控制

/*******************************竖直加速度控制器结束******************************************************************/ 
     Throttle=Int_Sort(Throttle_Hover//悬停油门
              +Total_Controler.High_Acce_Control.Control_OutPut);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/

输出油门

油门在死区里怎么处理，死区外怎么处理，根据高度差输出期望速度，速度差产生期望加速度，加速度差产生油门

定高+定点模式

第一步定高控制，

      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);//油门来源于高度控制器输出
/*****************************************高度控制器结束，给定油门控制量***********************************************************/

第二步GPS定点控制：

*******************************水平位置控制器开始***********************************************************/   
  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方向上的期望角度****************

**********************************************************************************************/ 
   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(Total_Controler.Latitude_Position_Control.Expect==0&&Total_Controler.Longitude_Position_Control.Expect==0)//方向杆回中后，只设置一次

判断期望位置控制是否置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;//东西方向、恒滚、经度

获取当前位置，做差经PID控制输出期望速度

      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;

水平角度期望来源于水平速度环控制器输出

定点模式，水平角度期望来源于水平速度环控制器输出？？？？？？？

if(Roll_Control==0&&Pitch_Control==0)//无水平遥控量给定结束

    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 //只进行水平速度控制，无水平位置控制
   {
      //分两种情况，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;
   }
   }

两种情况产生期望角度

为什么两种情况？？？？？？？

然后就是姿态控制

/*************姿态环控制器*****************/
  Altitude_Control();

剩下的就和自稳模式一样了

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

无名飞控姿态解算和控制

无名飞控姿态解算和控制（三）的相关文章

动态内存分配（malloc/free）简单实现--隐式空闲链表

本文使用隐式空闲链表实现简单的动态内存分配动态内存分配器维护一个大块区域 xff0c 也就是堆 xff0c 处理动态的内存分配请求分配器将堆视为一组不同大小的块的集合来维护 xff0c 每个块要么是已分配的 xff0c 要么是空闲的实
【谷歌面试题】求数组中两个元素的最小距离

一个数组 xff0c 含有重复元素 xff0c 给出两个数num1和num2 xff0c 求这两个数字在数组中出现的位置的最小距离 O n 时间复杂度 xff0c O 1 空间复杂度 int minDistance int A int si
进程间通信

原作者地址不详摘要随着人们对应用程序的要求越来越高 xff0c 单进程应用在许多场合已不能满足人们的要求编写多进程多线程程序成为现代程序设计的一个重要特点 xff0c 在多进程程序设计中 xff0c 进程间的通信是不可避免的 Mi
通过能观性分析理解SLAM系统的可观维度。

目录 1 能观性分析大体理解2 可观性定义3 可观性矩阵 1 能观性分析大体理解什么是能观性分析 xff1f 能观性分析通过计算可观性矩阵 xff0c 分析它的零空间的秩 xff0c 来分析系统哪些状态维度可观不可观可观性矩阵对应系统
百度2014移动研发笔试题目——1013清华版

一简答题 1 简述计算机的存储系统分为哪几个层次 xff0c 为什么这样的分层能够提高程序的执行效率 2 浮点数在计算中如何表示 xff0c 如何对浮点数判等 3 简述TCP与UDP协议的差别 xff0c 两者与HTTP的关系并列举HT
史上最全的ubuntu16.04安装nvidia驱动+cuda9.0+cuDnn7.0

本文参考了 http www cnblogs com 5211314jackrose p 7081020 html https jingyan baidu com article 4853e1e55679491909f726f4 html
4、ORB-SLAM闭环检测之通过词袋寻找当前关键帧和闭环候选帧之间的匹配

目录 1 SearchByBoW 2 图解每一步实现原理 2 1 通过词袋模型寻找匹配 2 2 通过旋转直方图来筛除离群点 3 期待您加入前面我们已经了解到了sim3的求解流程具体计算过程中有三步比较重要 1 寻找两关键帧之间的粗匹配
基于Web服务的物联网-WoT(Web of Things)

当我们谈到智能手机 xff0c 多样性往往不是问题 xff0c 主流市场不是基于苹果IOS系统的就是谷歌的Android系统 xff0c 但即将到来的物联网浪潮 xff0c 智能对象是否也能这样 xff1f 这就复杂多了 xff0c 当前
如何选择 catkin_make和catkin_make_isolated

问题在编译cartographer的代码包的时候 xff0c 会被建议使用catkin make isolated xff0c 为何不使用catkin make呢 xff1f 原因 catkin make was the first sc
基于共享内存的分布式消息学习笔记

作者 xff1a 深圳自研业务组 jimwu 编辑 xff1a 上海业务组 alenai 目录 xff1a Tbus 简介 Tbus 原理 Tbus 配置与工具 Tbus 简单应用 Tbus 运维应用为 python 扩展总结 Tbus
分享一下工作以来我看过计算机书籍

由于自工作依赖一直专注于linux 下的c c 43 43 编程工作 xff0c 所以 xff0c 我的书籍也大的都是这方这边书尽管很经典 xff0c 但是我的能力实在有限 xff0c 只把数据结构的那点看了一下 xff0c 其他的看的
51单片机定时器初值计算详解

前言 xff1a 本文详细介绍了51单片机学习过程中定时器的初值计算问题以及相关概念 xff0c 力求把每一个学习过程中的可能会遇到的难点说清楚 xff0c 并举相关的例子加以说明学习完毕 xff0c 又顺手利用刚学到定时器的相关知识写了
STM32平台下官方DMP库6.12超详细移植教程

前记 Motion Driver官方库 xff1a Motion Driver 6 12 STM32工程源码 xff1a STM32F103C8 软件MPU6050 xff08 DMP xff09 MPU6050软件I2C驱动 xff0c
STM32F103C8-平衡小车笔记

STM32F103C8 平衡小车笔记 1 PID的作用 xff08 1 xff09 比例项 xff1a 提高响应速度 xff0c 减小静差 xff08 2 xff09 积分项 xff1a 消除稳态误差 xff08 3 xff09 微分项 x
嵌入式Linux系统开发笔记（十四）

U Boot环境变量 uboot 中有两个非常重要的环境变量 bootcmd 和 bootargs xff0c bootcmd 和 bootagrs 是采用类似 shell 脚本语言编写的 xff0c 里面有很多的变量引用 xff0c 这些
嵌入式Linux系统开发笔记（十五）

Linux内核启动验证 5 1 编译内核 span class token comment 清除工程 span span class token comment make distclean span span class token co

随机推荐

基于ROS搭建机器人仿真环境

别人的发复现及经验 https blog csdn net qq 38620941 article details 125321347 gazebo默认仿真环境 1 gazebo models 是系统下gazebo放置模型库的默认位置 2
嵌入式Linux系统开发笔记（十六）

根文件系统rootfs启动验证测试接下来我们使用测试一下前面创建好的根文件系统 rootfs xff0c 测试方法使用 NFS 挂载 6 1 检查是否在Ubuntu主机中安装和开启了NFS服务 xff08 特别注意 xff1a nfs 配
安卓5.0以上7.0以下使用Termux

参考 xff1a https zhuanlan zhihu com p 400507701 说明 xff1a Termux支持5 0以上的安卓系统 Termux7 3版本之后 xff0c 仅支持7 0以上的安卓系统 1 安装Termux 设
关于DSP的CCS6.0平台下的工程搭建（完全可移植）

本工程以CCS6 0下新建TMS320F28335工程为例 xff0c 其他系列处理器工程搭建类似 xff0c 参考本例即可工程搭建用到的F2833x TI官方库文件下载链接也可直接参考笔者搭建好CCS6 0的工程下载链接所建工程
STM32Fxx JTAG/SWD复用功能重映射

问题描述 xff1a 在实验室调车过程中 xff0c 遇到的一个问题 xff1a 为了每次下载程序方便 xff0c 队员们往往会把 Jlink 插在板子上 xff0c 可是在调车过程中发现 xff0c 有时程序会莫名死掉 xff0c 而同样
VS2012编译RTKLIB——GNSS定位开源库

RTKLIB 开源库有着强大的 GPS 数据实时和后处理功能 xff0c 由于笔者的毕业设计中需要对GPS 载波相位观测量进行 RTK 解算 xff0c 故而 xff0c 对 RTKLIB 开源库进行了学习与研究 RTKLIB 提供了很
51单片机串行口波特率计算

1 工作方式介绍 xff1a 方式 0 xff1a 这种工作方式比较特殊 xff0c 与常见的微型计算机的串行口不同 xff0c 它又叫同步移位寄存器输出方式在这种方式下 xff0c 数据从 RXD 端串行输出或输入 xff0c 同步信
数码管显示问题总结

1 数码管显示原理我们最常用的是七段式和八段式 LED 数码管 xff0c 八段比七段多了一个小数点 xff0c 其他的基本相同所谓的八段就是指数码管里有八个小 LED 发光二极管 xff0c 通过控制不同的 LED 的亮灭来显示出不同
单片机与嵌入式linux 比较

MCU门槛低 xff0c 入门容易 xff0c 但是灵活 xff0c 其实对工程师的软硬件功底要求更高 xff0c 随着半导体的飞速发展 xff0c MCU能实现很多匪夷所思匪夷所思的功能 xff0c 比如 xff0c 使用GPIO模拟1个
rtk 精确定位简介

RTK又称载波相位差分 xff1a 基准站通过数据链及时将其载波观测量及站坐标信息一同传送给用户站用户站接收GPS卫星的载波相位与来自基准站的载波相位 xff0c 并组成相位差分观测值进行及时处理 xff0c 能及时给出厘米级的定位结果
STM32开发利器：STM32CubeMX

这篇博客篇幅不长 xff0c 主要是为大家介绍ST公司推出的STM32CubeMX开发工具 xff0c 当成下周更新STM32 10个项目工程的预备篇 xff0c 同时FPGA FPGA 20个例程篇 xff1a 8 SD卡任意地址的读写
ROS命名空间

ROS命令空间是一个很重要的内容 xff0c 官方文档 xff1a http wiki ros org Names 分为三类 xff1a relative xff0c global xff0c private 下边是一个官网给的示例 Nod
STM32CubeMX关于添加DSP库的使用

前言人生如逆旅 xff0c 我亦是行人一介绍 STM32 系列基于专为要求高性能低成本低功耗的嵌入式应用专门设计的 ARM Cortex M3 内核而 DSP 应该是 TMS320 系列 xff0c TMS320 系列 DSP
STM32H750VBT6的DSP使用的学习——基于CubeMX

前言人生如逆旅 xff0c 我亦是行人 1 STM32H7的DSP功能介绍 xff08 STMicroelectronics xff0c 简称ST xff09 推出新的运算性能创记录的H7系列微控制器新系列内置STM32平台中存储容量最
ROS中激光雷达数据类型传递转换及自定义点云数据类型介绍

目录一 ROS中激光雷达数据类型传递转换 xff1b 二点云数据解析三自定义点云数据类型一 ROS中激光雷达数据类型传递转换 xff1b ROS中涉及激光雷达传递的消息类型有两种 xff0c 一种是针对2D雷达 sensor ms
C/C++优秀书籍清单

转载自 xff1a https www cnblogs com kimiway p 3225767 html 书籍是码农进步的阶梯读好书好读书干一行爱一行除了工作还有生活在陪伴家人同时也不忘提高自己为更好的生活努力 1 C程序
打印_battery_status.scale

在px4的姿态控制中 xff0c publish控制量时代码乘以了一个 battery status scale xff0c scale effort by battery status if params bat scale en amp
无名飞控

无名飞控Time c文件由于无名飞控主要核心 xff1a 传感器滤波姿态解算惯导控制等代码在TIME c里面运行 xff0c 所以先来分析这个文件打开文件第一个函数 xff1a void Timer4 Configuration
无名飞控姿态解算和控制（一）

无名飞控的姿态解算和控制从imu和磁力计 xff0c 气压计拿到数据后 xff0c 进入AHRSUpdate GraDes Delayback函数 xff0c 其中X w av Y w av Z w av来自陀螺仪 xff0c X g a
无名飞控姿态解算和控制（三）

继续码代码上一篇主要写了自稳模式下的代码流程 xff0c 这次主要是飞控的定高和定点控制流程首先是定高控制模式在Main Leading Control里选择定高模式代码 xff1a else if Controler Mode 6

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

无名飞控姿态解算和控制（三） 的相关文章

随机推荐

热门标签

无名飞控姿态解算和控制（三）的相关文章