INAV 定翼机 Pitch-to-Throttle 机制深度解析：nav_fw_pitch2thr 的源码实现与气动理论

• 无人机
• 嵌入式
• 智能硬件

本文面向参与 INAV（Navigation-enabled flight control software）开发的人员，系统讲解nav_fw_pitch2thr（pitch-to-throttle，俯仰-油门耦合）的完整实现链路、线性近似的适用边界，以及三个可独立推导增益的理论气动模型为何对同一架飞机给出截然不同的答案。读完后你将掌握：fixedWingPitchToThrottleCorrection()的过滤、死区与钳位细节，该设置在settings.yaml中的全部相关字段及其取值范围，以及评估/修改该模型时需要考虑的理论局限。用户侧的调参实战指南请参考 Fixed Wing Pitch To Throttle Tuning。

功能定位：为什么定翼机需要 pitch-to-throttle

在 GPS 辅助模式（Position Hold、RTH、航点任务、Cruise）中，INAV 需要飞机在爬升时自动加大油门、俯冲时自动减小油门，以维持大致恒定的空速——因为爬升要克服重力分量、俯冲则相反。nav_fw_pitch2thr就是这个"每度俯仰角对应多少油门"的耦合增益，单位为PWM 微秒/度（μs/°）。

该设置只影响 GPS 辅助导航模式，不影响手动/Acro/Angle 飞行，因此在 Position Hold 下测试它是安全的。其基础公式（来自 src/main/fc/settings.yaml 中该设置自身的 description）为：

throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle)

注意符号约定：pitch_angle以度为单位，爬升时为负、俯冲时为正，所以"减号 × 负值"在爬升时表现为加油门、俯冲时减油门；最终结果被约束在nav_fw_min_thr与nav_fw_max_thr之间。

当前实现：核心函数与调用链

核心函数：fixedWingPitchToThrottleCorrection()，位于 src/main/navigation/navigation_fixedwing.c，由同文件的applyFixedWingPitchRollThrottleController()（L1703）调用。函数原型在 src/main/navigation/navigation.h 中声明：

int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);

核心逻辑：

int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs) { static timeUs_t previousTimePitchToThrCorr = 0; const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; previousTimePitchToThrCorr = currentTimeUs; // Apply low-pass filter to pitch angle to smooth throttle correction int16_t filteredPitch = pt1FilterApply3(&pitchToThrFilterState, pitch, US2S(deltaMicrosPitchToThrCorr)); int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle; #ifdef USE_FW_AUTOLAND if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) { pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f; } #endif if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) { // Unfiltered throttle correction outside of pitch deadband return DECIDEGREES_TO_DEGREES(pitch) * pitchToThrottle; } else { // Filtered throttle correction inside of pitch deadband return DECIDEGREES_TO_DEGREES(filteredPitch) * pitchToThrottle; } }

从源码可以看到几个文档层面容易忽略的实现细节：

1. 输入单位是 decidegrees（0.1°）。函数内部通过DECIDEGREES_TO_DEGREES()转为度后再乘以增益，因此pitch_to_throttle的量纲严格是 μs/°。
2. PT1 低通滤波与死区切换逻辑：俯仰角先经过一个 PT1 滤波器（静态状态pitchToThrFilterState，见 L227）产生filteredPitch；若原始俯仰与滤波俯仰的偏差超过pitch_to_throttle_thresh（死区），说明存在持续性的俯仰变化，此时采用未滤波的原始值立即响应；若偏差在死区之内，说明只是短时姿态噪声，则采用滤波后的平滑值。这个"双路切换"设计是死区（nav_fw_pitch2thr_threshold）与平滑（nav_fw_pitch2thr_smoothing）两个相关设置共同作用的机制。
3. 滤波截止频率由平滑度设置推导。getPitchToThrottleSmoothnessCutoffFreq()（L217-L221）按0.001 × 2.0Hz × (10 − smoothness)³ + 0.01计算截止频率——基础频率为NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ = 2.0f（L62），pitch_to_throttle_smooth取值 0–9，数值越大滤波越弱（截止频率越高），最低可降至约 0.01 Hz。滤波器状态在resetFixedWingAltitudeController()中随控制量一并重置（L234-L245）。

修正量的三级钳位

applyFixedWingPitchRollThrottleController()中对throttleCorrection的处理顺序（L1731-L1749）：

1. 相对钳位：先约束到[min_throttle - cruise_throttle, max_throttle - cruise_throttle]；但在NAV_CTL_LAND（着陆）状态下上界收紧为0——因为着陆阶段机头上仰时绝不允许加大油门（仰角通常用于减速）：

   if (navStateFlags & NAV_CTL_LAND) { // During LAND we do not allow to raise THROTTLE when nose is up to reduce speed throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0); } else { throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection); }

2. 叠加最小地速控制量：在NAV_CTL_POS模式且非着陆、位置源可用时，applyFixedWingMinSpeedController()的输出（地面速度低于min_ground_speed时的积分式补偿，上限 500 μs，见 L1648-L1673）也会被加入并重新钳位一次。这是一个独立的最小空速/地速机制，不属于nav_fw_pitch2thr本身，但在油门链路中先后叠加。

3. 绝对钳位：cruiseThrottle + throttleCorrection最终被constrain(..., minThrottle, maxThrottle)限制在电池配置的绝对油门范围内（L1749）。

此外还有两条容易与 pitch-to-throttle 混淆的分支：

• 手动油门干预：allow_manual_thr_increase开启且非 failsafe/autoland 时，飞行员高于巡航值的油门杆会叠加到修正值上（L1753-L1762）。
• Auto Speed 路径：自动速度模式启用时，pitch-to-throttle 不走上面的分支，而是作用于无皮托管时的最低油门保护——fixedWingPitchToThrottleCorrection(-attitude.values.pitch, ...)被叠加到auto_speed_level_min_thr上以防止顶风侧低速失速（L2043-L2046）。

自主着陆最终进近阶段的增益放大

在USE_FW_AUTOLAND编译目标上，当处于FW_AUTOLAND_STATE_FINAL_APPROACH（最终进近）且pitch < 0（爬升/抬机头方向）时，增益会被navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f缩放（L1687-L1691）。对应设置 src/main/fc/settings.yaml 中的nav_fw_land_final_approach_pitch2throttle_mod（字段finalApproachPitchToThrottleMod，百分比，范围 100–400，默认 100），用于在短跑道短五边进近时更激进地收油门；相关着陆阶段行为见 Fixed Wing Landing。

相关设置参数一览

以下参数均定义于 src/main/fc/settings.yaml，按油门链路中的角色整理：

需要强调：这是一个局部线性增益，不是查找表、也不是飞行包线映射——每 1° 俯仰变化，油门固定变化 N 微秒，超出电池配置油门范围的部分由钳位吃掉。

为什么采用线性近似

物理上，保持恒定空速时飞行路径角 γ 所需功率包含sin(γ)项，是非线性的。但对小角度，sin(γ) ≈ γ（γ 以弧度计）：

（参考 Houghton & Carpenter，《Aerodynamics for Engineering Students》，第 5 版）

在 15° 以内，线性近似误差约 1% 量级，远小于姿态估计本身的精度——因此对定翼机导航实际飞行的俯仰范围（多数 GPS 辅助机动保持在 ±20° 以内），一次简单乘法就是足够好的模型。真正的限制不是曲线拟合误差，而是油门行程本身太窄：典型可用油门跨度约 1000 μs，不足以补偿理论值所要求的持续大坡度爬升所需功率；在包线边缘，起决定作用的是钳位（saturation）而非线性化误差。

三个理论模型估计增益——以及它们为何互不一致

下面三个相互独立、各自有效的简化气动模型都可以用来估算nav_fw_pitch2thr的"正确"值（依据飞机的升阻比 L/D 与推重比 T/W）。它们对同一架飞机给出明显不同的答案，因为各自在不同的飞行状态下做了线性化。需要明确：这三个模型均未在 INAV 中实现、未经飞行测试、也未经评审作为默认值变更提案——它们只是气动推理辅助工具，不构成调参建议（用户侧仍应以 实战调参指南 的试飞流程为准）。

模型 1：爬升/下降功率比

保持恒定空速、飞行路径角 γ 时所需功率：

P_climb = P_level × (1 + sin(γ) × L/D) P_descent = P_level × (1 - sin(γ) × L/D)

（沿飞行路径的力平衡：推力 = 阻力 ± W·sin(γ)；功率 = 推力 × 速度。Houghton & Carpenter pp. 26-44。）近似油门与功率成正比并代入小角度关系，得：

nav_fw_pitch2thr ≈ throttle_cruise × 0.01745 × L/D

取throttle_cruise = 1500 μs、L/D = 10：理论值 ≈ 262 μs/°——但这意味着仅 2° 俯仰就会把油门推到饱和，显然不可用。若回退到在饱和前覆盖现实 ±10–20° 工作范围（(max_throttle - cruise_throttle) / max_practical_climb_angle），则得到约 25–50 μs/°，取决于假定的工作范围。

模型 2：平飞点的局部力导数

任意俯仰角 γ 下的力平衡：T = W[cos(γ)/(L/D) + sin(γ)]。对 γ 求导并在 γ = 0（平飞）处取值：

dT/dγ = W / 57.3 (每度)

用推重比 T/W = α（全油门时的推力/重量）并换算到 INAV 的 1000 μs 油门范围：

nav_fw_pitch2thr ≈ 1000 / (57.3 × α) ≈ 17.5 / α μs/°

取 α = 1.0（约 1:1 推重比，多数 RC 飞机的典型值）：≈17.5 μs/°。该模型在 γ = 0 处对 L/D 不敏感——线性化点只有 T/W 起作用——并且它预测T/W 越高的飞机增益应越低：同样绝对值的推力变化，对更大的可用推力而言占比更小。

模型 3：纯重力竖直飞行（90°）

在精确 90° 俯仰时，阻力只取决于空速；同空速下稳态竖直爬升与竖直下降相比时阻力项相消，剩下纯粹的推力-重力平衡。对 α = T/W = 1（爬升即悬停，V=0）的情况，把全部 1000 μs 行程平摊到 ±90° 包线上：

nav_fw_pitch2thr = 1000 μs / 90° ≈ 11.1 μs/°

这个数值恰好接近当前默认值 10。但 10 是否正是出于这个原因选定的无法考证——该设置早于本仓库最早可用的历史记录（仓库只有一条 "commit everything" 式导入），没有 changelog 或提交说明可查。因此应把这种接近视为"值得记录的巧合"，而非已确认的解释。

三者为何不一致

每个模型都在不同的点或状态域对同一套底层力/功率平衡做了线性化：模型 1 在现实的爬升角度范围内积分功率；模型 2 严格在平飞点取导数；模型 3 只看阻力按构造相消的 90° 极端点。同时，三者都没有考虑下文"局限"一节列出的因素（螺旋桨推力曲线非线性、电机效率随负载变化、桨洗效应、动态/瞬态效应）。这正是经验性、经飞行测试的调参比任何单一公式都更可靠的原因。

当前线性模型的已知局限

• 不随巡航油门自适应。同一增益适用于任何当前巡航油门占比，而巡航油门本身会随电池电压跌落或载荷变化。
• 不随 L/D 状态域变化自适应（襟翼、依赖空速的阻力极曲线均未建模）。
• 假设油门 ∝ 功率 ∝ 推力，对螺旋桨而言这三者都不严格成立：推力与功率都随空速和 RPM 非线性变化，电机/电调效率也随负载变化。
• 没有桨洗（propwash）建模——牵引式布局在爬升时桨洗流过机翼会带来额外升力，模型未体现。
• 仅稳态——俯仰角速率与加速度瞬态未被建模；PT1 滤波器与死区的存在，正是为了避免对短时姿态噪声而非持续俯仰变化做出反应。

以上各项目前在fixedWingPitchToThrottleCorrection()中均未处理。此处列出是为考虑修改油门修正模型的开发者提供上下文，不是待办清单。

小结与延伸阅读

nav_fw_pitch2thr用一行乘法实现了定翼机导航中的能量管理核心：爬升补油门、俯冲收油门，配合 PT1 滤波 + 死区切换抗姿态噪声，配合三级钳位防止越界，着陆态额外禁止仰角加油门，autoland 最终进近可放大增益。线性模型在 ±20° 内精度足够，包线边缘的行为由油门饱和主导；若要从理论上估算增益，三个简化模型给出的区间（约 11–50 μs/°）应理解为"合理范围"而非"标准答案"，最终值必须通过试飞确定。

• 用户侧调参实战（基线试飞、步进调整、判断过/欠补偿的迹象、按机型给出起始区间）：docs/Fixed Wing Pitch To Throttle Tuning.md
• 自主着陆各阶段油门行为（含nav_fw_land_final_approach_pitch2throttle_mod）：docs/Fixed Wing Landing.md
• 电池配置域（battery profile 对cruise/min/max throttle的作用域）与空中调整：docs/Battery.md、docs/Inflight Adjustments.md
• 核心源码：src/main/navigation/navigation_fixedwing.c（fixedWingPitchToThrottleCorrection、applyFixedWingPitchRollThrottleController）、src/main/fc/settings.yaml（nav_fw_pitch2thr及相关设置）
• 气动理论参考：Houghton, E.L. and Carpenter, P.W.,Aerodynamics for Engineering Students, 5th ed., Butterworth-Heinemann — pp. 26-44（力平衡、阻力、爬升/下降功率），pp. 62-67（性能计算、小角度有效性）

• 无人机
• 嵌入式
• 智能硬件