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

搞了半天还没看到有用的东西，接下来开始看角速度环、电机控制、姿态角获取、角度环这部分，一起看是因为联系比较紧密。
下午看高度和位置部分。。。。

首先看姿态角速度环控制：

Att_1level_Ctrl(2*1e-3f);

进入函数，开头有一个这个

ctrl_parameter_change_task();

注释说是改变控制参数任务，应该是用户进行pid参数调整的一个入口？回来再仔细看。

 for(u8 i = 0;i<3;i++)
{
	att_1l_ct.exp_angular_velocity[i] = val_2[i].out;// val_2[i].out;//
}

目标角速度，或者说期望角速度赋值，这个值val_2[i]，实际上就是二环，也就是姿态角度环的输出。具体来讲，对于给定的状态，可以是静止，可以是前进，如果是静止，那么姿态角为零，如果是前进，俯仰角保持一定角度。总之就是为了满足给定的状态必须要保持一个一定的姿态，也就是我们想要达到的姿态，也就是期望角度，如果我们能测得当前角度，就可以对两者利用pid得到增量，也就是计算出我们需要多大的角速度。
得到了我们需要的角速度，也就是我们要达到的角速度，即期望角速度，如果已知现在的角速度，那么可以再利用pid，得到角速度的增量，即角加速度，也就是电机输出。

	att_1l_ct.exp_angular_velocity[ROL] = LIMIT(att_1l_ct.exp_angular_velocity[ROL],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);
	att_1l_ct.exp_angular_velocity[PIT] = LIMIT(att_1l_ct.exp_angular_velocity[PIT],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);

期望角速度限幅，因为不能让它很大。

att_1l_ct.fb_angular_velocity[ROL] = ( sensor.Gyro_deg[X] );
att_1l_ct.fb_angular_velocity[PIT] = (-sensor.Gyro_deg[Y] );
att_1l_ct.fb_angular_velocity[YAW] = (-sensor.Gyro_deg[Z] );

反馈角速度赋值，也就是陀螺仪测得的当前角速度值。

 for(u8 i = 0;i<3;i++)
 {
		PID_calculate( dT_s,           
										0,			
										att_1l_ct.exp_angular_velocity[i],			
										att_1l_ct.fb_angular_velocity[i],			
										&arg_1[i], 
										&val_1[i],
										CTRL_1_INTE_LIM *flag.taking_off			
										 )	; 
 
	
	 ct_val[i] = (val_1[i].out);
 }

pid计算，并把结果赋给ct_val[i]

mc.ct_val_rol =                   FINAL_P *ct_val[ROL];
mc.ct_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];
mc.ct_val_yaw =                   FINAL_P *ct_val[YAW];

注释写的是：赋值，最终比例调节。这里的FINAL_P 是电机输出控制系数。

mc.ct_val_rol = LIMIT(mc.ct_val_rol,-1000,1000);
mc.ct_val_pit = LIMIT(mc.ct_val_pit,-1000,1000);
mc.ct_val_yaw = LIMIT(mc.ct_val_yaw,-400,400);	

然后限幅，没了。。。

反正思路很简单也很明了，接下来看电机输出控制：

Motor_Ctrl_Task(u8 dT_ms)

if(flag.unlock_sta)
{		
	IDLING = 10*LIMIT(Ano_Parame.set.idle_speed_pwm,0,30);
	
	if(flag.motor_preparation == 0)
	{
		motor_prepara_cnt += dT_ms;
		
		if(flag.motor_preparation == 0)
		{			
			if(motor_prepara_cnt<300)
			{
				motor[m1] = IDLING;
			}
			else if(motor_prepara_cnt<600)
			{
				motor[m2] = IDLING;
			}
			else if(motor_prepara_cnt<900)
			{
				motor[m3] = IDLING;
			}	
			else if(motor_prepara_cnt<1200)
			{	
				motor[m4] = IDLING;
			}
			else
			{
				flag.motor_preparation = 1;
				motor_prepara_cnt = 0;
			}
		}
		
	}	
}

如果解锁，IDLING（空挡？）赋值，然后接下来看不懂了。。。应该是确定点击准备好。

else
{
	flag.motor_preparation = 0;
}

否则电机没准备好

	if(flag.motor_preparation == 1)
	{	
		motor_step[m1] = mc.ct_val_thr  +mc.ct_val_yaw -mc.ct_val_rol +mc.ct_val_pit;
		motor_step[m2] = mc.ct_val_thr  -mc.ct_val_yaw +mc.ct_val_rol +mc.ct_val_pit;
		motor_step[m3] = mc.ct_val_thr  +mc.ct_val_yaw +mc.ct_val_rol -mc.ct_val_pit;
		motor_step[m4] = mc.ct_val_thr  -mc.ct_val_yaw -mc.ct_val_rol -mc.ct_val_pit;
		
	
		for(i=0;i<MOTORSNUM;i++)
		{	
			motor_step[i] = LIMIT(motor_step[i],IDLING,1000);
//			motor_lpf[i] += 0.5f *(motor_step[i] - motor_lpf[i]) ;		
			
		}
	}



for(i=0;i<MOTORSNUM;i++)
{
	if(flag.unlock_sta)
	{
		if(flag.motor_preparation == 1)
		{
			motor[i] = LIMIT(motor_step[i],IDLING,999);
		}

	}
	else
	{		
		motor[i] = 0;
	}	

}

//ÅäÖÃÊä³ö
for(u8 i =0;i<4;i++)
{
	Drv_MotorPWMSet(i,motor[i]);
}

赋值、限幅、输出等等。。也比较简单，不浪费时间了