四轴飞行器飞控编写教程

longqiu_LQ

1341人浏览 · 2026-04-02 18:04:48

longqiu_LQ · 2026-04-02 18:04:48 发布

四轴飞行器飞控编写教程

写在前面

这份教程专门为零基础的初学者编写。如果你刚接触四轴飞行器不知道从何下手，听说过PID控制但不理解它是怎么工作的，看过飞控代码但感觉像天书一样看不懂，想自己动手写飞控但不知道从哪里开始——那么这份教程就是为你准备的。

我们只讲飞控算法，不涉及硬件底层。也就是说，我们不会去讲寄存器配置、PWM波形生成、中断优先级设置等内容，而是专注于飞控是怎么想的、怎么计算的、怎么控制飞机的。我们的目标是让你理解：飞机为什么能稳定飞行，以及如何让它稳定飞行。

建议按照章节顺序阅读，因为后面的内容会用到前面的知识。如果某个地方不理解，可以多读几遍，或者跳过去看后面的内容，有时候后面的例子会帮你理解前面的概念。

第一章：飞控系统概述

飞控是做什么的？

飞控，全称"飞行控制器"，是四轴飞行器的"大脑"。

想象一下，如果让你手动控制四个电机，让飞机稳稳地悬停在空中，你能做到吗？几乎不可能！因为你需要同时控制四个电机的转速，需要根据飞机的姿态实时调整，需要对外界扰动（如风）做出快速反应，而且需要在毫秒级别完成所有计算和调整。这就是飞控的工作——它以每秒200次甚至更高的速度，不断地读取传感器数据、计算姿态、调整电机，让飞机稳定飞行。

用一句话概括飞控的工作：让飞机从"当前姿态"变成"期望姿态"，并保持稳定。

举个例子：当你推动遥控器想让飞机向前飞时，"期望姿态"就是飞机低头。飞控会读取传感器发现当前是水平的，然后计算需要让飞机低头，于是调整电机让前面两个转速降低、后面两个升高，飞机就开始低头向前飞了。这个过程中，飞控一直在工作，不断调整，确保飞机按照你的指令飞行。

飞控系统的四大模块

飞控系统可以分为四个核心模块：传感器模块负责感知飞机当前状态，姿态解算模块把原始数据转换成我们容易理解的角度，PID控制模块根据期望和现实的差距计算修正方案，电机输出模块把修正方案分配给四个电机。

飞控系统架构

传感器就像是飞机的"感觉器官"，它告诉飞控飞机当前是水平的还是倾斜的、是在上升还是下降、转了多少角度。姿态解算模块把传感器给出的原始数据（加速度、角速度）转换成我们容易理解的角度。PID控制器根据"期望姿态"和"当前姿态"的差距，计算出需要怎么调整电机。最后，电机输出模块把修正量转换成四个电机的具体转速。

把这四个模块串起来就是飞控的完整工作流程：首先读取传感器数据，然后进行姿态解算得到当前姿态角度，接着读取遥控器数据得到期望姿态，计算误差后通过PID算出修正量，最后通过电机混控输出给四个电机。这个循环每5毫秒执行一次，也就是每秒执行200次！

第二章：姿态与坐标系

什么是姿态？

姿态就是飞机在空中的"姿势"，就像人可以站、坐、躺一样，飞机也有不同的姿态。描述飞机姿态需要用三个角度，专业术语叫欧拉角。

Pitch俯仰角：描述的是飞机低头和抬头的动作。想象你站在原地，低头看地面时俯仰角为负，抬头看天空时俯仰角为正，平视前方时俯仰角为零。对飞机来说，俯仰角为负意味着机头向下飞机会向前飞，俯仰角为正意味着机头向上飞机会向后飞。

Roll横滚角：描述的是飞机左右倾斜。想象你站着，向左歪着站时横滚角为负，向右歪着站时横滚角为正，站得笔直时横滚角为零。对飞机来说，横滚角为负意味着左低右高飞机会向左飞，横滚角为正意味着左高右低飞机会向右飞。

Yaw偏航角：描述的是飞机左右转向。想象你原地转身，向左转时偏航角为负，向右转时偏航角为正，面向前方时偏航角为零。对飞机来说，偏航角变化意味着机头转向，飞机改变朝向。

用一张图来理解三个轴的关系：想象你坐在飞机里，Pitch就像点头（机头上下），Roll就像歪头（左右倾斜），Yaw就像转身（左右转向）。

四轴飞行器的电机布局

四轴飞行器有四个电机，呈X形布局。从上往下看，左前是M2，右前是M1，左后是M3，右后是M4。其中M1和M3顺时针旋转，M2和M4逆时针旋转——注意对角电机的转向相同，这样可以抵消反扭矩，让飞机不会乱转。

在这里插入图片描述

电机控制原理

这里是飞控的最重要的部分。四个电机转速相同，飞机就水平悬停；四个电机转速不同，飞机就倾斜或转向。通过让某些电机转得快一些、某些慢一些，就能控制飞机的姿态。

向前飞需要让飞机低头，原理是前面两个电机转速降低、后面两个升高。当前面升力小、后面升力大时，飞机就会低头向前飞。

向左飞需要让飞机左倾，原理是左边两个电机转速降低、右边两个升高。当左边升力小、右边升力大时，飞机就会左倾向左飞。

转向需要利用反扭矩原理。因为对角电机转向相同，改变对角电机的转速差就能产生转向力矩。想让飞机左转（逆时针），就让顺时针转的M1、M3转速升高、逆时针转的M2、M4转速降低，反扭矩会使飞机逆时针转。

动作	M1(右前)	M2(左前)	M3(左后)	M4(右后)	效果
悬停	基准	基准	基准	基准	保持水平
向前飞	降低	降低	升高	升高	飞机低头向前
向左飞	升高	降低	降低	升高	飞机左倾向左
左转	升高	降低	升高	降低	飞机逆时针转

这个表，就是飞控要做的事情。飞控就是根据遥控器的指令，计算出需要调整哪些电机转速、怎么调整，然后输出给四个电机。

第三章：传感器与姿态解算

认识IMU传感器

IMU是飞控的"眼睛"和"耳朵"，它包含两个核心传感器：加速度计和陀螺仪。

加速度计测量三个方向的加速度，包括重力加速度。把手机平放在桌面上，加速度计会测到Z轴有一个约9.8 m/s²的值（重力）；把手机立起来，这个值会转移到Y轴或X轴。在飞控中，加速度计告诉我们"哪里是下方"，通过重力方向可以推算出飞机的倾斜角度。但加速度计有个缺点：它会被振动干扰，电机振动、空气扰动都会影响测量值，而且飞机运动时测量值会叠加运动加速度，变得不准确。

陀螺仪测量三个轴的角速度（旋转速度），单位是度/秒。你原地转身，陀螺仪会测到一个旋转速度，转得越快测量值越大。在飞控中，陀螺仪告诉我们"飞机转得有多快"，对角速度积分可以得到旋转角度。但陀螺仪也有缺点：它有"漂移"，积分会累积误差，时间长了算出来的角度会越来越不准。

为什么需要姿态解算？

你可能会问：有了加速度计和陀螺仪，不是就能算出角度了吗？问题在于，两者单独使用都不够可靠。加速度计长期稳定不漂移，但短期噪声大、受振动干扰；陀螺仪短期准确响应快，但长期漂移、积分误差累积。

解决方案是融合两者的优点：用陀螺仪提供短期准确的数据，用加速度计修正陀螺仪的漂移。这就是姿态解算算法的工作。

第四章：Mahony姿态解算算法（重点）

这一章是整个姿态解算的核心，内容会稍微深入一些。

为什么用四元数而不用欧拉角？

在讲Mahony算法之前，先回答一个关键问题：为什么我们不直接用欧拉角，而要用四元数？

欧拉角虽然直观（pitch、roll、yaw三个角度），但存在一个严重问题：万向节死锁（Gimbal Lock）。当飞机旋转到某些特定角度时，欧拉角的计算会出现奇异点，导致姿态解算失效。

举个通俗的例子：想象你拿着一个指南针，当指南针水平放置时，它能正确指示方向。但如果你把指南针竖起来（旋转90度），它的指针就不再能正确指示方向了——这就是一种"死锁"。

四元数用四个数（q0, q1, q2, q3）来表示旋转，虽然不够直观，但不会出现死锁问题，计算也更平滑，适合连续旋转。我们不需要深入理解四元数的数学原理，只需要知道：

四元数可以表示任意姿态
四元数可以转换成欧拉角
我们最终使用的还是欧拉角（pitch, roll, yaw）

Mahony算法的核心思想

Mahony算法的核心思想可以概括为一句话：用陀螺仪积分得到姿态，用加速度计修正漂移。

具体怎么实现呢？分几个步骤来讲：

第一步：理解两种"重力方向"

陀螺仪积分后，我们得到了一个姿态（用四元数表示），根据这个姿态可以"估计"出重力应该指向哪个方向。

同时，加速度计直接"测量"了重力方向（因为静止时加速度计测的就是重力）。

如果陀螺仪没有漂移，这两个方向应该完全一致。但如果陀螺仪有漂移，这两个方向就会有差异。这个差异就是误差！

第二步：用误差修正陀螺仪

计算出误差后，我们用这个误差来修正陀螺仪的读数。这就像一个PI控制器：

P项（比例）：误差越大，修正越强
I项（积分）：累积的误差也要修正

修正后的陀螺仪数据更加准确，积分得到的姿态也更加准确。

第三步：更新四元数

用修正后的陀螺仪数据更新四元数。这一步涉及四元数的微分方程：

四元数微分方程q̇ = 0.5 × q ⊗ ω

其中q是四元数，ω是角速度，⊗是四元数乘法。这个公式告诉我们：四元数的变化率等于当前四元数与角速度的乘积。

第四步：归一化四元数

四元数必须保持单位长度（|q| = 1），否则会引入误差。所以每一步都要归一化。

第五步：四元数转欧拉角

最后把四元数转换成我们熟悉的欧拉角。

Mahony算法的数学推导（可选）

如果想更深入理解，这里有一些关键公式。如果觉得太难，可以跳过，不影响理解算法的整体流程。

四元数与旋转矩阵的关系

四元数q = (q0, q1, q2, q3)对应的旋转矩阵R是：

四元数对应旋转矩阵R = | 1-2(q2²+q3²)   2(q1q2-q0q3)   2(q1q3+q0q2) |
    | 2(q1q2+q0q3)   1-2(q1²+q3²)   2(q2q3-q0q1) |
    | 2(q1q3-q0q2)   2(q2q3+q0q1)   1-2(q1²+q2²) |

旋转矩阵的第三列就是陀螺仪"估计"的重力方向：

估计重力方向公式估计重力 = (2(q1q3-q0q2), 2(q2q3+q0q1), 1-2(q1²+q2²))

误差计算

误差等于测量重力方向叉乘估计重力方向：

误差计算公式error = accel_measured × gravity_estimated

叉乘的结果是一个向量，垂直于两个输入向量，大小等于两个向量的夹角的正弦值。当两个方向一致时，叉乘结果为零；当有偏差时，叉乘结果就是修正方向。

误差修正

误差修正公式gyro_corrected = gyro_raw + Kp × error + Ki × ∫error

其中Kp是比例增益，Ki是积分增益。这两个参数决定了修正的强度。

Mahony算法的代码实现思路

理解了原理后，我们来看看代码是怎么实现的。这里给大家一个伪代码帮大家来理解流程。

Mahony算法伪代码// 全局变量：四元数（初始值代表水平姿态）
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;

// 全局变量：积分误差
float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;

// 算法参数
float twoKp = 2.0f;   // 2倍比例增益
float twoKi = 0.0f;   // 2倍积分增益（通常设为0也能工作）

void MahonyAHRSupdateIMU(float gx, float gy, float gz,    // 陀螺仪（弧度/秒）
                         float ax, float ay, float az)    // 加速度计（任意单位）
{
    // ===== 步骤1：归一化加速度计数据 =====
    // 先计算加速度的模长
    float norm = sqrt(ax*ax + ay*ay + az*az);
    // 防止除零
    if (norm == 0.0f) return;
    // 归一化
    ax /= norm;
    ay /= norm;
    az /= norm;

    // ===== 步骤2：计算估计的重力方向 =====
    // 根据当前四元数，估计重力方向（旋转矩阵第三列）
    float halfvx = q1*q3 - q0*q2;
    float halfvy = q0*q1 + q2*q3;
    float halfvz = q0*q0 - 0.5f + q3*q3;

    // ===== 步骤3：计算误差（叉乘） =====
    // 误差 = 测量重力 × 估计重力
    float halfex = ay*halfvz - az*halfvy;
    float halfey = az*halfvx - ax*halfvz;
    float halfez = ax*halfvy - ay*halfvx;

    // ===== 步骤4：积分误差（如果启用了Ki） =====
    if (twoKi > 0.0f) {
        integralFBx += twoKi * halfex * (1.0f / sampleFreq);
        integralFBy += twoKi * halfey * (1.0f / sampleFreq);
        integralFBz += twoKi * halfez * (1.0f / sampleFreq);
        // 把积分误差加到陀螺仪上
        gx += integralFBx;
        gy += integralFBy;
        gz += integralFBz;
    }

    // ===== 步骤5：比例修正（加到陀螺仪上） =====
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;

    // ===== 步骤6：积分四元数微分方程 =====
    // 预乘时间因子
    gx *= 0.5f * (1.0f / sampleFreq);
    gy *= 0.5f * (1.0f / sampleFreq);
    gz *= 0.5f * (1.0f / sampleFreq);

    // 保存旧的四元数值
    float qa = q0, qb = q1, qc = q2;

    // 更新四元数
    q0 += -qb*gx - qc*gy - q3*gz;
    q1 +=  qa*gx + qc*gz - q3*gy;
    q2 +=  qa*gy - qb*gz + q3*gx;
    q3 +=  qa*gz + qb*gy - qc*gx;

    // ===== 步骤7：归一化四元数 =====
    norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 /= norm;
    q1 /= norm;
    q2 /= norm;
    q3 /= norm;
}

代码要点解释：

归一化加速度计：因为加速度计的单位和量程可能不同，归一化后只关心方向，不关心大小。
估计重力方向：用四元数计算出"按照当前姿态，重力应该指向哪里"。公式看起来复杂，其实就是旋转矩阵的第三列。
叉乘计算误差：叉乘的结果告诉你"怎么旋转才能让估计方向和测量方向一致"。
积分误差：如果误差一直存在（陀螺仪有固定偏移），积分项会累积起来，最终消除这个偏移。
四元数更新：这是最核心的一步，用陀螺仪数据更新四元数。公式来自四元数微分方程。
归一化四元数：由于浮点数计算误差，四元数长度会逐渐偏离1，必须每步归一化。

四元数转欧拉角

最后，我们需要把四元数转换成欧拉角，方便后续使用：

四元数转欧拉角代码void quaternionToEuler(float q0, float q1, float q2, float q3, Euler_angles* angles)
{
    // Pitch（俯仰角）
    float sinp = 2.0f * (q0*q2 - q3*q1);
    if (fabs(sinp) >= 1)
        angles->pitch = asin(1.0或-1.0) * 180/π;  // 处理90度边界
    else
        angles->pitch = asin(sinp) * 180/π;

    // Roll（横滚角）
    angles->roll = atan2(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2)) * 180/π;

    // Yaw（偏航角）
    angles->yaw = atan2(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3)) * 180/π;
}

注意：这里乘以180/π是把弧度转换成角度，方便阅读和调试。

Mahony算法的参数调节

Mahony算法有两个关键参数：Kp（比例增益）和Ki（积分增益）。

Kp的作用：决定了加速度计修正陀螺仪的强度。Kp越大，收敛越快，但对加速度计噪声也更敏感。典型值在0.5到5之间。

Ki的作用：消除陀螺仪的固定偏移（零偏）。如果陀螺仪零偏很小，可以设为0。典型值在0到0.1之间。

调节建议：

首先把Ki设为0，只调Kp
Kp太小：收敛慢，姿态响应迟钝
Kp太大：姿态震荡，对加速度计噪声敏感
找到合适的Kp后，如果发现姿态有固定偏差，再逐渐增加Ki

完整的姿态解算流程

把所有内容串起来，完整的姿态解算流程如下：

完整姿态解算流程代码void AttitudeUpdate(void)
{
    // 1. 读取IMU原始数据
    float ax, ay, az;    // 加速度计原始数据
    float gx, gy, gz;    // 陀螺仪原始数据
    ReadIMU(&ax, &ay, &az, &gx, &gy, &gz);

    // 2. 转换单位
    // 加速度计：归一化或转换为g
    // 陀螺仪：从原始值转换为弧度/秒
    gx = gx_raw * π / 180.0f / 灵敏度;
    gy = gy_raw * π / 180.0f / 灵敏度;
    gz = gz_raw * π / 180.0f / 灵敏度;

    // 3. 可选：数据滤波
    LowPassFilter(&ax, &ay, &az);
    // 注意：陀螺仪通常不滤波，因为会引入延迟

    // 4. Mahony算法更新四元数
    MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);

    // 5. 四元数转欧拉角
    quaternionToEuler(q0, q1, q2, q3, &angles);

    // 现在 angles.pitch, angles.roll, angles.yaw 就是当前姿态
}

这个函数应该以固定频率调用（如200Hz），通常放在定时中断里。

第五章：PID控制算法

什么是PID控制？

PID控制是工业中最常用的控制算法，从空调温控到汽车巡航，到处都有它的身影。它的核心思想很简单：根据误差的大小计算出修正量，使系统趋向目标值。

举个生活中的例子：想象你在开车，想让车速保持在100km/h。你看仪表盘发现当前速度是80km/h，误差就是20km/h。你踩油门，踩多深取决于误差大小。车速上升后误差减小，油门逐渐松开，最终车速稳定在100km/h。这个过程就是一个P（比例）控制。

PID三个部分详解

**P（比例控制）**的公式是"输出 = Kp × 误差"，意思是误差越大修正越强。比如目标角度是0°、当前角度是10°，误差就是10°，如果Kp=2那么输出就是20。P控制的特点是响应快、立即产生修正，但可能导致超调（修正过头），而且如果只有P控制可能无法完全消除误差。它就像开车时误差大就猛踩油门。

**I（积分控制）**的公式是"输出 = Ki × 误差积分"，意思是把误差累加起来持续修正。为什么需要I？有时候P控制的输出不够大，无法完全消除误差。比如飞机一直偏向左边2°，P控制输出一个小修正但不够，I控制每周期累加这个误差，累积到一定程度就能产生足够大的修正把它消除。I控制的特点是能消除稳态误差，但可能导致超调和震荡，而且需要限制积分值防止"积分饱和"。它就像持续的小错误也要纠正不能忽视。

**D（微分控制）**的公式是"输出 = Kd × 误差变化率"，意思是预测误差的变化趋势提前修正。为什么需要D？P控制只看当前误差，不知道误差在变大还是变小，D控制能"预判"趋势提前刹车。比如当前误差是5°、上一周期误差是10°，误差变化是-5°，说明误差在减小修正够了不需要再加，D输出为负可以抵消一部分P输出。D控制的特点是提供阻尼抑制震荡，但对噪声敏感需要配合滤波，太大会导致响应迟钝。它就像预判快到目标了就开始刹车。

积分饱和是什么？

积分饱和是PID控制中的一个常见问题。假设飞机在地上，你突然把目标高度设为10米，误差非常大积分项开始快速累积，积分值变得很大很大。当飞机接近目标时，积分项仍然很大，会导致飞机严重超调甚至冲过头。解决方法是积分限幅，限制积分值的范围不让它无限增长。

完整的PID公式

完整的PID公式是：输出 = Kp × e(t) + Ki × ∫e(t)dt + Kd × de(t)/dt，其中e(t)是误差（期望值减测量值）。

用代码表示更直观：


void Com_PID_Calculation(PID_Struct *pid, float dt)
{
    // 计算误差
    float error = pid->desire - pid->measure;

    // 积分项：累加误差
    pid->integral += error;

    // 积分限幅：防止积分饱和
    if (pid->integral > pid->Limiting) pid->integral = pid->Limiting;
    if (pid->integral < -pid->Limiting) pid->integral = -pid->Limiting;

    // 微分项：误差变化率
    float der = error - pid->last_error;
    pid->last_error = error;

    // PID输出 = P项 + I项 + D项
    pid->output = pid->kp * error + pid->ki * pid->integral + pid->kd * der;
}

级联PID（双环控制）

单环PID存在一个问题：响应不够快，抗干扰能力弱。想象你在控制飞机角度，单环PID是角度误差直接输出给电机，但如果有风吹来飞机先倾斜PID才开始修正，反应就慢了。解决方案是级联PID，也就是外环加内环。

外环是角度环，输入期望角度和测量角度，输出期望角速度，作用是确保最终角度正确。内环是角速度环，输入期望角速度和测量角速度，输出电机修正量，作用是快速响应扰动。

为什么这样设计？因为角速度来自陀螺仪响应非常快（毫秒级），角度需要积分计算响应较慢。内环可以快速抑制扰动不等它影响角度，外环负责最终精度。


void Com_PID_Cascade(PID_Struct *outter, PID_Struct *inner, float dt)
{
    // 第一步：计算外环PID
    Com_PID_Calculation(outter, dt);

    // 第二步：外环输出作为内环的期望值
    inner->desire = outter->output;

    // 第三步：计算内环PID
    Com_PID_Calculation(inner, dt);
}

第六章：飞控核心流程

飞控的"心脏"——定时中断

飞控的核心代码运行在一个定时中断里。什么是中断？想象你在看书，定时中断就像闹钟每隔固定时间响一次，闹钟响了你就放下书本去处理其他事情，处理完回来继续看书。为什么用定时中断？因为这样可以保证飞控以固定的频率运行（如每5ms一次），不受其他代码影响，确保实时性。

主控制循环

飞控的主控制循环每5ms执行一次，流程如下：


void MainControlLoop(void)  // 5ms定时器中断
{
    // 步骤1：姿态解算——读取IMU数据，计算当前姿态
    AttitudeUpdate();
    // 得到 angles.pitch, angles.roll, angles.yaw

    // 步骤2：遥控器数据解析——得到期望值
    ParseRemoteController();
    // 得到期望角度、油门、飞行模式等

    // 步骤3：高度控制——如果是定高模式，执行高度PID
    if (altitude_hold_mode) {
        AltitudePID(current_height, target_height, 0.005f);
    }

    // 步骤4：姿态PID控制——根据期望角度和当前角度计算电机修正量
    AttitudePID();

    // 步骤5：电机输出——把PID输出转换成四个电机的转速
    MotorMixer();
}

姿态PID控制详解

姿态PID控制是飞控的核心算法。三个方向（Pitch、Roll、Yaw）各有一套级联PID，每套PID包含外环（角度环）和内环（角速度环）。

角度环（外环）与角速度环（内环）的级联PID控制逻辑及输出流程，可通过以下程序框图清晰理解，贴合飞控实际代码执行逻辑：

在这里插入图片描述

框图说明：遥控器输入的期望角度进入角度环（外环），与姿态解算得到的当前角度计算误差，输出期望角速度；该期望角速度作为角速度环（内环）的输入，与陀螺仪测量的当前角速度计算误差，输出电机修正量；修正量与基础油门经电机混控后，输出四个电机的具体转速，驱动飞机调整姿态，形成闭环控制。

以Pitch控制为例：外环的期望值来自遥控器（用户想让飞机低头多少），测量值来自姿态解算（当前实际俯仰角）。内环的测量值来自陀螺仪Y轴角速度（俯仰角速度）。通过级联PID计算，最终输出俯仰方向的电机修正量。


void AttitudePID(void)
{
    // Pitch（俯仰）控制
    pid_pitch.desire = 遥控器映射到角度;
    pid_pitch.measure = angles.pitch;  // 当前俯仰角
    pid_gyro_y.measure = gyroy / 灵敏度;  // 角速度
    Com_PID_Cascade(&pid_pitch, &pid_gyro_y, dt);

    // Roll（横滚）控制——原理与Pitch相同
    pid_roll.desire = 遥控器映射到角度;
    pid_roll.measure = angles.roll;
    pid_gyro_x.measure = gyrox / 灵敏度;
    Com_PID_Cascade(&pid_roll, &pid_gyro_x, dt);

    // Yaw（偏航）控制
    pid_yaw.desire = 遥控器映射到角度;
    pid_yaw.measure = angles.yaw;
    pid_gyro_z.measure = gyroz / 灵敏度;
    Com_PID_Cascade(&pid_yaw, &pid_gyro_z, dt);
}

电机混控

PID控制输出的是三个修正量：Pitch修正量、Roll修正量、Yaw修正量。这些修正量需要转换成四个电机的转速，这个过程叫电机混控。

混控公式怎么理解？以Pitch修正为例：向前飞要低头，前电机（M1、M2）要减油门，后电机（M3、M4）要加油门，所以M1、M2加Pitch而M3、M4减Pitch（当Pitch为负时）。Roll修正和Yaw修正的道理类似。


M1(右前) = 基础油门 + Pitch修正 - Roll修正 - Yaw修正
M2(左前) = 基础油门 + Pitch修正 + Roll修正 + Yaw修正
M3(左后) = 基础油门 - Pitch修正 - Roll修正 + Yaw修正
M4(右后) = 基础油门 - Pitch修正 + Roll修正 - Yaw修正

第七章：飞行模式与状态机

飞控的几种模式

一个完整的飞控通常支持多种飞行模式。手动模式完全由你控制，飞控只保持姿态稳定，适合有经验的飞手。自稳模式松开摇杆飞机会自动保持水平，适合初学者。定高模式自动保持高度你只控制方向，也适合初学者。定点模式自动保持位置悬停在原地，对新手最友好。自动模式按预设航线飞行，适合航拍等应用。

什么是状态机？

状态机是一种管理"模式切换"的方法。想象电梯：当前状态是停在一楼，按3楼按钮状态就变为"上升"，到达3楼状态又变为"停止"。电梯只能处于一种状态：停止、上升、下降，状态之间有明确的切换条件。飞控的状态机也是一样的道理。

飞控状态机设计

典型的飞控有四种状态：IDLE（停机状态电机停转）、HAND_FLYING（手动模式完全由遥控器控制）、FIXED_HEIGHT（定高模式自动保持高度）、FIXED_POINT（定点模式自动保持位置）。

状态之间的切换逻辑是：从IDLE状态，如果满足解锁、遥控器连接、急停关闭、起飞指令等条件，就可以切换到HAND_FLYING或FIXED_HEIGHT状态。在HAND_FLYING状态下如果触发急停或上锁就回到IDLE。在FIXED_HEIGHT状态下可以切换到FIXED_POINT，也可以回到IDLE。在FIXED_POINT状态下可以切换回FIXED_HEIGHT，也可以回到IDLE。

第八章：定高与定点控制

为什么需要定高？

在手动模式下，你需要不断调整油门来保持高度，这很累人。定高模式下，飞控会自动调整油门让飞机保持在你想要的高度。

高度测量

要控制高度首先要测量高度。常用的测量方式有三种。

TOF（飞行时间）传感器发射红外光测量光线往返时间从而计算距离，精度高（毫米级）但量程有限通常只能测几米内。超声波传感器原理类似但使用声波代替光波，成本低量程较大但精度较低且受环境影响大。气压计测量大气压变化推算高度变化，适合大高度变化但精度低且受天气影响。

高度控制

高度控制也使用级联PID。外环是高度环，输入期望高度和测量高度，输出期望垂直速度。内环是速度环，输入期望垂直速度和测量垂直速度，输出高度修正量。

测量垂直速度有两种方法：一种是对TOF高度微分，短期准但噪声大；另一种是对加速度积分，长期准但有漂移。实际应用中会融合两种方法。

定点控制

定点控制让飞机保持在水平面上的某个位置不漂移。它使用光流传感器，通过摄像头检测地面纹理的移动来计算飞机的水平位移。

定点控制是更复杂的级联控制：位置环输出期望速度，速度环输出期望角度，然后才进入姿态控制。也就是说定点PID的输出会作为姿态PID的期望值。

光流传感器有个问题：飞机旋转时传感器也会检测到"位移"，但这是虚假位移。解决方法是用陀螺仪测量旋转角速度，计算旋转引起的虚假位移，然后从光流数据中减去。

第九章：PID调参指南

调参的重要性

PID参数决定了飞控的性能：参数合适时飞机稳定、响应快、操控好；参数不合适时飞机震荡、漂移甚至失控。调参是飞控开发中最关键也最考验经验的环节。

调参前的准备

首先要确保硬件正常：电机安装正确转向正确，传感器校准完成，遥控器连接正常。其次要确保安全：卸下螺旋桨防止意外伤人，准备急停开关，在空旷场地测试。最后要准备调试工具：能实时查看传感器数据和PID输出，能快速修改参数。

调参顺序

记住口诀：先内环后外环，先P后D再I。

为什么要先调内环？因为内环（角速度环）直接控制电机，内环不稳定外环根本无法工作，内环调好后外环才能发挥效果。

角速度环调参

调P（比例系数）：初始值建议P=0.3。测试方法是把飞机固定在测试架上只能绕一个轴转动，给一个干扰观察飞机反应。如果P太小，反应迟钝修正缓慢晃动几下才稳住；如果P刚好，反应迅速一两次晃动后稳定；如果P太大，高频震荡嗡嗡响甚至发散。调参方法是从小到大逐渐增加P，出现轻微震荡时往回退一点，找到"刚好不震荡"的值。

调D（微分系数）：初始值建议D=0。D的作用是增加阻尼抑制震荡。如果D太小容易超调震荡，如果D刚好响应平滑无震荡，如果D太大响应迟钝对噪声敏感。调参方法是P引起轻微震荡时逐渐增加D，震荡消除即可不要过大，D过大反而会引入噪声震荡。

调I（积分系数）：初始值建议I=0.01（很小的值）。I的作用是消除稳态误差。如果I太小有偏差时无法自动纠正一直歪着，如果I刚好能自动回到水平无低频震荡，如果I太大低频震荡超调。调参方法是在P和D调好后给飞机一个固定干扰（如挂个重物），观察能否自动回正，不能回正则增加I，出现低频震荡则减小I。

角度环调参

角速度环调好后才能调角度环。角度环通常只需要P，初始值建议P=5.0。测试方法是推动遥控器摇杆松开后飞机应自动回正。如果P太小回正慢需要等很久，如果P刚好快速回正无超调，如果P太大回正时超调震荡。调参方法是从小到大增加P直到飞机能快速响应遥控器指令但不要出现震荡。

参考参数

以下是一组典型的PID参数，仅供参考。注意参数正负号取决于传感器安装方向和电机转向，每架飞机的参数都可能不同，需要根据实际飞行效果微调。


// Pitch（俯仰）控制
pid_pitch.kp = 5.0f;    // 外环，角度环
pid_gyro_y.kp = -0.3f;  // 内环，角速度环
pid_gyro_y.ki = -0.025f;
pid_gyro_y.kd = -4.0f;

// Roll（横滚）控制
pid_roll.kp = 5.0f;     // 外环
pid_gyro_x.kp = 0.3f;   // 内环
pid_gyro_x.ki = 0.025f;
pid_gyro_x.kd = 4.0f;

// Yaw（偏航）控制
pid_yaw.kp = 2.0f;      // 外环
pid_gyro_z.kp = -1.0f;  // 内环

// 高度控制
pid_height.kp = 1.0f;       // 外环
pid_z_speed.kp = 0.2f;      // 内环
pid_z_speed.ki = 0.02f;
pid_z_speed.kd = 0.4f;

调参常见问题

飞机剧烈震荡通常是PID参数过大，解决方法是减小角速度环P值或增加D值，同时检查控制周期是否稳定。飞机反应迟钝通常是PID参数过小，解决方法是增加P值或减小D值。飞机有固定偏差通常是I参数太小或传感器零偏，解决方法是增加I值或校准传感器零偏。飞机漂移严重通常是姿态解算不准，解决方法是检查Mahony算法参数、调整Kp增益、上电后静置校准。

第十章：常见问题与解决方法

起飞问题

飞机起飞后立即翻转是最常见的问题之一。可能的原因有电机顺序接错、电机转向错误、PID正负号错误、传感器方向错误。排查方法是逐个测试电机确认顺序和转向，用手倾斜飞机观察PID输出方向是否正确，检查传感器安装方向。

飞机无法起飞的可能原因有油门不足、电机不转、电池电量不足、飞行模式错误。排查方法是检查电机是否能正常转动、检查电池电压、确认处于正确的飞行模式。

稳定性问题

飞机悬停时震荡可能的原因有PID参数过大、传感器噪声大、机械振动。解决方法是减小角速度环P值、增加传感器滤波、检查电机平衡和机架刚性。

飞机缓慢漂移可能的原因有陀螺仪零偏、加速度计校准不准、定点传感器失效。解决方法是上电后静置校准陀螺仪、重新校准加速度计、检查光流/TOF传感器。

控制问题

遥控器无响应可能的原因有SBUS信号丢失、解析错误、中断配置问题。解决方法是检查接收机连接、检查SBUS波特率（通常100000）、检查串口中断是否正常。

飞机不听指挥可能的原因有遥控器通道映射错误、PID输出方向错误、电机混控公式错误。解决方法是检查遥控器通道定义、验证PID输出与期望动作的关系、检查电机混控公式。

传感器问题

姿态角不准可能的原因有传感器校准问题、Mahony算法参数不对、滤波参数不当。解决方法是重新校准传感器、调整Mahony的Kp和Ki参数、调整滤波器截止频率。

高度跳变可能的原因有TOF传感器数据不稳定、测量范围超限、地面反射问题。解决方法是增加数据滤波、检查测量范围、更换测量表面。

附录：关键公式速查

欧拉角定义：Pitch（俯仰角）绕Y轴旋转，机头抬起为正低头为负，范围-90°到+90°。Roll（横滚角）绕X轴旋转，右侧下沉为正左侧下沉为负，范围-180°到+180°。Yaw（偏航角）绕Z轴旋转，机头右偏为正左偏为负，范围-180°到+180°。

PID公式：输出 = Kp × e(t) + Ki × ∫e(t)dt + Kd × de(t)/dt，其中e(t) = 期望值 - 测量值。离散形式：error = desire - measure，integral += error，derivative = error - last_error，output = Kp×error + Ki×integral + Kd×derivative。

电机混控公式：M1(右前) = Throttle + Pitch_out + Roll_out - Yaw_out，M2(左前) = Throttle + Pitch_out - Roll_out + Yaw_out，M3(左后) = Throttle - Pitch_out - Roll_out - Yaw_out，M4(右后) = Throttle - Pitch_out + Roll_out - Yaw_out。注意正负号取决于具体布局和转向。

Mahony算法核心：用加速度计修正陀螺仪漂移，修正公式：gyro_corrected = gyro_raw + Kp × error + Ki × ∫error，其中error = accel_normalized × gravity_estimated（叉乘）。

四元数转欧拉角：