对无人机高度环的思考和第一次炸机体验

	前两天学长布置了一个任务，大体就是在无人机起飞一段时间后再次改变飞行高度。一开始我觉得挺简单，随便改改就行了，所以今天才开始写，然后。。。炸机了。。。。先说说第一次炸机的体验。。就是看到无人机炸机的疯狂往屋顶上窜的时候，大脑先是一阵空白，然后我也没想起来要赶快离的远点，就是满脑子的  “为什么会这样？” “这TM跟我想的不对啊”，大概过了三四秒才反应过来，使劲向下拉油门，其实拉油门也没什么了，毕竟我程序写的是在5S后一键降落。然后我又看着飞机疯狂向下坠地。很庆幸，当时有两个同学在写程序，没有伤到同学。。总之炸机确实非常危险，要尽量避免事故的发生。
	我使用的是匿名的拓空者，这里贴上匿名飞控高度环和自动起飞任务的代码部分（未修改之前的）：

void Auto_Take_Off_Land_Task(u8 dT_ms)//
{
	static u16 take_off_ok_cnt;
	
	one_key_take_off_task(dT_ms);
	//如果已经解锁
	if(flag.unlock_sta)
	{
		if(flag.taking_off)
		{	
			if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL)
			{
				flag.auto_take_off_land = AUTO_TAKE_OFF;		
			}
		}

	}
	else
	{
		auto_taking_off_speed = 0;	
		flag.auto_take_off_land = AUTO_TAKE_OFF_NULL;	
	}

	
	if(flag.auto_take_off_land ==AUTO_TAKE_OFF)
	{
		//设置最大起飞速度
		s16 max_take_off_vel = LIMIT(Ano_Parame.set.auto_take_off_speed,20,200);
		//
		take_off_ok_cnt += dT_ms;
		//这个地方是自动起飞速度（一键起飞，一键降落）的关键
		auto_taking_off_speed = AUTO_TAKE_OFF_KP *(Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out);
		//计算自动起飞速度
		auto_taking_off_speed = LIMIT(auto_taking_off_speed,0,max_take_off_vel);
		
		//退出起飞流程条件1，满足高度或者流程时间大于5000毫秒。
		if(take_off_ok_cnt>=5000 || (Ano_Parame.set.auto_take_off_height - loc_ctrl_2.exp[Z] <2))//(auto_ref_height>AUTO_TAKE_OFF_HEIGHT)
		{
			//代表此时已经飞行稳定，加入积分控制
			flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;
			
			
		}
		//退出起飞流程条件2，2000毫秒后判断用户正在控制油门。
		if(take_off_ok_cnt >2000 && ABS(fs.speed_set_h_norm[Z])>0.1f)// 一定已经taking_off,如果还在推杆，退出起飞流程
		{
			//代表此时已经飞行稳定，加入积分控制
			flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;
		}
	
	}
	else 
	{
		take_off_ok_cnt = 0;
		
		if(flag.auto_take_off_land ==AUTO_TAKE_OFF_FINISH)
		{
			auto_taking_off_speed = 0;
			
		}
		
	}



	//调用自动降落后直接给予z方向上下降的速度
	if(flag.auto_take_off_land == AUTO_LAND)
	{
		//设置自动下降速度
		auto_taking_off_speed = -(s16)LIMIT(Ano_Parame.set.auto_landing_speed,20,200);

	}
}
//高度环控制
void Alt_2level_Ctrl(float dT_s)
{
	//自动起飞任务
	Auto_Take_Off_Land_Task(1000*dT_s);
	//设置起飞速度
	fs.alt_ctrl_speed_set = fs.speed_set_h[Z] + auto_taking_off_speed;
	//计算出期望高度然后限幅
	loc_ctrl_2.exp[Z] += fs.alt_ctrl_speed_set *dT_s;
	loc_ctrl_2.exp[Z] = LIMIT(loc_ctrl_2.exp[Z],loc_ctrl_2.fb[Z]-200,loc_ctrl_2.fb[Z]+200);
	//
	loc_ctrl_2.fb[Z] = (s32)wcz_hei_fus.out;/
	
	if(fs.alt_ctrl_speed_set != 0)
	{
		flag.ct_alt_hold = 0;
	}
	else
	{
		if(ABS(loc_ctrl_1.exp[Z] - loc_ctrl_1.fb[Z])<20)
		{
			flag.ct_alt_hold = 1;
		}
	}

	if(flag.taking_off == 1)
	{

		PID_calculate( dT_s,            //周期（单位：秒）
						0,				//前馈值
						loc_ctrl_2.exp[Z],				//期望值（设定值）
						loc_ctrl_2.fb[Z],			//反馈值（）
						&alt_arg_2, //PID参数结构体
						&alt_val_2,	//PID数据结构体
						100,//积分误差限幅
						0			//integration limit，积分限幅									
						 );

	}
	else
	{
		loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z];
		alt_val_2.out = 0;
		
	}
	
	alt_val_2.out  = LIMIT(alt_val_2.out,-150,150);
}

在自动起飞任务里匿名调用了一键起飞任务，当调用一键起飞或者手动推油门起飞时，则会执行该函数。又因为飞机起飞时数据偏差可能会很大，影响积分控制，所以在起飞的一段时间里会取消积分控制，执行PD控制。而当程序退出起飞流程后或者说飞行稳定后，则加入积分控制。而飞机初始的起飞高度，又由该指令控制

auto_taking_off_speed = AUTO_TAKE_OFF_KP *(Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out);

这里使用了一个比例控制，该式右边第一项为KP项，一个比例系数。括号内第一项为自动起飞高度，也就是飞机在执行自动起飞时最后达到的高度。括号内第二项为当前高度。第一项也可以看作是一个期望高度。也只有当起飞高度=期望高度时，自动起飞速度才会置0，自动起飞速度在高度环会被调用作为起飞速度的一项。而在自动起飞期望高度该代码中该项被置零，所以我们只需要将自动起飞期望高度设置成我们第一次想要达到的高度即可。（这里修改起飞高度好像要用匿名上位机写入flash？）

最重要的是第二步，在飞机初始高度保持一段时间后在改变飞机的飞行高度。于是头脑简单的我直接给高度环里的期望高度粗暴的加上50，就像这样：

loc_ctrl_2.exp[Z] += fs.alt_ctrl_speed_set *dT_s+50;

然后就。。。医院wifi挺好的。。。
为什么不能直接这样粗暴的加上50呢？
我后来分析了炸机原因，发现高度环是每11ms调用一次的，也就是说每11ms，不管竖直方向上的速度，这个高度环的期望值都会自增50，这代表什么概念呢，大概就是要使反馈高度跟上这样的期望高度，飞机要每11ms竖直飞行50cm才可以达到。总的来说这里的问题出在反馈高度的增加值远远不如期望高度增加的快，所以导致飞机疯狂向屋顶上冲导致炸机。这里，在退出起飞流程后，Z轴上的速度只取决于起飞速度的大小，而起飞速度的大小则会取决于油门。所以，在这里我觉得可以通过改变起飞速度的值来达到再次改变高度的目的。

//这是分割线

在经历了昨天的炸机之后，今天我自己又写了写程序，效果不错。可以完成无人机在起飞流程结束后完成高度改变，所以最后我将无人机高度变化的操作封装了成了一个函数，可以在匿名的拓空者（试过了没问题）代码中直接调用，在这里分享给大家。

void Auto_height_change(float height,float time_s)
{
	//定义速度和相对高度
	float speed,rela_height;
	//只有退出起飞流程后才能执行该函数
	if(flag.auto_take_off_land == AUTO_TAKE_OFF_FINISH)
	{
			//计算出相对高度，无人机在该基础上应该改变多少高度
			rela_height=height-wcz_hei_fus.out;
			//时间必须大于0，否则无效
			if(time_s>0)
			{
				//计算出应该Z轴具有的速度
				speed = rela_height / time_s;
				//赋值z轴速度
				Program_Ctrl_User_Set_Zcmps(speed);
				//将标志位置1进入计时状态
				change_flag=1;
			}
	}
}

void Auto_height_change_task(float height,float time_s)
{
	double true_time;
	Auto_height_change(height,time_s);
	if(change_flag)
	{
//		cnt_height_change++;
		//计算出当前的真实时间
		//当开始计时后每次执行该函数真实时间增加0.05s（即50ms）
		true_time += 0.05;
	}
	
//	true_time = cnt_height_change*0.05;
	//如果时间一旦达到规定的时间
	if(true_time==time_s)
	{
		//将真实时间清0，以便于下次再次计算
		true_time=0;
		//将标志位清0退出计时
		change_flag=0;
		//z轴方向不在赋予速度，以达到定高的效果
		Program_Ctrl_User_Set_Zcmps(0);
	}
}

我比较菜，代码也就写成这样了。这俩函数的第一个参数就是你想要达到的高度，你可以随便设置。第二个参数就是无人机从起飞完成的高度到你设置高度的时间，如果你想要较快的变化，可以将时间设置的短一些；想慢慢观察可以将时间设置的长一些。最后一个函数我是将其写在轮询任务9中，也就是50ms就会调用一次该函数。总的来说，最终的控制效果确实可以达到，但是会产生高度的误差。一方面可能是高度环和该函数调用周期不同而产生的差值，另一方面可能是数据融合高度不稳定造成的。嗯，就这样。