偏航推杆导致油门失控问题分析

问题现象

当偏航（YAW）推杆推到最大值（>1820）或最小值（<1180）后，油门完全失控——油门会自动飙到最大值，用户此时只能控制油门在最大值和最小值之间切换，无法正常调节油门大小。

---

根本原因

pidYaw.desired（偏航角期望值）没有上限/下限限制，在偏航推杆持续处于极限位置时会无限增长/衰减，导致外环偏航PID输出暴涨，最终通过串级PID传导至电机混控，吞掉所有PWM余量，油门基线形同虚设。

---

详细分析

1. 问题触发路径

偏航推杆极限位 (YAW > 1820 或 YAW < 1180)
    ↓
FlyPidControl() 每 3ms 调用一次
    ↓
pidYaw.desired 持续累加/累减 0.75（无上下限！）
    ↓
pidYaw.out = kp × (desired - measured)  急剧增大
    ↓
pidRateZ.desired = pidYaw.out  内环期望值飙升
    ↓
pidRateZ.out = kp × (pidRateZ.desired - gyro_Z)  内环输出飙升
    ↓
电机混控中 ± pidRateZ.out 成为主导项
    ↓
油门基线 (THR-1000)*0.7 被完全淹没
    ↓
油门失控

2. 关键代码位置

文件：Fly/FlyContrl.c，函数 FlyPidControl()，第 220-227 行：

if (FlyRecData.YAW > 1820)
{
    pidYaw.desired -= 0.75f; // 每3ms减0.75，每秒减250度！
}
else if (FlyRecData.YAW < 1180)
{
    pidYaw.desired += 0.75f; // 每3ms加0.75，每秒加250度！
}

问题： pidYaw.desired 没有任何 Limit() 约束。如果偏航推杆持续在极限位置 1 秒，期望值就会漂移 ±250 度；持续 4 秒就是 ±1000 度。

3. 定量推算失控过程

假设偏航推杆推到最大值（YAW > 1820）持续 3 秒：

时间	pidYaw.desired	pidYaw.out (kp=6.0)	pidRateZ.out (kp=2.0)
0s	0	0	0
0.5s	-125	-750	-1500
1.0s	-250	-1500	-3000
2.0s	-500	-3000	-6000
3.0s	-750	-4500	-9000

电机混控公式（FlyContrl_Motor_3ms() 第 44-47 行）：

MOTOR1 += +pidRateX.out - pidRateY.out - pidRateZ.out;  // -(-9000) = +9000 → 限幅到1000
MOTOR2 += +pidRateX.out + pidRateY.out + pidRateZ.out;  // +(-9000) = -9000 → 限幅到0
MOTOR3 += -pidRateX.out + pidRateY.out - pidRateZ.out;  // -(-9000) = +9000 → 限幅到1000
MOTOR4 += -pidRateX.out - pidRateY.out + pidRateZ.out;  // +(-9000) = -9000 → 限幅到0

结果： 电机1和3满转(1000)，电机2和4停转(0)，油门基线 (THR-1000)*0.7（最大仅700）完全被 ±9000 级别的PID输出淹没。无论用户如何操作油门推杆，电机输出都被死死限制在 0 或 1000。

4. 为什么"只能控制油门在最大值到最小值"？

因为 Limit() 函数将每个电机PWM限制在 [0, 1000] 范围内。当 pidRateZ.out 绝对值超过 1000 后，电机输出就变成了纯开关量——不是 0 就是 1000，中间没有任何过渡。用户推油门时，只能看到电机从 0 跳到 1000（或反之），中间没有任何线性控制区间。

---

修复方案

方案一：限制 pidYaw.desired 的范围（推荐）

在 FlyPidControl() 中，对 pidYaw.desired 添加限幅：

// 在 FlyPidControl() 的 case 2 中，偏航期望值计算后添加
if (FlyRecData.YAW > 1820)
{
    pidYaw.desired -= 0.75f;
}
else if (FlyRecData.YAW < 1180)
{
    pidYaw.desired += 0.75f;
}

// ★ 添加以下限幅代码 ★
if (pidYaw.desired > 360.0f)   pidYaw.desired = 360.0f;
if (pidYaw.desired < -360.0f)  pidYaw.desired = -360.0f;

偏航角正常情况下不需要超过 ±360 度的期望值，合理限幅即可。

方案二：将偏航期望值改为绝对位置映射（更优）

当前的累加模式存在漂移问题。更好的做法是将摇杆位置直接映射为偏航角速度期望值：

// 替换当前的累加逻辑
if (FlyRecData.YAW > 1820)
{
    pidRateZ.desired = 150.0f;  // 直接设定期望角速度（度/秒），而不是不断累加角度
}
else if (FlyRecData.YAW < 1180)
{
    pidRateZ.desired = -150.0f;
}
else
{
    pidRateZ.desired = 0.0f;
}

这样做的好处：

• 偏航期望值不会无限累加
• 摇杆回中后偏航立即停止
• 移除外环偏航角PID，简化控制结构
• 这是大多数穿越机/无人机飞控的通用做法——偏航直接控制角速度，而非角度

方案三：对PID输出增加总限幅（兜底保护）

无论采用哪种方案，建议在 pidUpdate() 函数中增加输出限幅：

void pidUpdate(PidObject *pid, const float dt)
{
    // ... 原有计算 ...

    pid->out = pid->kp * error + pid->ki * pid->integ + pid->kd * deriv;

    // ★ 增加输出限幅 ★
    if (pid->out > 500.0f)   pid->out = 500.0f;
    if (pid->out < -500.0f)  pid->out = -500.0f;

    pid->prevError = error;
}

同时在 pidRest() 中也要重置 desired 和 measured：

void pidRest(PidObject **pid, const uint8_t len)
{
    uint8_t i;
    for (i = 0; i < len; i++)
    {
        pid[i]->integ = 0;
        pid[i]->prevError = 0;
        pid[i]->out = 0;
        pid[i]->desired = pid[i]->measured;  // ★ 复位时期望值同步到当前测量值 ★
    }
}

---

额外发现：解锁/锁定逻辑的竞争条件

在 FlyContrl_Unlock_10ms() 第 98 行，立即锁定条件是：

if (FlyRecData.THR < 1100 && (FlyRecData.YAW < 1100 || FlyRecData.YAW > 1900))

这意味着只有当油门同时也处于低位时，偏航极限位才会触发锁定。如果用户在飞行中（油门较高时）推偏航到极限，此条件不满足，飞机不会锁定，但 pidYaw.desired 已经开始疯狂漂移——这正是本次bug的触发场景。

建议增加一个飞行中偏航极限位的保护逻辑，例如检测到 pidYaw.desired 超过阈值时自动钳位，而不是依赖解除锁定。

---

总结

项目	内容
问题根因	pidYaw.desired 无限累加，无上下限保护
影响范围	偏航推杆极限位时油门完全失控
严重程度	严重 - 飞行中触发会导致炸机
推荐修复	方案一 + 方案三（双重保护），长远考虑方案二（重构偏航控制策略）
修复文件	Fly/FlyContrl.c（FlyPidControl 函数）、Fly/pid.c（pidUpdate 和 pidRest 函数）