-
/********************
(C)
COPYRIGHT
2014
ANO
Tech
***************************
*
文件名
：
ANO_
*
描述
：飞行控制
***********
**************************************************
****
*****************/
include
ANO_FlyControl fc;
/*
先整定内环，后整定外环。
参数整定找最佳，从小到大顺序查
先是比例后积分，最后再把微分加
曲线振荡很频繁，比例度盘要放大
曲线漂浮绕大湾，比例度盘往小扳
曲线偏离回复慢，积分时间往下降
曲线波动周期长，积分时间再加长
曲线振荡频率快，先把微分降下来
动差大来波动慢。微分时间应加长
理想曲线两个波，前高后低
4
比
1
*/
/*
ROLL
和
PIT
轴向按照以上公式计算
PID
输出，
但
YAW
轴比较特殊，
因为偏航角法
线方向刚好和地球重力平行，
这个方向的角度无法由加速度计
直接测得，
需要增加一个电子罗盘来
替代加速度计。如果不使用
罗盘的话，
我们可以单纯的通过角速度积分来测得偏航角，<
/p>
缺点是由于积分环节
中存在积分漂移，偏航角随着时间的推移
会偏差越来越大。
我们不使用罗盘就没有
比例项，
只仅使用微分环节
来控制。
*/
ANO_FlyControl::ANO_FlyControl()
{
yawRate = 120;
//
重置
PID
参数
< br>
PID_Reset();
}
//
重置
P
ID
参数
void
ANO_FlyControl:
{
//
因为
YAW
角度会漂移，所以参数和
ROLL
、
PITCH
不一样
pid[PIDROLL].set_pid(70, 15,
120, 2000000); //ROLL
角度的内环
控制
系数
,20000:
积分上限
pid[PIDPITCH].set_pid(70,
30,
120,
2000000)
;//PITCH
角度的内
环控制系数
pid[PIDYAW].set_pid(100,
50, 0, 2000000);
//YAW
角度的内环控
制系数
pid[PIDLEVEL].set_pid(280,
0, 0, 0); //
外环控制系数
pid[PIDMAG].set_pid(15, 0,
0, 0); //
电子罗盘控制系数
}
/*
【扫盲知识】
ID_Reset(void)
串
级
PID
：采用的角度
P
和角速度
PID
的双闭环
P
ID
算法
------>
角
度的误差被作为期望输入到角速度控制器中
（角度的微分就是角速
度）
对于本系统则采用了将角度控制与角速度控制级联的方式组成整个
串级
PID
控制器。
串级
PID
算法中，角速度内环占着极为重要的地位。在对四旋翼飞
行的物理模型进
行分析后，
可以知道造成系统不稳定的物理表现之
一就是不稳定的角
速度。
因此，
若能够直接对系统的角速度进行较好的闭环控制，
必然会改善
系统的动态特性
及其稳定性，
通常也把角速度内环称为增稳环节。
而角度外环的作用
则体现在对四旋翼飞
行器的姿态角的精确控制。
外环：
输入为角度
,
输出为角速度
内环：输入为角速度，输出为
PWM
增量
使用串级
pid
，
分为：角度环控制
pid
环，和角速度控制环稳定环。
主调为角度环（外环）
，副调为角速度环（内环）
。
参数整定原则为先内后外，故在整定内环时将外环的
PID
均设为
0
所谓外环就是只是一个
P
在起作用，
也
就是比例在起作用；
P
也就是
修正力度
，越大越容易使飞机震荡。
震荡的特点是：频率小、幅度大
*/
/*
【横滚（
Roll
）和俯仰（
Pitch
）
的控制算法】
横滚（
Roll
）和俯仰（
Pitch
）的控制算法是一样
的，控制参数也比较
接近。
首先得到轴姿态的角度差（
angle error
）
，将这个值乘以角度系数
p
后限幅（限幅必须有，否则剧烈打舵时容易引发震荡）作为角速度控
制器期望值
（
target_rate
）
。
target_rate
与陀螺仪得到的当前角速
度
作差，得到角速度误差（
rate_error
）乘以
kp
得到
P
。在
I
值小于限
幅值（
这个值大概在
5%
油门）或者
rate
_error
与
i
值异号时将
rate_error
累加到
I
中。
前后两次
rate_error
< br>的差作为
D
项，
值得注
意的是加需要入
20hz
（也可以采用其它合适
频率）滤波，以避免震
荡。将
P
,I,
D
三者相加并限幅（
50%
油门）得到
最终
PID
输出。
*/
//
串环
PID
调节详情参见：
/supe
r_mic
...
36723
//
飞行器姿态外环控制
void
ANO_FlyControl::Attitude_Outter_Loop(void)
{
int32_t errorAngle[2];
Vector3f Gyro_ADC;
//
计算角度误差值
,
角度误差值
=
期望值
-
此刻姿态值
//constrain_
int32
作用：
32
位整型数限幅，
使其控制输入的最大飞
行倾角不大于
25
度（如果控制量比
25
度大，飞机早就坠毁了）
//d[ROLL]
：遥控数据
.x
：此刻姿态
(
< br>角度
)
//1.
得到轴姿态的角度差（
errorAngle
）<
/p>
//2.
这
个
角
度
差
值
进
行
限
幅
(constrain_int32)
（
正
负
FLYANGLE_MAX
）
（限幅必须有，
否则剧烈打舵时容易引发震荡）
作
为角速度控制器期望值（
target_rate
）
errorAngle[ROLL] =
constrain_int32((d[ROLL] * 2) ,
-((int)FLYANGLE_MAX), +FLYANGLE_MAX) -
.x * 10;
errorAngle[PITCH] =
constrain_int32((d[PITCH] * 2) ,
-((int)FLYANGLE_MAX), +FLYANGLE_MAX) -
.y * 10;
//
获取此时
陀螺仪上的角速度，取角速度的四次平均值
Gyro_ADC = _Gyro() / 4;
/*
得到外环
PID
输出（角速度的差值）
(
实质是相当于内环的
P
比例
项
)-------->
< br>_rate
与陀螺仪得到的当前角速度作差，得到角速度误差
（
RateError
）
乘以
kp
（外环控制系数
pid[PIDLEVEL]--->(280, 0, 0,
0)
）得到给内环的
P
。
*/
//
横滚
roll
：
外环
控制。
输入为角度
,
输出为角速度。<
/p>
RateError[ROLL]
作为内环的输入。
RateError[ROLL]
=
pid[PIDLEVEL].get_p(errorAngle[ROLL])
-
Gyro_ADC.x; //Gyro_ADC.x:<
/p>
陀螺仪
X
轴的值
//
俯
仰<
/p>
pitch
：
外
环
控
制
。
输<
/p>
入
为
角
度
,
输
出
为
角
速
度
。
RateError[PITCH]
作为内环的输入。
RateError[PITCH]
=
pid[PIDLEVEL].get_p(errorAngle[PITCH])
-
Gyro_ADC.y;//Gyro_ADC.y:
陀螺仪
Y
轴的值
/*
偏航（
Yaw
）的控制算法和前两者略有不同，是将打舵量（遥控数据