匿名飞控笔记（二）

四轴PID控制系统

1.外环PID

当前角度误差 = 期望角度 - 当前角度
外环PID_P项 = 外环Kp * 当前角度误差

当前角度误差积分及其积分限幅
外环PID_I项 = 外环Ki * 当前角度误差积分

外环PID输出 = 外环PID_P项 + 外环PID_I项

2.内环PID

当前角速度误差 = 外环PID输出 - 当前角速度（直接用陀螺仪输出）
内环PID_P项 = 内环Kp * 当前角速度误差

当前角速度误差积分及其积分限幅
内环PID_I项 = 内环Ki * 当前角速度误差积分

当前角速度微分（本次角速度误差 - 上次角速度误差）
内环PID_D项 = 内环Kd * 当前角速度的微分
内环PID输出 = 内环PID_P项 + 内环PID_I项 + 内环PID_D项

一般对串级 pid 的调节都是先整定内环再外环，但方便分析先从外环开始。

如上图1，外环就是角度环，通过期望角度和反馈角度得到角速度

外环：
Ano_Scheduler.c

static void Loop_Task_2(u32 dT_us)	//6ms执行一次
{
//	
	float t2_dT_s;
	t2_dT_s = (float)dT_us *1e-6f;
	//========================
	/*获取姿态角（ROLL PITCH YAW）*/
	calculate_RPY();
	
	/*姿态角度环控制*/
	Att_2level_Ctrl(6e-3f,CH_N);			//by WPR :外环角度PID

	//
	
//	
}

进入Att_2level_Ctrl（）函数
Att_2level_Ctrl（）第一个参数是 dT_s, //周期（单位：秒）
Ano_AttCtrl.c

/*角度环控制*/
void Att_2level_Ctrl(float dT_s,s16 *CH_N)
{
	
	//by WPR :期望角度来自定位环的输出，也就是 loc_ctrl_1.out[Y] loc_ctrl_1.out[X] 
	//by WPR :因为我们的期望角，实质上是我们希望无人机保持什么样的状态，比如倾斜一定角度以达目的，或者不倾斜到达稳定的目的。（如果要自稳要积分先行，因为有偏差的话肯定稳不了）
	
	
	/*积分微调*/
	exp_rol_tmp = - loc_ctrl_1.out[Y];
	exp_pit_tmp = - loc_ctrl_1.out[X];
	
	if(flag.flight_mode == ATT_STAB)
	{
		if(ABS(exp_rol_tmp + att_2l_ct.exp_rol_adj) < 5)
		{
			att_2l_ct.exp_rol_adj += 0.2f *exp_rol_tmp *dT_s;
			att_2l_ct.exp_rol_adj = LIMIT(att_2l_ct.exp_rol_adj,-1,1);
		}
		
		if(ABS(exp_pit_tmp + att_2l_ct.exp_pit_adj) < 5)
		{
			att_2l_ct.exp_pit_adj += 0.2f *exp_pit_tmp *dT_s;
			att_2l_ct.exp_pit_adj = LIMIT(att_2l_ct.exp_pit_adj,-1,1);
		}
	}
	else
	{
		att_2l_ct.exp_rol_adj = 
		att_2l_ct.exp_pit_adj = 0;
	}
	
	/*正负参考ANO坐标参考方向*/
	att_2l_ct.exp_rol = exp_rol_tmp + att_2l_ct.exp_rol_adj;// + POS_V_DAMPING *imu_data.h_acc[Y];
	att_2l_ct.exp_pit = exp_pit_tmp + att_2l_ct.exp_pit_adj;// + POS_V_DAMPING *imu_data.h_acc[X];
	
	/*期望角度限幅*/
	att_2l_ct.exp_rol = LIMIT(att_2l_ct.exp_rol,-MAX_ANGLE,MAX_ANGLE);
	att_2l_ct.exp_pit = LIMIT(att_2l_ct.exp_pit,-MAX_ANGLE,MAX_ANGLE);
	

	//
	if(flag.speed_mode == 3)
	{
		max_yaw_speed = MAX_SPEED_YAW;
	}
	else if(flag.speed_mode == 2 )
	{
		max_yaw_speed = 220;
	}
	else 
	{
		max_yaw_speed = 200;
	}
	//
	fc_stv.yaw_pal_limit = max_yaw_speed;
	/*摇杆量转换为YAW期望角速度 + 程控期望角速度*/
	set_yaw_av_tmp = (s32)(0.0023f *my_deadzone(CH_N[CH_YAW],0,65) *max_yaw_speed) + (-program_ctrl.yaw_pal_dps) + pc_user.pal_dps_set;

	/*最大YAW角速度限幅*/
	set_yaw_av_tmp = LIMIT(set_yaw_av_tmp ,-max_yaw_speed,max_yaw_speed);
	
	/*没有起飞，复位*/
	if(flag.taking_off==0)//if(flag.locking)
	{
		att_2l_ct.exp_rol = att_2l_ct.exp_pit = set_yaw_av_tmp = 0;
		att_2l_ct.exp_yaw = att_2l_ct.fb_yaw;
	}
	/*限制误差增大*/
	if(att_2l_ct.yaw_err>90)
	{
		if(set_yaw_av_tmp>0)
		{
			set_yaw_av_tmp = 0;
		}
	}
	else if(att_2l_ct.yaw_err<-90)
	{
		if(set_yaw_av_tmp<0)
		{
			set_yaw_av_tmp = 0;
		}
	}	

	//增量限幅
	att_1l_ct.set_yaw_speed += LIMIT((set_yaw_av_tmp - att_1l_ct.set_yaw_speed),-30,30);
	/*设置期望YAW角度*/
	att_2l_ct.exp_yaw += att_1l_ct.set_yaw_speed *dT_s;
	/*限制为+-180度*/
	if(att_2l_ct.exp_yaw<-180) att_2l_ct.exp_yaw += 360;
	else if(att_2l_ct.exp_yaw>180) att_2l_ct.exp_yaw -= 360;	
	
	/*计算YAW角度误差*/
	att_2l_ct.yaw_err = (att_2l_ct.exp_yaw - att_2l_ct.fb_yaw);
	/*限制为+-180度*/
	if(att_2l_ct.yaw_err<-180) att_2l_ct.yaw_err += 360;
	else if(att_2l_ct.yaw_err>180) att_2l_ct.yaw_err -= 360;
	


		/*赋值反馈角度值*/
		att_2l_ct.fb_yaw = imu_data.yaw ;
			
		att_2l_ct.fb_rol = (imu_data.rol ) ;
		att_2l_ct.fb_pit = (imu_data.pit ) ;
			


//by WPR :进入PID计算，得到三个角的期望值（fb_yaw、fb_rol、fb_pit）

	PID_calculate( dT_s,            //周期（单位：秒）
										0 ,				//前馈值
										att_2l_ct.exp_rol ,				//期望值（设定值）
										att_2l_ct.fb_rol ,			//反馈值（）
										&arg_2[ROL], //PID参数结构体
										&val_2[ROL],	//PID数据结构体
	                  5,//积分误差限幅
										5 *flag.taking_off			//integration limit，积分限幅
										 )	;
										
	PID_calculate( dT_s,            //周期（单位：秒）
										0 ,				//前馈值
										att_2l_ct.exp_pit ,				//期望值（设定值）
										att_2l_ct.fb_pit ,			//反馈值（）
										&arg_2[PIT], //PID参数结构体
										&val_2[PIT],	//PID数据结构体
	                  5,//积分误差限幅
										5 *flag.taking_off		//integration limit，积分限幅
										 )	;
	
	PID_calculate( dT_s,            //周期（单位：秒）
										0 ,				//前馈值
										att_2l_ct.yaw_err ,				//期望值（设定值）
										0 ,			//反馈值（）
										&arg_2[YAW], //PID参数结构体
										&val_2[YAW],	//PID数据结构体
	                  5,//积分误差限幅
										5 *flag.taking_off			//integration limit，积分限幅
										 )	;

}

内环：
Ano_Scheduler.c

static void Loop_Task_1(u32 dT_us)	//2ms执行一次
{
//	
	float t1_dT_s;
	t1_dT_s = (float)dT_us *1e-6f;
	//========================
	/*姿态角速度环控制*/
	Att_1level_Ctrl(2*1e-3f);		//by WPR :内环角速度PID
	
	/*电机输出控制*/
	Motor_Ctrl_Task(2);	

//	
}

进入Att_1level_Ctrl（）函数
Ano_AttCtrl.c

/*角速度环控制*/
void Att_1level_Ctrl(float dT_s)
{
	改变控制参数任务（最小控制周期内）
	ctrl_parameter_change_task();
	

		/*目标角速度赋值*/
		 for(u8 i = 0;i<3;i++)
		{
			att_1l_ct.exp_angular_velocity[i] = val_2[i].out;// val_2[i].out;//
		}
	
		/*目标角速度限幅*/
		att_1l_ct.exp_angular_velocity[ROL] = LIMIT(att_1l_ct.exp_angular_velocity[ROL],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);
		att_1l_ct.exp_angular_velocity[PIT] = LIMIT(att_1l_ct.exp_angular_velocity[PIT],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);


	/*反馈角速度赋值*/
	att_1l_ct.fb_angular_velocity[ROL] = ( sensor.Gyro_deg[X] );
	att_1l_ct.fb_angular_velocity[PIT] = (-sensor.Gyro_deg[Y] );
	att_1l_ct.fb_angular_velocity[YAW] = (-sensor.Gyro_deg[Z] );
	

	/*PID计算*/									 
 for(u8 i = 0;i<3;i++)
 {
		PID_calculate( dT_s,            //周期（单位：秒）
										0,				//前馈值
										att_1l_ct.exp_angular_velocity[i],				//期望值（设定值）
										att_1l_ct.fb_angular_velocity[i],			//反馈值（）
										&arg_1[i], //PID参数结构体
										&val_1[i],	//PID数据结构体
                    200,//积分误差限幅
										CTRL_1_INTE_LIM *flag.taking_off			//integration limit，积分幅度限幅
										 )	; 
 
	
	 ct_val[i] = (val_1[i].out);
 }
										 

	/*赋值，最终比例调节*/
 //by WPR :通过PID直接算出三个输出，然后乘上电机输出比例系数，得到要输出的油门量
	mc.ct_val_rol =                   FINAL_P *ct_val[ROL];
	mc.ct_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];
	mc.ct_val_yaw =                   FINAL_P *ct_val[YAW];
	/*输出量限幅*/
	mc.ct_val_rol = LIMIT(mc.ct_val_rol,-1000,1000);
	mc.ct_val_pit = LIMIT(mc.ct_val_pit,-1000,1000);
	mc.ct_val_yaw = LIMIT(mc.ct_val_yaw,-400,400);	
}

注意：最后的结构体mc是motor control的意思，在Ano_AttCtrl.h中存在专门管理油门的结构体

//by WPR : 专门管油门的结构体
typedef struct
{
	s32 ct_val_rol;
	s32 ct_val_pit;
	s32 ct_val_yaw; 
	s32 ct_val_thr;
} _mc_st;
extern _mc_st mc;//by WPR :mc--motor control

想要控制飞机，重点在于最开始，也就是外环的期望角度，它决定了飞机的运动状态，由定位环输出，另一方面，只要有了这个期望速度，通过两个角度环，加上观测量，最终就可以得到电机的输出。也就是说，我们要实现控制飞机的飞行状态，就要从定位环入手。

Ano_Scheduler.c
在Loop_Task_8（）中的
/位置速度环控制/
Loc_1level_Ctrl(20,CH_N);

这里只看光流部分

//仅有光流
	else if(switchs.of_flow_on && (!switchs.gps_on))
	{
		mode_f[1] = 1;
		if(mode_f[1] != mode_f[0])
		{
			Loc_1level_PID_Init();
			mode_f[0] = mode_f[1];
		}
		
		loc_ctrl_1.exp[X] = fs.speed_set_h[X];
		loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];
		//
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[X],vel_fb_d_lpf[X]);
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[Y],vel_fb_d_lpf[Y]);		
		
		if(sens_hd_check.of_ok)
		{
			loc_ctrl_1.fb[X] = OF_DX2 + 0.03f *vel_fb_d_lpf[X];
			loc_ctrl_1.fb[Y] = OF_DY2 + 0.03f *vel_fb_d_lpf[Y];
			
			fb_speed_fix[0] = OF_DX2FIX;
			fb_speed_fix[1] = OF_DY2FIX;
		}
		else//sens_hd_check.of_df_ok
		{
			loc_ctrl_1.fb[X] = of_rdf.gnd_vel_est_h[X] + 0.03f *vel_fb_d_lpf[X];
			loc_ctrl_1.fb[Y] = of_rdf.gnd_vel_est_h[Y] + 0.03f *vel_fb_d_lpf[Y];
			
			fb_speed_fix[0] = of_rdf.gnd_vel_est_h[X];
			fb_speed_fix[1] = of_rdf.gnd_vel_est_h[Y];		
		}
		
		for(u8 i =0;i<2;i++)
		{
			PID_calculate( dT_ms*1e-3f,            //周期（单位：秒）
										loc_ctrl_1.exp[i] ,				//前馈值
										loc_ctrl_1.exp[i] ,				//期望值（设定值）
										loc_ctrl_1.fb[i] ,			//反馈值（）
										&loc_arg_1[i], //PID参数结构体
										&loc_val_1[i],	//PID数据结构体
										50,//积分误差限幅
										10 *flag.taking_off			//integration limit，积分限幅
										 )	;	
			
			//fix
			PID_calculate( dT_ms*1e-3f,            //周期（单位：秒）
										loc_ctrl_1.exp[i] ,				//前馈值
										loc_ctrl_1.exp[i] ,				//期望值（设定值）
										fb_speed_fix[i] ,			//反馈值（）
										&loc_arg_1_fix[i], //PID参数结构体
										&loc_val_1_fix[i],	//PID数据结构体
										50,//积分误差限幅
										10 *flag.taking_off			//integration limit，积分限幅
										 )	;	
			
			loc_ctrl_1.out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;	//(PD)+(I)	
		}