我现在自己编写了四旋翼控制程序，使用的是角速度和角度串级pid算法，如何开始调PID参数呢？下面是我控制算法的代码，control_inner是内环控制，输入的是陀螺仪数据，control_shell是外环控制，输入的是四元数姿态解算出来的姿态角，各位牛人帮忙看看有什么问题吗？
void control_inner(float x,float y,float z)
{
// Xinner.Kp=5;
Xinner.Ex=Xshell.Out-x*0.0005327/AtR;
Xinner.Eix+=Xinner.Ex;
Xinner.Out=Xinner.Kp * Xinner.Ex + \
Xinner.Ki * Xinner.Eix + \
Xinner.Kd * (mpu_gryo_data_History.X-x);
// Yinner.Kp=1;
Yinner.Ex=Yshell.Out-y*0.0005327/AtR;
Yinner.Eix+=Yinner.Ex;
Yinner.Out=Yinner.Kp * Yinner.Ex + \
Yinner.Ki * Yinner.Eix + \
Yinner.Kd * (mpu_gryo_data_History.Y-y);
// Zinner.Kp=1;
Zinner.Ex=Zshell.Out-z*0.0005327/AtR;
Zinner.Eix+=Zinner.Ex;
Zinner.Out=Zinner.Kp * Zinner.Ex + \
Zinner.Ki * Zinner.Eix + \
Zinner.Kd * (mpu_gryo_data_History.Z-z);
mpu_gryo_data_History.Y=y;
mpu_gryo_data_History.X=x;
mpu_gryo_data_History.Z=z;
}
void control_shell(float x,float y,float z)
{
// Xshell.Kp=3;
Xshell.Ex=(float)(RC_mid - rc_data.pitch)/100-x;
Xshell.Eix+=Xshell.Ex;
if(Xshell.Eix>13000)
Xshell.Eix=13000;
if(Xshell.Eix<-13000)
Xshell.Eix=-13000;
Xshell.Out=Xshell.Kp * Xshell.Ex + \
Xshell.Ki * Xshell.Eix ;
// Yshell.Kp=1;
Yshell.Ex=(RC_mid - rc_data.roll)/100-y;
Yshell.Eix+=Yshell.Ex;
if(Yshell.Eix>13000)
Yshell.Eix=13000;
if(Yshell.Eix<-13000)
Yshell.Eix=-13000;
Yshell.Out=Yshell.Kp * Yshell.Ex + \
Yshell.Ki * Yshell.Eix ;
// Zshell.Kp=1;
Zshell.Ex=(RC_mid - rc_data.yaw)/100-z;
Zshell.Eix+=Zshell.Ex;
if(Zshell.Eix>13000)
Zshell.Eix=13000;
if(Zshell.Eix<-13000)
Zshell.Eix=-13000;
Zshell.Out=Zshell.Kp * Zshell.Ex + \
Zshell.Ki * Zshell.Eix ;
}
| 
楼主
标题置顶
标题高亮
