搞了半天还没看到有用的东西,接下来开始看角速度环、电机控制、姿态角获取、角度环这部分,一起看是因为联系比较紧密。
下午看高度和位置部分。。。。
首先看姿态角速度环控制:
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]);
}
赋值、限幅、输出等等。。也比较简单,不浪费时间了