匿名无人机飞控代码整理7

最后来看定高。。。
1、高度数据融合任务，

WCZ_Fus_Task(11);
GPS_Data_Processing_Task(11);

数据融合好像也很重要？虽然不知道是干嘛的。。。语句很复杂，一点一点看一下。

if(flag.taking_off)
{
	baro_offset_ok = 2;
}
else
{
	if(baro_offset_ok == 2)
	{
		baro_offset_ok = 0;
	}
	//reset
	tof2baro_offset = 0;
}

如果起飞标志为1，那么baro_offset_ok 赋值为2，否则如果baro_offset_ok 为2，那么再把它清零，接着把tof2baro_offset清零。

什么意思呢，就是如果起飞标志置位，那么气压偏置位就ok了，否则，起飞标志为零时，也就是没有起飞，那么就把这两个offset清零。好吧，还是不知道在干嘛。。

if(baro_offset_ok >= 1)
{
	ref_height_get_1 = baro_height - baro_h_offset + baro_fix  + tof2baro_offset;

}
else
{
	if(baro_offset_ok == 0 )
	{
		baro_h_offset = baro_height;
		if(flag.sensor_imu_ok)
		{
			baro_offset_ok = 1;
		}
	}
}

如果baro_offset_ok大于等于1，也就是上一个判断语句中置为2的那个东西，那么参考高度就等于这个式子（虽然不知道这式子干嘛的）。。。
如果baro_offset_ok小于1，也就是等于0，那么就对baro_h_offset这个偏置赋值，如果imu是ok的，那么baro_offset_ok就等于1，（不知道在干嘛）

if((flag.flying == 0) && flag.auto_take_off_land == AUTO_TAKE_OFF	)
{
	wcz_f_pause = 1;
	
	baro_fix = 0;
}
else
{
	wcz_f_pause = 0;
	
	if(flag.taking_off == 0)
	{
		baro_fix1 = 0;
		baro_fix2 = 0;
			
	}
	baro_fix2 = -BARO_FIX;

	
	baro_fix = baro_fix1 + baro_fix2 - BARO_FIX;//+ baro_fix3;
}

如果是自动起飞且还没飞（？），那么wcz_f_pause为1，也就是这个什么东西暂停，baro_fix这个修正值也为0.
否则，如果不是上述状态，那么不暂停。再如果没有起飞。。。后面对一堆fix赋值，是真的看不懂。。

	if((sens_hd_check.of_df_ok || sens_hd_check.of_ok) && baro_offset_ok) 
	{
		if(switchs.tof_on || switchs.of_tof_on) 
		{
			if(switchs.of_tof_on) 
			{
				if(sens_hd_check.of_ok)
				{
					ref_tof_height = OF_ALT;
				}
				else if(sens_hd_check.of_df_ok)
				{
					ref_tof_height = Laser_height_cm;
				}
			}
			else//switchs.tof_on
			{
//				ref_tof_height = Laser_height_mm/10;
			}
			
			
			if(tof_offset_ok == 1)
			{
				ref_height_get_2 = ref_tof_height + baro2tof_offset;
				
				ref_height_used = ref_height_get_2;
				
				tof2baro_offset += 0.5f *((ref_height_get_2 - ref_height_get_1) - tof2baro_offset);
			
			}
			else
			{
				baro2tof_offset = ref_height_get_1 - ref_tof_height ; 	
				tof_offset_ok = 1;
			}
		}
		else
		{
			tof_offset_ok = 0;    			
			ref_height_used = ref_height_get_1 ;
		}
	}

接下来判断，如果tof或者of正常，且气压计记录相对值之后，如果tof数据有效，那么再如果光流带tof，光流优先，光流ok，就执行：

ref_tof_height = OF_ALT;

否则如果这个ok，of_df_ok，就执行这个：

ref_tof_height = Laser_height_cm;

没看懂在干嘛。。。
如果只是tof模块有效，就执行

ref_tof_height = Laser_height_mm/10;

当然了，这部分被注释掉了，因为没这模块。

	if(tof_offset_ok == 1)
		{
			ref_height_get_2 = ref_tof_height + baro2tof_offset;
			
			ref_height_used = ref_height_get_2;
			
			tof2baro_offset += 0.5f *((ref_height_get_2 - ref_height_get_1) - tof2baro_offset);
				
		}
		else
		{
			baro2tof_offset = ref_height_get_1 - ref_tof_height ; 
			
			tof_offset_ok = 1;			
		}

如果tof_offset_ok等于1,那么

ref_height_get_2 = ref_tof_height + baro2tof_offset;

然后滤波，如果不等于一，那么赋值baro2tof_offset，并把它置为1.

如果光流和tof都无效：

else
{
	
	tof_offset_ok = 0;
	
	ref_height_used = ref_height_get_1 ;
}

tof_offset_ok 置零，ref_height_used赋值为ref_height_get_1

如果硬件不正常：

else
{
	ref_height_used = ref_height_get_1;
}

也是对ref_height_used赋一个值。。不懂

WCZ_Data_Calc(dT_ms,wcz_f_pause,(s32)wcz_acc_use,(s32)(ref_height_used));

最后这个操作叫做世界z方向高度数据融合，利用某种滤波，结果大概是得到实际加速度、速度和高度。。。？
具体程序流程真的看不懂，记住这里是用来给出当前高度速度角速度信息的就行了吧。。。

2、高度速度环和高度环控制：

/*高度速度环控制*/
Alt_1level_Ctrl(11e-3f);

/*高度环控制*/
Alt_2level_Ctrl(11e-3f);

不详细说了，大概流程可以看的懂，列一下思路：

这里的高度环里的期望值，也就是目标高度是这样的：

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;

给出一个固定的速度，期望高度就是在上一个期望高度上面叠加这个速度，得到下一个时刻的期望高度。
高度环里的反馈值就是高度融合之后的高度测量值。
再利用pid，得到应有的速度。

在高度速度环里，速度的期望值：

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

是上一环的结果+最开始的速度设定值，并不完全是上一环的输出。
反馈值利用了什么微分先行pid，可能跟算法有关，不详细扣了，反正利用pid，就得到了速度环的输出，也就是加速度，作为油门值。。。

嗯，感觉代码好像看的差不多了，下午开始把工程的控制过程整合一下，争取半天完成。。。