代码解读五 文件名“Ano_LocCtrl.c”

关于这个位置速度环我还不是很理解，因为单凭这一个文件确实看不出来什么东西，这并不像角度环和角速度环一样有丰富的理论支撑，至少我现在还没看到，可能是我水平不够额，但这并不妨碍我们继续往前走，试着往下看吧，万一有收获呢？

1. 首先进入 文件名‘’Ano_Scheduler.c”中的这个函数，如下

static void Loop_100Hz(void)	//10ms执行一次
{
			test_rT[0]= GetSysTime_us ();
//				
	/*遥控器数据处理*/
	RC_duty_task(10);
	
	/*飞行模式设置任务*/
	Flight_Mode_Set(10);
	
	/*获取姿态角（ROLL PITCH YAW）*/
	calculate_RPY();
	
	/*姿态角度环控制*/
	Att_2level_Ctrl(10e-3f,CH_N);
	
	/*位置速度环控制（暂无）*/
	Loc_1level_Ctrl(10,CH_N);
	
	/*高度数据融合任务*/
	WCZ_Fus_Task(10);
	
	/*高度速度环控制*/
	Alt_1level_Ctrl(10e-3f);
	
	/*高度环控制*/
	Alt_2level_Ctrl(10e-3f);
		
	/*光流数据解析*/
	AnoOF_DataAnl_Task(10);
	
	/*灯光控制*/	
	LED_Task(10);
//		
			test_rT[1]= GetSysTime_us ();
			test_rT[2] = (u32)(test_rT[1] - test_rT[0]) ;	
				
}

很明显可以看到暂无这两个字，但是从我分析源代码的过程中却感觉到这个位置速度环和光流且和高度环也有关系，有什么关系？接着往下看

2. 贴出 文件名“Ano_LocCtrl.c” 的源代码，已经加了一些注释，也不知道对不对，凑合着看吧

#include "Ano_LocCtrl.h"		//位置控制，一直看成逻辑控制 =_=
#include "Ano_Imu.h"
#include "Ano_FlightCtrl.h"
#include "Ano_OF.h"
#include "Ano_Parameter.h"



//这个位置速度环是干嘛的？看起来和光流有关？
//位置控制和高度控制好像联系很紧密？


//位置速度环控制参数
_PID_arg_st loc_arg_1[2] ; 

//位置速度环控制数据
_PID_val_st loc_val_1[2] ; 



//为什么偏偏这个需要修正？

//位置速度环修正控制参数
_PID_arg_st loc_arg_1_fix[2] ; 

//位置速度环修正控制数据
_PID_val_st loc_val_1_fix[2] ; 




/*位置速度环PID参数初始化*/
void Loc_1level_PID_Init()
{
	loc_arg_1[X].kp = Ano_Parame.set.pid_loc_1level[KP];//0.22f  ;
	loc_arg_1[X].ki = 0.0f  ;
	loc_arg_1[X].kd_ex = 0.00f ;
	loc_arg_1[X].kd_fb = Ano_Parame.set.pid_loc_1level[KD];
	loc_arg_1[X].k_ff = 0.02f;
	
	loc_arg_1[Y] = loc_arg_1[X];
//fix	
	loc_arg_1_fix[X].kp = 0.0f  ;
	loc_arg_1_fix[X].ki = Ano_Parame.set.pid_loc_1level[KI] ;
	loc_arg_1_fix[X].kd_ex = 0.00f;
	loc_arg_1_fix[X].kd_fb = 0.00f;
	loc_arg_1_fix[X].k_ff = 0.0f;
	
	loc_arg_1_fix[Y] = loc_arg_1_fix[X];
}




//这两个结构体很重要，一方面为下面PID函数提供期望值和反馈值
//另一方面和高度环有明显关系，因为其期望值和反馈值都应用于高度环串级PID中
//具体在高度环文件中介绍
_loc_ctrl_st loc_ctrl_1;
_loc_ctrl_st loc_ctrl_2;

static float fb_speed_fix[2];




//下面所有仅用到了loc_ctrl_1而没用到loc_ctrl_2，至于为什么，本人并不清楚
//但是，这两个结构体都应用到了高度环中，而且请注意PID中前馈也不再是0了
/*位置速度环*/
void Loc_1level_Ctrl(u16 dT_ms,s16 *CH_N)
{
	//如果打开光流
	if(switchs.of_flow_on)
	{
		//把光流的东西都拿了过来
		loc_ctrl_1.exp[X] = fs.speed_set_h[X];	//X = 0 / Y = 1
		loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];

		loc_ctrl_1.fb[X] = OF_DX2;
		loc_ctrl_1.fb[Y] = OF_DY2;
		
		fb_speed_fix[0] = OF_DX2FIX;
		fb_speed_fix[1] = OF_DY2FIX;
		
		for(u8 i =0;i<2;i++)
		{
			
			//主PID 
			//输入参数两个结构体倒很常规，和以往一样，但是期望和反馈却调用的是光流中的值
			//匿名光流确实是有点特殊，但是由于本人并不了解光流所以就不深究了，有兴趣的研究下吧
			//如上 X = 0 / Y = 1 正好分别对应这个循环中的i值，但是为什么是两个PID？
			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，积分限幅
										 )	;	
			
			//修正PID
			//=_= 如上，为什么要修正？且为什么偏偏这个需要修正？
			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;		
		}		


	}
	else
	{
		//如果没打开光流就算了
		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] ;
	}
}

这个文件很短，可能确实如开头说的，没有用上，而且我也没理解这个位置速度环应用在哪里?因为那个角度环和角速度环用于三个遥控器通道的PID控制，高度环和高度速度环应用于油门通道的PID控制，如果这个位置速度环应用于比较复杂的控制中的话，我觉得可以放一放，先实现基本功能，以后有空的话可以来研究下，或者是可以从高度环中找到答案呢？