无名飞控姿态解算和控制（三）
来源：互联网 发布：人工智能 教学大纲 编辑：程序博客网 时间：2024/05/16 09:56
继续码代码
上一篇主要写了自稳模式下的代码流程，这次主要是飞控的定高和定点控制流程
首先是定高
控制模式在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.5ffloat 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 //20msuint16 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();
剩下的就和自稳模式一样了
阅读全文
0 0
- 无名飞控姿态解算和控制（三）
- 无名飞控姿态解算和控制
- 无名飞控的姿态解算和控制（二）
- 无名飞控姿态结算和控制（一）
- crazyflie-firmware之姿态解算和PID控制
- Pixhawk姿态解算流程图&姿态控制流程图
- 磁力计和加速度计初始姿态解算
- 姿态和位置，四旋翼的控制流程
- 无名飞控
- 姿态解算(二)，姿态更新
- 四轴飞行器飞控研究（三）--姿态完整改进算法
- 关于姿态解算与融合的代码注释篇（三）
- 姿态解算知识（三）-陀螺仪加速度计6轴数据融合
- 四元数AHRS姿态解算和IMU姿态解算分析
- 四元数AHRS姿态解算和IMU姿态解算分析
- [转载]四轴飞行器1.4 姿态解算和Matlab实时姿态显示
- 四轴飞行器1.4 姿态解算和Matlab实时姿态显示
- 飞行姿态解算笔记（一）
- 《使用ArcGIS JavaScript API 开发Web 3D应用》学习笔记
- Wise Installation System使用纪要
- 写在年度总结前面
- dom4j
- Android获取本机蓝牙地址
- 无名飞控姿态解算和控制（三）
- 手把手教你搭建AI开发环境 !（附代码、下载地址）
- cpu之Instruction_memory
- dom4j的Element
- Android studio 3.0 创建虚拟机
- 物料分类账分析
- 队列的基本用法 舞者
- SQL之批量更新select出来的数据
- python 快速接口测试
