二部分 四旋翼PID控制理论
此部分晦涩难懂，而且每个人采取的方式也不一样，笔者是两年之前做的,现在写的也只能作为参考，具体哪种控制方式好可以多方面参考资料，针对自己的实际情况进行调试。笔者当年做也是困在了这个环节，将一般的控制理论搞了个差不多，但是参数和算法的调试没有到位，因为有其他项目后期就再也没有继续调试，但是感觉已经学习到了大部分的知识，再调试有点耽误时间，看当时其他网友说可能一个月也调不好参数，因为涉及的参数众多关系复杂，而且调试过程复杂有一定的风险（说不定会被螺旋桨削去，，，注意安全）
四旋翼采用的是内环外环PID，有两层。不考虑气压传感器等其他传感器，只考虑将遥控器数据和MPU6050数据作为系统输入变量，将四个PWM值作为输出变量。遥控器采用四通道遥控器，除了油门外，另外三个控制三个角度，三个角度摇杆所处的位置就代表四旋翼的目标角度。油门作为电机转速的基础量，控制电机转速时候要在油门给的电机转速的基础上进行加减，但是要注意加减的比例和对pwm占空比的限幅，防止电机疯掉。一开始做无人机可能就会被这几个变量如何融合的问题所困扰，当时经过一段时间的查找资料最终搞明白了。
这是目标角度的计算公式，pwmout的几个变量是单片机捕获的遥控器接收机的信号，范围是1000-2000
target_X=(pwmout1-1502)/20;
target_Y=(pwmout2-1502)/20;
target_Z=(pwmout4-1502)/20;
这是内外环pid计算公式，其中外环是角度环，内环是角速度环，先有的目标角度，后有的目标角速度，外环的输出作为内环的输入。内环的输出就是电机转速的差值，依靠此差值分两个电机一组进行控制前后左右移动
///////////////////////////Íâ»·½Ç¶È»·PID/////////////////////////////////////////////////////////
err_OX=target_X-Q_ANGLE.X;
Pitch_out_i+=err_OX;
//------------------------Pitch»ý·ÖÏÞ·ù-------------------------------------------------//
if(Pitch_out_i>300) Pitch_out_i=300;
else if(Pitch_out_i<-300) Pitch_out_i=-300;
//------------------------Pitch»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Pitch_shell_OUT=Pitch_shell_P*err_OX+Pitch_shell_I*Pitch_out_i+Pitch_shell_D*(err_OX-err_LAST_OX);
err_LAST_OX=err_OX; //±£´æÉÏÒ»´ÎµÄÖµ
err_OY=target_Y-Q_ANGLE.Y;
Roll_out_i+=err_OY;
//------------------------Roll»ý·ÖÏÞ·ù-------------------------------------------------//
if(Roll_out_i>300) Roll_out_i=300;
else if(Roll_out_i<-300) Roll_out_i=-300;
//------------------------Roll»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Roll_shell_OUT=Roll_shell_P*err_OY+Roll_shell_I*Roll_out_i+Roll_shell_D*(err_OY-err_LAST_OY);
err_LAST_OY=err_OY; //±£´æÉÏÒ»´ÎµÄÖµ
Q_ANGLE.Z=0;
err_OZ=target_Z-Q_ANGLE.Z;
YAW_out_i+=err_OZ;
//------------------------YAW»ý·ÖÏÞ·ù-------------------------------------------------//
if(YAW_out_i>300) YAW_out_i=300;
else if(YAW_out_i<-300) YAW_out_i=-300;
//------------------------YAW»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
YAW_shell_OUT=YAW_shell_P*err_OZ+YAW_shell_I*YAW_out_i+YAW_shell_D*(err_OZ-err_LAST_OZ);
err_LAST_OZ=err_OZ; //±£´æÉÏÒ»´ÎµÄÖµ
//////+++++++++++++++//////ÄÚ»·½ÇËٶȻ·PID////+++++++++++++++++++++++////////////////////////////
err_IX=Pitch_shell_OUT-MPU6050_ACC_LAST.X;
Pitch_in_i+=err_IX;
//------------------------Pitch»ý·ÖÏÞ·ù-------------------------------------------------//
if(Pitch_in_i>300) Pitch_in_i=300;
else if(Pitch_in_i<-300) Pitch_in_i=-300;
//------------------------Pitch»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Pitch_core_OUT=Pitch_core_P*err_IX+Pitch_core_I*Pitch_in_i+Pitch_core_D*(err_IX-err_LAST_IX)-30;
err_LAST_IX=err_IX; //±£´æÉÏÒ»´ÎµÄÖµ
err_IY=Roll_shell_OUT-MPU6050_ACC_LAST.Y;
Roll_in_i+=err_IY;
//------------------------Roll»ý·ÖÏÞ·ù-------------------------------------------------//
if(Roll_in_i>300) Roll_in_i=300;
else if(Roll_in_i<-300) Roll_in_i=-300;
//------------------------Roll»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
Roll_core_OUT=Roll_core_P*err_IY+Roll_core_I*Roll_in_i+Roll_core_D*(err_IY-err_LAST_IY);
err_LAST_IY=err_IY; //±£´æÉÏÒ»´ÎµÄÖµ
err_IZ=YAW_shell_OUT-MPU6050_ACC_LAST.Z;
YAW_in_i+=err_IZ;
//------------------------YAW»ý·ÖÏÞ·ù-------------------------------------------------//
if(YAW_in_i>300) YAW_in_i=300;
else if(YAW_in_i<-300) YAW_in_i=-300;
//------------------------YAW»ý·Ö΢·ÖÊä³ö----------------------------------------------------//
YAW_core_OUT=YAW_core_P*err_IZ+YAW_core_I*YAW_in_i+YAW_core_D*(err_IZ-err_LAST_IZ);
err_LAST_IZ=err_IZ; //±£´æÉÏÒ»´ÎµÄÖµ
电机PWM输出的重要公式，其中pwmout3是油门的占空比采集值
if(pwmout3<1010)
{
c=0;
PWM1=c;
PWM2=c;
PWM3=c;
PWM4=c;
}
if(pwmout3>=1010)
{
PWM1=pwmout3+Pitch_shell_OUT-Roll_shell_OUT+YAW_shell_OUT;
PWM2=pwmout3+Pitch_shell_OUT+Roll_shell_OUT-YAW_shell_OUT;
PWM3=pwmout3-Pitch_shell_OUT+Roll_shell_OUT+YAW_shell_OUT;
PWM4=pwmout3-Pitch_shell_OUT-Roll_shell_OUT-YAW_shell_OUT;
}
