Esempio n. 1
1
float Height_Ctrl(float T,float thr,u8 ready,float en)
{
	static u8 step,speed_cnt,height_cnt;
	
	if(ready == 0)
	{
		ex_i_en = ex_i_en_f = 0;
		en = 0;
		thr_take_off = 0;
		thr_take_off_f = 0;
	}
	
	switch(step)
	{
		case 0:
		{

			//step = 1;
			break;
		}
		case 1:
		{

			
			step = 2;
			break;
		}
		case 2:
		{
		
			step = 3;
			break;
		}
		case 3:
		{
			
			step = 4;
			break;
		}	
		case 4:
		{
			
			step = 0;
			break;
		}	
		default:break;	
	
	}
	/*飞行中初次进入定高模式切换处理*/
	if(ABS(en - en_old) > 0.5f)//从非定高切换到定高
	{
		if(thr_take_off<10)//未计算起飞油门
		{
			if(thr_set > -150)
			{
				thr_take_off = 400;
				
			}
		}
		en_old = en;
	}
	
	/*定高控制*/
	//h_pid_init();
	
	thr_set = my_deathzoom_2(my_deathzoom((thr - 500),0,40),0,10);
	
	if(thr_set>0)
	{
		set_speed_t = thr_set/450 * MAX_VERTICAL_SPEED_UP;
		
		if(thr_set>100)
		{
			ex_i_en_f = 1;
			
			if(!thr_take_off_f)
			{
				thr_take_off_f = 1; //用户可能想要起飞
				thr_take_off = 350; //直接赋值 一次
				
			}
		}
	}
	else
	{
		if(ex_i_en_f == 1)
		{
			ex_i_en = 1;
		}
		set_speed_t = thr_set/450 * MAX_VERTICAL_SPEED_DW;
	}
	
	set_speed_t = LIMIT(set_speed_t,-MAX_VERTICAL_SPEED_DW,MAX_VERTICAL_SPEED_UP);
	
	//exp_speed =my_pow_2_curve(exp_speed_t,0.45f,MAX_VERTICAL_SPEED);
	LPF_1_(10.0f,T,my_pow_2_curve(set_speed_t,0.25f,MAX_VERTICAL_SPEED_DW),set_speed);
	
	set_speed = LIMIT(set_speed,-MAX_VERTICAL_SPEED_DW,MAX_VERTICAL_SPEED_UP);
	
/////////////////////////////////////////////////////////////////////////////////	
	baro_ctrl(T,&hc_value); //高度数据获取: 气压计数据
	
/////////////////////////////////////////////////////////////////////////////////		
	//计算高度误差(可加滤波)
	set_height_em += (set_speed - hc_value.m_speed) *T;
	set_height_em = LIMIT(set_height_em,-5000 *ex_i_en,5000 *ex_i_en);
	
	set_height_e += (set_speed - 1.05f *hc_value.fusion_speed) *T;
	set_height_e = LIMIT(set_height_e,-5000 *ex_i_en,5000 *ex_i_en);
	
	LPF_1_(0.05f,T,set_height_em,set_height_e);
	
	
/////////////////////////////////////////////////////////////////////////////////		
/////////////////////////////////////////////////////////////////////////////////
	if(en < 0.1f)
	{
		exp_speed = hc_value.fusion_speed;
		exp_acc = hc_value.fusion_acc;
	}
/////////////////////////////////////////////////////////////////////////////////	
	float acc_i_lim;
	acc_i_lim = safe_div(150,h_acc_arg.ki,0);
	
	fb_speed_old = fb_speed;
	fb_speed = hc_value.fusion_speed;
	fb_acc = safe_div(fb_speed - fb_speed_old,T,0);
	
	thr_pid_out = PID_calculate( T,            //周期
														exp_acc,				//前馈
														exp_acc,				//期望值(设定值)
														fb_acc,			//反馈值
														&h_acc_arg, //PID参数结构体
														&h_acc_val,	//PID数据结构体
														acc_i_lim*en			//integration limit,积分限幅
														 );			//输出		

	//step_filter(1000 *T,thr_pid_out,thr_pid_out_dlim);
	
	//起飞油门
	if(h_acc_val.err_i > (acc_i_lim * 0.2f))
	{
		if(thr_take_off<THR_TAKE_OFF_LIMIT)
		{
			thr_take_off += 150 *T;
			h_acc_val.err_i -= safe_div(150,h_acc_arg.ki,0) *T;
		}
	}
	else if(h_acc_val.err_i < (-acc_i_lim * 0.2f))
	{
		if(thr_take_off>0)
		{
			thr_take_off -= 150 *T;
			h_acc_val.err_i += safe_div(150,h_acc_arg.ki,0) *T;
		}
	}
	
	thr_take_off = LIMIT(thr_take_off,0,THR_TAKE_OFF_LIMIT); //一半
	
	
	//油门补偿
	tilted_fix = safe_div(1,LIMIT(reference_v.z,0.707f,1),0); //45度内补偿
	
	//油门输出
	thr_out = (thr_pid_out + tilted_fix *(thr_take_off) );
	
	thr_out = LIMIT(thr_out,0,1000);
	

	
/////////////////////////////////////////////////////////////////////////////////	
	static float dT,dT2;
	dT += T;
	speed_cnt++;
	if(speed_cnt>=10) //u8  20ms
	{

		exp_acc = PID_calculate( dT,            //周期
														exp_speed,				//前馈
														(set_speed + exp_speed),				//期望值(设定值)
														hc_value.fusion_speed,			//反馈值
														&h_speed_arg, //PID参数结构体
														&h_speed_val,	//PID数据结构体
														500 *en			//integration limit,积分限幅
														 );			//输出	
		
		exp_acc = LIMIT(exp_acc,-3000,3000);
		
		//integra_fix += (exp_speed - hc_value.m_speed) *dT;
		//integra_fix = LIMIT(integra_fix,-1500 *en,1500 *en);
		
		//LPF_1_(0.5f,dT,integra_fix,h_speed_val.err_i);
		
		dT2 += dT;
		height_cnt++;
		if(height_cnt>=10)  //200ms 
		{
			/////////////////////////////////////

		 exp_speed = PID_calculate( dT2,            //周期
																0,				//前馈
																0,				//期望值(设定值)
																-set_height_e,			//反馈值
																&h_height_arg, //PID参数结构体
																&h_height_val,	//PID数据结构体
																1500 *en			//integration limit,积分限幅
																 );			//输出	
			
			exp_speed = LIMIT(exp_speed,-300,300);
			/////////////////////////////////////
			dT2 = 0;
			height_cnt = 0;
		}
		
		speed_cnt = 0;
		dT = 0;				
	}		
/////////////////////////////////////////////////////////////////////////////////	
	if(step==0)
	{
		step = 1;
	}
	
	if(en < 0.1f)
	{
		return (thr);
	}
	else
	{
		return (thr_out);
	}
}
Esempio n. 2
0
static void pidInterrupt(void* object) {
    PIDController* pid = object;

    if(pid->enabled) {
        // process the PID data //
        pid->data.input = pid->pidInput(pid->state);
        PID_calculate(&pid->data);
        pid->pidOutput(pid->state, pid->data.output);
    }
}
Esempio n. 3
0
/**
  * @brief 底盘控制程序
  * @param _T程序调用周期(s)
  * @retval None
  * @details 	根据当前的期望底盘移动速度和当前底盘功率
	*						对四个底盘电机的电流进行控制
  */
void ChassisControl(float _T)
{	
	if (SysMode!=SYS_CALISTATE&&SysMode!=SYS_STOPSTATE)
	{
		switch(ControlMode)
		{
			case MC_NORMAL:
				ChassisRotateOut=PID_calculate( _T,            //周期(单位:秒)
																				0,				//前馈值
																				0,				//期望值(设定值)
																				0,			//反馈值()
																				&Chassis_Rot_PID_arg, //PID参数结构体
																				&Chassis_Rot_PID_val,	//PID数据结构体
																				0.2		//integration limit,积分限幅
																				 );
				break;
			
			case MC_MODE1:
				
				break;
			
			case MC_MODE2:
				
				break;
			
			default:
				break;
		}
	}
	else
	{
		ChassisRotateOut=0.0f;
		ChassisGoToward=0.0f;
		ChassisGoLeftRight=0.0f;	
	}
	ChassisMotorSpeed1=-ChassisGoToward*0.075f+ChassisGoLeftRight*0.075f+ChassisRotateOut;
	ChassisMotorSpeed2=ChassisGoToward*0.075f+ChassisGoLeftRight*0.075f+ChassisRotateOut;
	ChassisMotorSpeed3=ChassisGoToward*0.075f-ChassisGoLeftRight*0.075f+ChassisRotateOut;
	ChassisMotorSpeed4=-ChassisGoToward*0.075f-ChassisGoLeftRight*0.075f+ChassisRotateOut;
	CMOutput1=PID_calculate( _T,            //周期(单位:秒)
																0,				//前馈值
																0,				//期望值(设定值)
																0,			//反馈值()
																&Chassis_Vec_PID_arg, //PID参数结构体
																&Chassis_Vec_PID_val1,	//PID数据结构体
															 	0.2		//integration limit,积分限幅
																 );
	CMOutput2=PID_calculate( _T,            //周期(单位:秒)
																0,				//前馈值
																0,				//期望值(设定值)
																0,			//反馈值()
																&Chassis_Vec_PID_arg, //PID参数结构体
																&Chassis_Vec_PID_val2,	//PID数据结构体
																0.2		//integration limit,积分限幅
																 );
	CMOutput3=PID_calculate( _T,            //周期(单位:秒)
																0,				//前馈值
																0,				//期望值(设定值)
																0,			//反馈值()
																&Chassis_Vec_PID_arg, //PID参数结构体
																&Chassis_Vec_PID_val3,	//PID数据结构体
																0.2		//integration limit,积分限幅
																 );
	CMOutput4=PID_calculate( _T,            //周期(单位:秒)
																0,				//前馈值
																0,				//期望值(设定值)
																0,			//反馈值()
																&Chassis_Vec_PID_arg, //PID参数结构体
																&Chassis_Vec_PID_val4,	//PID数据结构体
																0.2		//integration limit,积分限幅
																 );
	
	
}