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

姿态角获取，这里的RPY应该是rollpitch和yaw

calculate_RPY();

看函数定义：

void calculate_RPY()
{
	///输出姿态角///
	
		t_temp = LIMIT(1 - my_pow(att_matrix[2][0]),0,1);
		
		//imu_data.pit = asin(2*q1q3 - 2*q0q2)*57.30f;
	
		if(ABS(imu_data.z_vec[Z])>0.05f)//避免奇点的运算
		{
			imu_data.pit =  fast_atan2(att_matrix[2][0],my_sqrt(t_temp))*57.30f;
			imu_data.rol =  fast_atan2(att_matrix[2][1], att_matrix[2][2])*57.30f; 
			imu_data.yaw = -fast_atan2(att_matrix[1][0], att_matrix[0][0])*57.30f; 
		}
}

总之就是得到自身姿态。

姿态角度环控制： Att_2level_Ctrl(6e-3f,CH_N);

exp_rol_tmp = - loc_ctrl_1.out[Y];
exp_pit_tmp = - loc_ctrl_1.out[X];

把location环的输出，赋给期望滚转和俯仰的暂存值。

if(flag.flight_mode == ATT_STAB)
{
	if(ABS(exp_rol_tmp + att_2l_ct.exp_rol_adj) < 5)
	{
		att_2l_ct.exp_rol_adj += 0.2f *exp_rol_tmp *dT_s;
		att_2l_ct.exp_rol_adj = LIMIT(att_2l_ct.exp_rol_adj,-1,1);
	}
	
	if(ABS(exp_pit_tmp + att_2l_ct.exp_pit_adj) < 5)
	{
		att_2l_ct.exp_pit_adj += 0.2f *exp_pit_tmp *dT_s;
		att_2l_ct.exp_pit_adj = LIMIT(att_2l_ct.exp_pit_adj,-1,1);
	}
}
else
{
	att_2l_ct.exp_rol_adj = 
	att_2l_ct.exp_pit_adj = 0;
}

如果状态为ATT_STAB，即状态稳定，（可能是自稳的意思？）那么如果这个括号里的东西的绝对值小于五，就对这个adj加东西，否则，adj清零。
这里不知道在干嘛，注释就写了一个积分微调，+=确实是积分操作，adj猜测是调整的意思，但还是没看懂在干什么。。。

att_2l_ct.exp_rol = exp_rol_tmp + att_2l_ct.exp_rol_adj;
att_2l_ct.exp_pit = exp_pit_tmp + att_2l_ct.exp_pit_adj;
att_2l_ct.exp_rol = LIMIT(att_2l_ct.exp_rol,-MAX_ANGLE,MAX_ANGLE);
att_2l_ct.exp_pit = LIMIT(att_2l_ct.exp_pit,-MAX_ANGLE,MAX_ANGLE);

对期望角速度赋值，总之是得到了一个期望角速度。

if(flag.speed_mode == 3)
{
	max_yaw_speed = MAX_SPEED_YAW;
}
else if(flag.speed_mode == 2 )
{
	max_yaw_speed = 220;
}
else 
{
	max_yaw_speed = 200;
}

设置最大yaw速度，也就是航向角变化速度，虽然不知道干嘛用。。。

fc_stv.yaw_pal_limit = max_yaw_speed;

set_yaw_av_tmp = (s32)(0.0023f *my_deadzone(CH_N[CH_YAW],0,65) *max_yaw_speed) + (-program_ctrl.yaw_pal_dps) + pc_user.pal_dps_set;


set_yaw_av_tmp = LIMIT(set_yaw_av_tmp ,-max_yaw_speed,max_yaw_speed);

把摇杆量、program、user量转换为期望角速度量？？

if(flag.taking_off==0)//if(flag.locking)
{
	att_2l_ct.exp_rol = att_2l_ct.exp_pit = set_yaw_av_tmp = 0;
	att_2l_ct.exp_yaw = att_2l_ct.fb_yaw;
}

如果没有起飞，清零

if(att_2l_ct.yaw_err>90)
{
	if(set_yaw_av_tmp>0)
	{
		set_yaw_av_tmp = 0;
	}
}
else if(att_2l_ct.yaw_err<-90)
{
	if(set_yaw_av_tmp<0)
	{
		set_yaw_av_tmp = 0;
	}
}	

限制error增大的，这里的和上面的av是啥意思。。。还没看懂，不然就会比较好理解了。

att_1l_ct.set_yaw_speed += LIMIT((set_yaw_av_tmp - att_1l_ct.set_yaw_speed),-30,30);

att_2l_ct.exp_yaw += att_1l_ct.set_yaw_speed *dT_s;

if(att_2l_ct.exp_yaw<-180) att_2l_ct.exp_yaw += 360;
else if(att_2l_ct.exp_yaw>180) att_2l_ct.exp_yaw -= 360;	


att_2l_ct.yaw_err = (att_2l_ct.exp_yaw - att_2l_ct.fb_yaw);

if(att_2l_ct.yaw_err<-180) att_2l_ct.yaw_err += 360;
else if(att_2l_ct.yaw_err>180) att_2l_ct.yaw_err -= 360;

计算了yaw角度误差，这里是把反馈yaw角赋为0，即当前角度，然后期望yaw角度为想要转过的角度。

    att_2l_ct.fb_yaw = imu_data.yaw ;
		
	att_2l_ct.fb_rol = (imu_data.rol ) ;
	att_2l_ct.fb_pit = (imu_data.pit ) ;

第一行是没用的，因为没用到反馈yaw角度。

PID_calculate( dT_s,          
									0 ,				
									att_2l_ct.exp_rol ,			
									att_2l_ct.fb_rol ,			
									&arg_2[ROL], 
									&val_2[ROL],	
                 
									5 *flag.taking_off			
									 )	;
									
PID_calculate( dT_s,            
									0 ,			
									att_2l_ct.exp_pit ,				
									att_2l_ct.fb_pit ,		
									&arg_2[PIT], 
									&val_2[PIT],	
              
									5 *flag.taking_off		
									 )	;

PID_calculate( dT_s,            
									0 ,			
									att_2l_ct.yaw_err ,		
									0 ,		
									&arg_2[YAW], 
									&val_2[YAW],	
                 
									5 *flag.taking_off			
									 )	;

对三个角做pid，得到角速度环的期望角速度。
同时要注意这里pitch和roll的期望值是根据定位环的输出得来的。
所以接下来就看定位环了。。。