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

先跳过高度部分，这里看location部分

可以看出来这部分也是直接与我们的循迹、追踪任务相关的。所以这部分的掌握应该很重要把。
前面有很多暂时不需要看，flyctrl那个巨长。。。所以就先看速度环控制部分了。

if(switchs.uwb_on && (!switchs.of_flow_on) && (!switchs.gps_on))
{
	mode_f[1] = 3;
	if(mode_f[1] != mode_f[0])
	{
		Loc_1level_PID_Init();
		mode_f[0] = mode_f[1];
	}	
	
	loc_ctrl_1.out[X] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[X] ;
	loc_ctrl_1.out[Y] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[Y] ;
		
}

如果只有uwb（其实没有），就对loc_ctrl_1.out赋值，这个结构体是这样的：

typedef struct
{
	float exp[VEC_XYZ];
	s32 fb[VEC_XYZ];

	
	float out[VEC_XYZ];
}_loc_ctrl_st;

做pid用。

	else if(switchs.uwb_on && switchs.of_flow_on && (!switchs.gps_on))
	{
		mode_f[1] = 4;
		if(mode_f[1] != mode_f[0])
		{
			Loc_1level_PID_Init();
			mode_f[0] = mode_f[1];
		}	



		h2w_2d_trans(fs.speed_set_h,imu_data.hx_vec,loc_ctrl_1.exp);
		
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.w_acc[X],vel_fb_d_lpf[X]);
		LPF_1_(5.0f,dT_ms*1e-3f,imu_data.w_acc[Y],vel_fb_d_lpf[Y]);		
	
		if(sens_hd_check.of_ok)
		{
			vel_fb_h[0] = OF_DX2;
			vel_fb_h[1] = OF_DY2;
		}
		else//sens_hd_check.of_df_ok
		{
			vel_fb_h[0] = of_rdf.gnd_vel_est_h[X];
			vel_fb_h[1] = of_rdf.gnd_vel_est_h[Y];	
		}
		
		h2w_2d_trans(vel_fb_h,imu_data.hx_vec,vel_fb_w);
		
		loc_ctrl_1.fb[X] = vel_fb_w[0] + 0.03f *vel_fb_d_lpf[X];
		loc_ctrl_1.fb[Y] = vel_fb_w[1] + 0.03f *vel_fb_d_lpf[Y];
		
		fb_speed_fix[0] = uwb_data.w_vel_cmps[0];
		fb_speed_fix[1] = uwb_data.w_vel_cmps[1];
		
		for(u8 i =0;i<2;i++)
		{
			PID_calculate( dT_ms*1e-3f,           
										loc_ctrl_1.exp[i] ,				
										loc_ctrl_1.exp[i] ,				
										loc_ctrl_1.fb[i] ,			
										&loc_arg_1[i], 
										&loc_val_1[i],	
										50,
										10 *flag.taking_off			
										 )	;	
			
			
			PID_calculate( dT_ms*1e-3f,           
										loc_ctrl_1.exp[i] ,				
										loc_ctrl_1.exp[i] ,				
										fb_speed_fix[i] ,			
										&loc_arg_1_fix[i], 
										&loc_val_1_fix[i],	
										50,
										10 *flag.taking_off		
										 )	;	
			
			pos_ctrl_w_out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;	
		}	
		
		w2h_2d_trans(pos_ctrl_w_out,imu_data.hx_vec,pos_ctrl_h_out); 
		
		loc_ctrl_1.out[0] = pos_ctrl_h_out[0];
		loc_ctrl_1.out[1] = pos_ctrl_h_out[1];	
	}

否则，如果只有光流和uwb，没有gps，那么对mode做相同的操作，接着使用了物坐标系对世界坐标的转换：

void h2w_2d_trans(float h[VEC_XYZ],float ref_ax[VEC_XYZ],float w[VEC_XYZ])
{
	w[X] = h[X] *ref_ax[X] + h[Y] *(-ref_ax[Y]);
	w[Y] = h[X] *ref_ax[Y] + h[Y] *  ref_ax[X];
	
}

也就是把速度（相对于物坐标系的），转换成世界坐标系的，赋给loc_ctrl_1.exp，也就是预期的速度。怎么理解呢。。。不知道，再看。。

低通滤波是干嘛的，不知道。。

	if(sens_hd_check.of_ok)
	{
		vel_fb_h[0] = OF_DX2;
		vel_fb_h[1] = OF_DY2;
	}
	else
	{
		vel_fb_h[0] = of_rdf.gnd_vel_est_h[X];
		vel_fb_h[1] = of_rdf.gnd_vel_est_h[Y];	
	}

如果光流ok，那么把物坐标系的速度期望值赋为，这个，否则赋为那个，是这样吧。。
然后又转坐标系：

h2w_2d_trans(vel_fb_h,imu_data.hx_vec,vel_fb_w);

把物坐标系的测量来的速度信息转化为世界坐标系。

loc_ctrl_1.fb[X] = vel_fb_w[0] + 0.03f *vel_fb_d_lpf[X];
loc_ctrl_1.fb[Y] = vel_fb_w[1] + 0.03f *vel_fb_d_lpf[Y];

对控制结构体中的反馈部分赋值。

	fb_speed_fix[0] = uwb_data.w_vel_cmps[0];
	fb_speed_fix[1] = uwb_data.w_vel_cmps[1];

速度修正，可能利用了uwb。。。
然后用了两个pid，一个结果加一个修正值，结果pos_ctrl_w_out[i]等于两个值相加

w2h_2d_trans(pos_ctrl_w_out,imu_data.hx_vec,pos_ctrl_h_out); 

再把得到的结果转回物坐标系。

	loc_ctrl_1.out[0] = pos_ctrl_h_out[0];
	loc_ctrl_1.out[1] = pos_ctrl_h_out[1];	

转完输出给loc_ctrl_1.out[]。。。要记住这个loc_ctrl_1就是物坐标系

如果仅有光流：

else if(switchs.of_flow_on && (!switchs.gps_on))
{
	mode_f[1] = 1;
	if(mode_f[1] != mode_f[0])
	{
		Loc_1level_PID_Init();
		mode_f[0] = mode_f[1];
	}
	
	loc_ctrl_1.exp[X] = fs.speed_set_h[X];
	loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];
	//
	LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[X],vel_fb_d_lpf[X]);
	LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[Y],vel_fb_d_lpf[Y]);		
	
	if(sens_hd_check.of_ok)
	{
		loc_ctrl_1.fb[X] = OF_DX2 + 0.03f *vel_fb_d_lpf[X];
		loc_ctrl_1.fb[Y] = OF_DY2 + 0.03f *vel_fb_d_lpf[Y];
		
		fb_speed_fix[0] = OF_DX2FIX;
		fb_speed_fix[1] = OF_DY2FIX;
	}
	else//sens_hd_check.of_df_ok
	{
		loc_ctrl_1.fb[X] = of_rdf.gnd_vel_est_h[X] + 0.03f *vel_fb_d_lpf[X];
		loc_ctrl_1.fb[Y] = of_rdf.gnd_vel_est_h[Y] + 0.03f *vel_fb_d_lpf[Y];
		
		fb_speed_fix[0] = of_rdf.gnd_vel_est_h[X];
		fb_speed_fix[1] = of_rdf.gnd_vel_est_h[Y];		
	}
	
	for(u8 i =0;i<2;i++)
	{
		PID_calculate( dT_ms*1e-3f,            //ÖÜÆÚ£¨µ¥Î»£ºÃ룩
									loc_ctrl_1.exp[i] ,				//Ç°À¡Öµ
									loc_ctrl_1.exp[i] ,				//ÆÚÍûÖµ£¨É趨ֵ£©
									loc_ctrl_1.fb[i] ,			//·´À¡Öµ£¨£©
									&loc_arg_1[i], //PID²ÎÊý½á¹¹Ìå
									&loc_val_1[i],	//PIDÊý¾Ý½á¹¹Ìå
									50,//»ý·ÖÎó²îÏÞ·ù
									10 *flag.taking_off			//integration limit£¬»ý·ÖÏÞ·ù
									 )	;	
		
		//fix
		PID_calculate( dT_ms*1e-3f,            //ÖÜÆÚ£¨µ¥Î»£ºÃ룩
									loc_ctrl_1.exp[i] ,				//Ç°À¡Öµ
									loc_ctrl_1.exp[i] ,				//ÆÚÍûÖµ£¨É趨ֵ£©
									fb_speed_fix[i] ,			//·´À¡Öµ£¨£©
									&loc_arg_1_fix[i], //PID²ÎÊý½á¹¹Ìå
									&loc_val_1_fix[i],	//PIDÊý¾Ý½á¹¹Ìå
									50,//»ý·ÖÎó²îÏÞ·ù
									10 *flag.taking_off			//integration limit£¬»ý·ÖÏÞ·ù
									 )	;	
		
		loc_ctrl_1.out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;	//(PD)+(I)	
	}		
}

处理差不多好像。。。但是少了坐标变换。。？不太懂

接下来一个就是有gps的。。。好长的判断语句，不看了。。。

最后一个是gps、光流、uwb都没有，那就直接利用期望速度得出期望姿态角，这里比较好理解，但恐怕实际上的情况是有光流无gps、uwb，所以应该是第三段代码，但有光流实际上就是在定位。。。

所以说在实际中，我们想要前进，就改变switch。
打开of的switch，那么就可以实现定位，关闭并设置期望速度，就可以做到前进。。

在具体一点，怎么移动、物坐标系和世界坐标系等等，再看。。。