一直没看的无人机定高

除了定高的部分，其他没看的还有一些控制函数。

定高的原理的话，也是利用了两级pid，

按照原本的理解，从外环开始看的话，反馈高度比较容易获得，就是激光测距得到的高度。期望高度好像无法得到。内环的话，期望速度是外环的输出，实际速度不知道怎么测得的，应该是高度的微分，等下仔细看看。

说期望高度“无法得到”，是因为没有必要，或者说我们通常不这么做。

首先是因为我们通常不使用定高在某个高度这个功能（吧），而是通常利用一键起飞，飞起来后再控制无人机能够按照一定的速度，去上升和下降。

然后从控制效果的角度看，如果按照我们通常的两级pid的控制思路：期望高度是我们给定的高度，反馈高度是我们测得的高度，利用pid可以得到期望速度，再利用这个期望速度结合实际速度，可以得到我们应得的加速度，也就是电机输出量。这也许可以达到定高的目的（而且是在不考虑最初期望高度和反馈速度的巨大偏差可能带来的不稳定的情况下），但会使我们丧失对飞机运动的控制：因为我们常常需要飞机上升或者下降，通常这个需求是在控制中被作为期望速度输入。但看上述的控制过程，我们会发现期望速度会由外环给出，我们不能对它赋值以实现控制。
这种情况下想要实现飞机的上升和下降，就只能去改变飞机的期望高度，而且我们很难去控制它的速度，这当然是我们不想要看到的。

为什么角度环没有遇到这种问题？这是一个较为细微的概念，对于初学者对于控制的原理和过程不明确，就不太容易理解。原因在于，我们对无人机的控制需求大体只有转向、左右、前后、上下移动，这也是遥控器上的那两个东西所能控制的部分。

我们实现前后左右移动，也就是想要飞机达到我们期望的前后左右方向的速度，只要使飞机的pitch和roll保持固定的角度就可以，想要前进，就让pitch增大，而且想要前进的速度越大，pitch就要越大。因此在角度环，我们外环是角度，用期望速度换算成期望角度、内环是角速度，得到电机输出量，就可以实现稳定或者移动。

但上下就不同了。我们要想实现一定的上下速度，速度本身就是目标值。对应角度环的话，这个速度应该对应角速度。

那问题来了，为什么我们不直接使用一个环呢？只用速度环按道理来讲就可以实现以指定速度上下移动了吧。原因就在于。。。还不知道怎么去表述，但原因之一应该跟起飞降落有关。

接下来看代码：
对于高度控制的头文件，定义了如下函数，内容很简单：

高度环控制和pid初始化、高度速度环控制和pid初始化
以及自动上升下降任务

我们先看这个串级pid：
参数初始化就不说了，其实也不是很懂。

对于外环：

Auto_Take_Off_Land_Task(1000*dT_s);

先调用了自动起飞/降落任务，

fs.alt_ctrl_speed_set = fs.speed_set_h[Z] + auto_taking_off_speed;

这个 fs.alt_ctrl_speed_set速度，就是我们的实际期望速度，为什么叫实际期望速度？就是说它是由两个速度相加：
1、 fs.speed_set_h[Z]

这个速度是程序或者用户对飞机的控制指令，包括那三个东西相加。

2、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, 
					&alt_val_2,	
					100,
					0								
					 );
}
else
{
	loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z];
	alt_val_2.out = 0;		
}

如果taking_off标志位有效，那么说明飞机已经起来了，那么就执行pid，否则，认为飞机在地上，也就没有速度。期望值等于反馈值，所需速度也就为0.

对于内环：

loc_ctrl_1.exp[Z] = 0.6f *fs.alt_ctrl_speed_set + alt_val_2.out;

w_acc_z_lpf += 0.1f *(imu_data.w_acc[Z] - w_acc_z_lpf); 

loc_ctrl_1.fb[Z] = wcz_spe_fus.out + Ano_Parame.set.pid_alt_1level[KD] *w_acc_z_lpf;

反馈和期望速度赋值，注意这里的期望速度，两个值相加，应该是用于修正。

PID_calculate( dT_s,          
				0,				
				loc_ctrl_1.exp[Z],				
				loc_ctrl_1.fb[Z] ,			
				&alt_arg_1, 
				&alt_val_1,	
				100,
				(THR_INTE_LIM *10 - err_i_comp )*out_en									
				 );

pid就略了。

if(flag.taking_off == 1)
{
	LPF_1_(1.0f,dT_s,THR_START *10,err_i_comp);//err_i_comp = THR_START *10;			
}
else
{
	err_i_comp = 0;
}

如果起飞了，那么 err_i_comp就赋值这个，否则，飞机在地上，err_i_comp就为0。
这个err_i_comp应该也是修正用的。

loc_ctrl_1.out[Z] = out_en *(alt_val_1.out + err_i_comp);

loc_ctrl_1.out[Z] = LIMIT(loc_ctrl_1.out[Z],0,MAX_THR_SET *10);	

mc.ct_val_thr = loc_ctrl_1.out[Z];

飞行时out_en才为1，地上时为0.
也就是说是否把油门值输出，取决于当前taking_off状态，因为即使在地上时，没有alt_val_2.out，但仍然有fs.alt_ctrl_speed_set

接下来就是看看非常混乱的一键起飞/降落了。。。。
非常混乱，是因为这个相关的代码有好几个，不知道怎么控制的，尝试分析一下。。

先是注意到了高度控制的那个文件里，有一个自动起飞降落任务：

Auto_Take_Off_Land_Task

进去之后还有一个一键起飞降落任务：

one_key_take_off_task

先看这个一键起飞任务：

void one_key_take_off_task(u16 dt_ms)
{
	if(one_key_taof_start != 0)
	{
		one_key_taof_start += dt_ms;
		
		
		if(one_key_taof_start > 1400 && flag.motor_preparation == 1)
		{
			one_key_taof_start = 0;
				if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL)
				{
					flag.auto_take_off_land = AUTO_TAKE_OFF;

					flag.taking_off = 1;
				}
			
		}
	}
	//reset
	if(flag.unlock_sta == 0)
	{
		one_key_taof_start = 0;
	}

}

对one_key_taof_start进行判断，如果不是0，那就++，加到1400了，同时电机准备好了，就把one_key_taof_start清零。如果此时起飞降落标志位为起飞无效，那么就把状态改为起飞，taking _off=1。
如果没解锁，那么one_key_taof_start永远是0，无法累计。

也就是说，解锁之后等待1.4s，且状态为起飞无效，那么就把状态设置为起飞。

再看上一个任务：自动起飞降落任务
先执行了一键起飞任务，然后判断：

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;	
}

如果解锁，且taking _off有效，那么如果起飞无效，就改成有效。
否则，没有解锁，auto_taking_off_speed恒为0，起飞设置为无效。

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);
	

	if(take_off_ok_cnt>=5000 || (Ano_Parame.set.auto_take_off_height - loc_ctrl_2.exp[Z] <2))
	{
		flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;			
	}

	if(take_off_ok_cnt >2000 && ABS(fs.speed_set_h_norm[Z])>0.1f)
	{
		flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;
	}

}

如果自动起飞有效了，那么设置自动起飞速度。自动起飞流程有两个退出条件：
1、自动起飞流程运行的时间大于5s，或者满足高度。
2、自动起飞流程运行的时间大于2s，且用户在控制油门。
满足这两个条件之一，都认为自动起飞已经完成。

else 
{
	take_off_ok_cnt = 0;		
	if(flag.auto_take_off_land ==AUTO_TAKE_OFF_FINISH)
	{
		auto_taking_off_speed = 0;			
	}		
}

否则，自动起飞无效（还没飞或者已经会玩），就不计数。
如果已经飞完，那么把自动速度设为零。

如果状态是自动下降，就给一个自动下降速度。

if(flag.auto_take_off_land == AUTO_LAND)
{
	auto_taking_off_speed = -(s16)LIMIT(Ano_Parame.set.auto_landing_speed,20,200);
}

除此之外还有一个一键起飞函数：

one_key_take_off（）

内容比较短：

void one_key_take_off()
{
	if(flag.unlock_err == 0)
	{	
		if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL && one_key_taof_start == 0)
		{
			one_key_taof_start = 1;
			flag.unlock_cmd = 1;
		}
	}
}

上述这三个有控制关系，一般来讲，只要调用函数one_key_take_off（），且已经解锁了，一键就可以起飞。

嗯，飞行就讲到这里，还有一个问题就是，期望高度如何去设定，等看看遥控器程序再回来解答。接下来的任务就是如何编写控制函数了。。。orz会不会太难了啊唉