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

工程估计要看很多遍才能顺下来，而且有的地方需要看的比较细，对于一些比较重要又比较难的部分打算写下来一点一点分析。
姿态解算部分先跳过，姑且就认为它的imu算法（长的一批）可以得到正确的姿态信息。

这次先分析飞行状态任务：

Flight_State_Task(u8 dT_ms,s16 *CH_N)

程序好长还没注释，在一直没能仔细看它的具体功能。

先来认识几个结构体：

typedef struct
{
	s16 alt_ctrl_speed_set;
	float speed_set_h[VEC_XYZ];	
	float speed_set_h_cms[VEC_XYZ];
	
	float speed_set_h_norm[VEC_XYZ];
	float speed_set_h_norm_lpf[VEC_XYZ];
	
}_flight_state_st;

看命名应该是飞行状态结构体，从上到下分别是高度控制速度设置变量（完全nonsense的翻译。。。）、速度设置向量、以cm/s为单位的速度设置向量（话说垂直方向的速度为啥要用向量表示？），最后两个是速度设置模和它的滤波，也是向量，刚开始还没看懂，以为norm是模的意思，后来看对它的操作才明白，实际上这里的norm是归一化的意思（normalization ），就是一个比例，不代表具体的数值，一般用它乘以你定义的最大速度之类的，得到实际的速度。。。。

进入函数后是这样的：

s16 thr_deadzone;
static float max_speed_lim,vel_z_tmp[2];

thr_deadzone = (flag.wifi_ch_en != 0) ? 0 : 50;
fs.speed_set_h_norm[Z] = my_deadzone(CH_N[CH_THR],0,thr_deadzone) *0.0023f;
fs.speed_set_h_norm_lpf[Z] += 0.5f *(fs.speed_set_h_norm[Z] - fs.speed_set_h_norm_lpf[Z]);

先定义一个thr_deadzone，死区油门，第二行定义了啥不知道。

然后对thr_deadzone赋值，接着下面用了一个my_deadzone函数，这个函数看了好半天才知道它要干啥：先看函数的三个输入变量，当第二个变量为0时，对于输入的CH_N[CH_THR]，如果它大于输入的thr_deadzone，那么就会返回CH_N[CH_THR]-thr_deadzone的值，否则就会返回0。

值赋给fs.speed_set_h_norm[Z] 后滤一下波。

if(flag.unlock_sta)
{	
	if(fs.speed_set_h_norm[Z]>0.01f && flag.motor_preparation == 1) // 0-1
	{
		flag.taking_off = 1;
	}	
}		

如果解锁了，再如果推的油门大于0.01且电机准备标志位置位，那么就将taking off状态位置位，意思是转换起飞状态。

fc_stv.vel_limit_z_p = MAX_Z_SPEED_UP;
fc_stv.vel_limit_z_n = -MAX_Z_SPEED_DW;	

设置上升和下降速度的最大值，为了防止上升或者下降太快。

if(flag.taking_off)
{
		
	if(flying_cnt<1000)//800ms
	{
		flying_cnt += dT_ms;
	}
	else
	{
	
		flag.flying = 1;  
	}
	
	if(fs.speed_set_h_norm[Z]>0)
	{
		
		vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_UP);
	}
	else
	{
		
		vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_DW);
	}

	
	vel_z_tmp[1] = vel_z_tmp[0] + program_ctrl.vel_cmps_h[Z] + pc_user.vel_cmps_set_z;

	vel_z_tmp[1] = LIMIT(vel_z_tmp[1],fc_stv.vel_limit_z_n,fc_stv.vel_limit_z_p);

	fs.speed_set_h[Z] += LIMIT((vel_z_tmp[1] - fs.speed_set_h[Z]),-0.8f,0.8f);
}

这里的逻辑过程是：

如果taking off标志位已经置位，也就是进入了起飞状态：
那么如果不够1000ms，就等待，如果够了1000ms，那么我们就认为他已经在飞行了，将flying标志位置一

接下来如果fs.speed_set_h_norm[Z]>0，也就是这个速度设置大于零，那么设置上升速度，赋给vel_z_tmp[0] ，如果小于零，那么设置下降速度给vel_z_tmp[0]

然后是对飞控系统的z速度目标量进行综合设定，vel_z_tmp[1] 等于设置的上升或者下降速度vel_z_tmp[0] ，加上program_ctrl.vel_cmps_h[Z]（？不知道这个干啥的，关于它的操作，全部在一个叫flyctrl的文件夹中，超级长的代码，估计下一个就分析它了。。。）再加上pc_user.vel_cmps_set_z（这个也没看懂呢，user定义的速度是啥玩意，遥控器？），这部分看不懂，而且也不知道为啥这三个速度要叠加，先放一放。反正就得到一个总的速度目标量，然后限幅，然后传给速度设置变量fs.speed_set_h[Z]。

如果taking off标志位没有置位，那么直接速度设置为零。

float speed_set_tmp[2];

fs.speed_set_h_norm[X] = (my_deadzone(+CH_N[CH_PIT],0,50) *0.0022f);
fs.speed_set_h_norm[Y] = (my_deadzone(-CH_N[CH_ROL],0,50) *0.0022f);
	
LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[X],fs.speed_set_h_norm_lpf[X]);
LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[Y],fs.speed_set_h_norm_lpf[Y]);

max_speed_lim = MAX_SPEED;

接下来，要对x、y方向的速度做控制（？），前面仍然是定义死区，然后滤波，把最大速度赋给这个谁谁谁。

if(switchs.of_flow_on && !switchs.gps_on )
	{
		max_speed_lim = 1.5f *wcz_hei_fus.out;
		max_speed_lim = LIMIT(max_speed_lim,50,150);
	}	

如果光流开关开了，gps没开，那么最大速度调整一下（不知道为啥）

fc_stv.vel_limit_xy = max_speed_lim;

再把最大速度赋给这个谁。。

speed_set_tmp[X] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[X] + program_ctrl.vel_cmps_h[X] + pc_user.vel_cmps_set_h[X];
speed_set_tmp[Y] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[Y] + program_ctrl.vel_cmps_h[Y] + pc_user.vel_cmps_set_h[Y];

length_limit(&speed_set_tmp[X],&speed_set_tmp[Y],fc_stv.vel_limit_xy,fs.speed_set_h_cms);

fs.speed_set_h[X] = fs.speed_set_h_cms[X];
fs.speed_set_h[Y] = fs.speed_set_h_cms[Y];	

对xy方向的速度做一样的综合，赋给最终的控制量

land_discriminat(dT_ms);

这是个检测着陆的函数，具体再看。

if(rolling_flag.rolling_step == ROLL_END)
{
	if(imu_data.z_vec[Z]<0.25f)
	{

		flag.unlock_cmd = 0;
	}

}	

倾斜过大上锁？可能是自动上锁，注释里写着慎用。。。

if(sensor.gyr_CALIBRATE != 0 || sensor.acc_CALIBRATE != 0 ||sensor.acc_z_auto_CALIBRATE)
{
	imu_state.G_reset = 1;
}

校准，没看懂为啥这样写

if(imu_state.G_reset == 1)
{
	flag.sensor_imu_ok = 0;
	LED_STA.rst_imu = 1;
	WCZ_Data_Reset(); 
}
else if(imu_state.G_reset == 0)
{	
	if(flag.sensor_imu_ok == 0)
	{
		flag.sensor_imu_ok = 1;
		LED_STA.rst_imu = 0;
		ANO_DT_SendString("IMU OK!");
	}
}

仍然没看懂。。。。

	if(flag.unlock_sta == 0)
	{
		flag.flying = 0;
		landing_cnt = 0;
		flag.taking_off = 0;
		flying_cnt = 0;
		
		
		flag.rc_loss_back_home = 0;
		

		if(flag.taking_off == 0)
		{
  		//wxyz_fusion_reset();
		}
	}

飞行状态复位，如果没解锁，所有都置零，复位融合不知道啥玩意，被注释掉了。

总结一下就是。。。我也说不清这函数干嘛的。。先这样，总之过一遍混个脸熟，看完其他的估计就可以理解了，再回来补充。