本讲对应 Betaflight 源码
源码文件
src/main/flight/pid.c核心函数
pidLevel()核心变量
currentAngle
errorAngle本讲对应控制流程
RC（遥控器） │ ▼ Target Angle（目标姿态角） │ ▼ Current Angle（当前姿态角） │ ▼ Angle Error（姿态角误差）
本讲学习目标
阅读完本讲以后，你应该能够回答：
□ 飞控怎样知道飞机现在在哪里？
□ Current Angle（当前姿态角）是怎样得到的？
□ 为什么源码中要减去 angleTrim？
□ 为什么源码中还要除以 10？
□ Angle Error（姿态角误差）是怎样计算出来的？
□ 为什么电机仍然不能直接执行 Angle Error（姿态角误差）？
上一讲，
飞手通过遥控器打杆，
飞控首先得到：
Target Angle（目标姿态角）
它表示：
飞手希望飞机最终停留的位置。
例如：
Pitch = 30°
这只是：
飞手的目标。
但是，
飞控仍然无法控制飞机。
原因很简单：
它还不知道：
飞机现在到底在哪里？
因此，
飞控下一步必须获得：
Current Angle（当前姿态角）
在 Betaflight 中，
对应源码如下：
const float currentAngle =
(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
这一行代码，
就是：
Current Angle（当前姿态角）
的来源。
不过，
这里面其实包含了三个信息：
attitude.raw[]
↓
angleTrim->raw[]
↓
/10.0f
如果不了解它们，
这一行源码就很难真正理解。
首先来看：
attitude.raw[axis]
它表示：
飞机当前估算姿态角。
例如：
Roll（横滚角）
Pitch（俯仰角）
Yaw（偏航角）
都会保存在：
attitude.raw[]
里面。
那么，
这些数据又来自哪里？
答案是：
姿态解算（Attitude Estimation）
飞控不断读取：
Gyroscope（陀螺仪）
+
Accelerometer（加速度计）
经过姿态解算以后，
最终得到：
飞机当前姿态。
然后保存到：
attitude.raw[]
里面。
因此，
阅读源码时，
可以直接理解成：
attitude.raw[]
=
飞机当前姿态。
说明
本课程当前只讨论：
Angle Mode（自稳模式）的控制流程。
姿态解算（Mahony、Quaternion、DCM 等）属于另一套知识体系，
后续将单独安排专题讲解。
目前，
我们只需要知道：
attitude.raw[] = 飞机当前姿态。
继续看源码：
attitude.raw[axis]
-
angleTrim->raw[axis]
为什么，
还要减去：
angleTrim？
原因其实非常简单。
现实中的飞机，
安装时，
几乎不可能做到：
绝对水平。
例如：
飞控安装
↓
天然前倾 0.8°
或者：
机架安装
↓
天然左倾 1°
如果，
这些安装误差，
直接参与控制，
那么：
即使飞机放在桌子上，
飞控也会认为：
飞机已经倾斜。
显然，
这是错误的。
因此，
Betaflight 会保存：
angleTrim->raw[]
用于记录：
安装微调角。
真正参与控制之前，
需要先扣除：
安装误差。
因此：
attitude.raw
↓
减去
↓
angleTrim
得到：
真正参与控制的当前姿态。
继续看源码：
const float currentAngle =
(attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
最后还有一个：
/10.0f
很多阅读者第一次看到这里都会疑惑：
为什么要除以 10？
原因其实很简单。
虽然：
attitude.raw[]
表示：
飞机当前姿态。
但是，
它内部保存的数据单位，
并不是：
°
而是：
0.1°
例如：
Pitch = 30°
内部实际上保存的是：
300
同样：
Roll = -15°
内部保存的是：
-150
因此，
为了后续控制计算，
需要重新转换回：
°
所以：
/10.0f
就是：
0.1°
↓
°
单位转换。
经过这一行代码，
飞控最终得到：
Current Angle（当前姿态角）
它表示：
已经扣除安装误差，
并且单位已经转换为 °
的飞机当前姿态。
到这里，
飞控已经同时知道：
Target Angle（目标姿态角）
以及：
Current Angle（当前姿态角）
于是，
真正的控制，
第一次开始发生。
飞控开始计算：
飞机距离目标，
还有多远。
Betaflight 中，
对应代码如下：
const float errorAngle =
angleTarget
-
currentAngle;
也就是：
Angle Error（姿态角误差）
=
Target Angle（目标姿态角）
-
Current Angle（当前姿态角）
这是：
整个 Angle Mode（自稳模式）
最重要的一步。
假设：
飞手希望：
Pitch = 30°
所以：
Target Angle
=
30°
而此时，
飞机真正姿态只有：
Pitch = 18°
那么：
Current Angle
=
18°
因此：
Angle Error
=
30°
-
18°
=
12°
飞控得到的信息就是：
飞机距离目标，
还差：
12°
很多初学者，
容易产生一个误解：
既然已经知道：
Target Angle（目标姿态角）
为什么还要算误差？
原因就在于：
控制系统真正关心的，
不是：
目标是多少。
而是：
距离目标还有多少。
例如：
如果：
Angle Error = 0°
说明：
飞机已经到达目标。
如果：
Angle Error > 0°
说明：
飞机还需要继续旋转。
因此：
整个控制系统，
真正开始工作的变量，
不是：
Target Angle（目标姿态角）
而是：
Angle Error（姿态角误差）
看到这里，
很多阅读者都会认为：
Angle Error（姿态角误差）
已经知道了。
是不是直接交给 PID，
飞机就能够飞过去？
答案仍然是否定的。
原因非常简单：
电机不会执行：
角度。
电机真正能够执行的是：
角速度。
例如：
120°/s
而不是：
12°
所以，
飞控还必须完成一次重要的翻译：
Angle Error（姿态角误差）
↓
Desired Angular Rate（目标角速度）
只有完成这一步，
后面的
Rate PID（角速度 PID 控制器）
才能真正开始控制电机。
现在，
飞控已经完成了：
Target Angle（目标姿态角）
+
Current Angle（当前姿态角）
↓
Angle Error（姿态角误差）
这里，
Current Angle（当前姿态角）
来自：
attitude.raw[]
经过：
扣除安装误差（angleTrim）
↓
单位转换（/10.0f）
最终，
得到真正参与控制的：
Current Angle（当前姿态角）。
然后：
飞控计算：
Angle Error（姿态角误差）
=
Target Angle
-
Current Angle
但是，
Angle Error（姿态角误差）
仍然不是：
电机能够执行的控制命令。
因为：
飞机不能执行：
角度。
飞机只能执行：
角速度。
因此，
整个控制链，
还必须继续向下推进。
现在，
飞控已经知道：
Angle Error（姿态角误差）
但是，
它仍然不能直接驱动电机。
下一讲，
我们终于进入
Betaflight Angle Mode（自稳模式）
最核心的一步：
Angle Error（姿态角误差）
↓
Desired Angular Rate（目标角速度）
也就是：
飞控究竟怎样把：
还差 12°
翻译成：
请以 80°/s 的速度继续旋转。
同时，
我们也将正式进入：
float angleRate =
errorAngle * pidRuntime.angleGain
+ angleFeedforward;
这一行代码，
它正是
Angle Mode 外环控制（Outer Loop）
真正开始工作的地方。
此内容由惯性聚合(RSS阅读器)自动聚合整理，仅供阅读参考。 原文来自 — 版权归原作者所有。