匿名飞控笔记（二）

笔记（二）"/>

匿名飞控笔记（二）

四轴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，积分限幅)	;	//fixPID_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)	}