上一篇讲了Commander——它决定"该不该飞"。该飞了，往哪飞？这就是Navigator的事。

Mission模式飞航线，是无人机最常见的任务场景。QGC里画几个点，上传，无人机就老老实实一个个飞过去。看起来简单，背后Navigator做了大量的活：航点解析、路径跟踪、地理围栏校验、ROI航向控制、降落航线规划……每一项都是实打实的算法。

我把Navigator的源码翻了一遍，把这条"从航点到电机"的完整决策链理清楚。

Navigator在系统里的位置

先回顾一下数据流。Commander输出导航状态（Mission/RTL/Hold/Land/Takeoff），Navigator收到后，根据当前模式从SD卡读取航点信息，计算出目标位置，发布position_setpoint_triplet给位置控制器。位置控制器再往下走PID链路，最终驱动电机。

Navigator不是自己干所有事，它是一个调度器。内部有6个独立的飞行模式模块：

Navigator的主循环run()以20Hz频率运行，每次循环做三件事：

1. 订阅vehicle_local_position和vehicle_status，获取当前位置和飞行状态
2. 根据Commander下发的导航模式，调用对应模块的on_active()
3. 将计算出的position_setpoint_triplet发布到uORB

这个triplet是个关键数据结构，包含三个航点：previous、current、next。位置控制器拿到它，就知道"从哪来、到哪去、下一步去哪"。

Mission模式：一条航线是怎么飞的

Mission是Navigator最复杂的模块。核心流程可以拆成三步：读航点、判断是否到达、推进到下一个。

航点读取与解析

航点存在SD卡上，Mission模块通过read_mission_item()从SD卡读取当前索引的航点，填充到mission_item_s结构体。每个航点包含：

• 经纬高（AMSL绝对高度）
• 航点类型：WAYPOINT、LOITER、TAKEOFF、LAND等
• 停留时间（time_inside）
• 盘旋半径和方向
• 航向约束（yaw）
• 接受半径（acceptance_radius）

然后set_mission_item()把当前航点复制到pos_sp_triplet.current，前一个航点放previous，下一个航点放next。三航点信息齐全，位置控制器就可以工作了。

航点到达判定：不是到了就算

这是容易踩坑的地方。飞机什么时候算"到达"航点？不是飞到正上方，而是进入以航点为中心的某个范围内。

多旋翼和固定翼的判定逻辑完全不同：

多旋翼用NAV_ACC_RAD参数定义接受半径，默认很小（约0.5m）。飞机进入这个半径就算到达，立刻切换下一个航点。如果你把NAV_ACC_RAD调大，飞机会提前转弯，路径更平滑但精度降低。

固定翼不用NAV_ACC_RAD，而是用L1距离（新版改为NPFG距离）。计算公式：

L1_distance = (1/π) × NPFG_PERIOD × NPFG_DAMPING × ‖v_xy_ground‖

默认参数下，20m/s地速时L1距离约70m。也就是说，固定翼在离航点70m的地方就已经开始转向下一个航点了。这是物理限制——固定翼不能原地悬停，必须提前转弯。

航点推进逻辑

简化后的核心逻辑：

// Mission::on_active() 简化
voidMission::on_active()
{
// 1. 读取当前航点
    set_mission_item();

// 2. 判断当前航点是否到达
if (is_mission_item_reached()) {
// 停留时间检查
if (time_inside_ready()) {
// 推进到下一个航点
            advance_mission_index();
            set_mission_item();
        }
    }

// 3. 更新航向
    heading_sp_update();

// 4. 发布位置设定值
    navigator->set_position_setpoint_triplet_updated();
}

is_mission_item_reached()的判定条件因航点类型不同：

• WAYPOINT：水平距离 < acceptance_radius 且 垂直距离 < NAV_FW_ALT_RAD
• LOITER：盘旋时间到 或 圈数到
• TAKEOFF：到达起飞高度
• LAND：着陆检测

time_inside是航点的停留时间参数。比如测绘任务里需要在每个点悬停3秒拍照，就设time_inside=3。停留期间飞机不会推进到下一个航点。

路径跟踪算法：从L1到NPFG

航点到达判定解决了"什么时候切换"，路径跟踪算法解决"怎么飞过去"。

L1控制器（经典方案）

PX4早期的固定翼路径跟踪用的是L1控制器。核心思想：在飞机前方L1距离处取一个参考点，计算侧向加速度指令，让飞机朝参考点转弯。

a_lat = (2 × v² / L1) × sin(η)

η是地速方向到参考点的角度。L1越小转弯越激进，L1越大路径越平滑。

关键参数：

• NPFG_PERIOD（旧名FW_L1_PERIOD）：默认15m，越小响应越快但可能震荡
• NPFG_DAMPING（旧名FW_L1_DAMPING）：默认0.75

NPFG控制器（PX4 1.13+）

PX4 1.13之后引入了NPFG（Nonlinear Path Following Guidance），替代L1。NPFG的改进：

1. 抗风能力：L1用地速计算，大风时地速方向可能跟航向差很大，L1会判断失误。NPFG用空速估计，能补偿风的影响
2. 超风处理：当风速超过空速，固定翼会被风吹着走。NPFG能检测这种极端情况，切换到最小能耗策略
3. 动态跟踪误差边界：根据当前地速动态调整可接受的偏航距离

关键参数还是NPFG_PERIOD和NPFG_DAMPING，调参方法跟L1一样：先缩PERIOD直到响应灵敏但不震荡，再微调DAMPING。

多旋翼的路径跟踪简单得多——直接飞到目标点，用的是jerk-limited轨迹规划。转弯速度自动根据接受半径和最大加速度/jerk计算，保证平滑。

航向控制：飞过去还要朝对方向

光飞到航点不够，很多时候还得朝对方向。Navigator通过heading_sp_update()计算航向设定值。

航向策略由MPC_YAW_MODE控制：

ROI（Region of Interest）是很有用的功能。比如航拍任务，你可以设一个ROI点（比如一栋楼），然后无人机飞航线时始终朝向这栋楼。Navigator通过vehicle_roi uORB消息把ROI信息传给位置控制器。

多旋翼可以独立控制航向和飞行方向，所以这些策略都能正常工作。固定翼航向和飞行方向耦合，航向设定会被忽略——飞机只能朝飞的方向看。

地理围栏：不让你飞的地方别飞

Geofence是Navigator的另一个重要职责，它在所有模式下都生效，不只是Mission。

PX4提供两层围栏：

基础围栏（Failsafe Geofence）

一个以Home为中心的圆柱体：

围栏参数默认都是0，等于没开。实际项目一定要配上。

高级围栏（Geofence Plan）

QGC里可以画多个多边形和圆形区域，分别标记为inclusion（只能在里面飞）和exclusion（不能飞进去）。这比基础围栏灵活得多——可以在禁飞区周围画exclusion区域，在作业区域画inclusion区域。

注意：任何不包含Home位置的围栏都会被飞控拒绝上传。飞行中上传的围栏如果立即被违反，也会被拒绝。

围栏检测在Navigator主循环里每次都执行，不依赖当前飞行模式。一旦触发，按GF_ACTION设定的动作响应。

航线可行性检查：飞之前先验一遍

Mission上传和首次解锁时，PX4会做一系列可行性检查。检查不通过，任务不允许启动：

• 第一个航点离Home太远（MIS_DIST_1WP）
• 航点跟围栏冲突
• 固定翼降落滑道角度超限（FW_LND_ANG，默认5度）
• 多个降落起始点（MAV_CMD_DO_LAND_START只能有一个）
• 缺少起飞/降落项（MIS_TKO_LAND_REQ，固定翼和VTOL默认要求有降落项）

这些检查避免了"飞到一半发现任务不合理"的尴尬。

固定翼降落航线：最复杂的单条航线

固定翼Mission里，降落航线是最讲究的。一条标准的降落航线由两个航点组成：

1. 盘旋点（MAV_CMD_NAV_LOITER_TO_ALT）：飞机飞到这个点上空，盘旋下降到指定高度
2. 降落点（MAV_CMD_NAV_LAND）：从盘旋高度沿滑道角下滑，接地前拉平（flare）

滑道角由两个航点的3D位置决定，不能超过FW_LND_ANG。QGC上传时会检查，角度太大直接拒绝。

降落阶段的参数很细：

拉平逻辑是：从高度跟踪切换到浅下沉率设定值，同时限制油门，机头上仰减速，实现软着陆。腹板着陆的话一直保持拉平状态直到触地检测和上锁。

任务暂停与恢复

Mission可以被暂停——切换到Hold或Position模式就行。切回Mission模式后，从当前位置飞向当前活跃航点，继续执行。

注意：暂停期间如果你移动了飞机，恢复后不会沿着原来的航线飞，而是直接飞向当前航点。

任务只有在解锁或上传新任务时才会重置。DO_JUMP指令的循环计数器在手动切换航点时不会重置——这是很多人不知道的坑。如果你跳到航点1想"重新开始"，跳转计数没清零，循环次数可能不够。

关键参数速查

踩坑记录

我调航线的时候遇到过几个典型问题：

1. 固定翼飞不到航点正上方

这不是bug，是特性。L1/NPFG距离70m意味着飞机在离航点70m处就开始转弯。如果你需要飞到航点正上方（比如投送物资），得多旋翼干这活，或者把航点间距拉大到3倍L1距离以上。

2. NAV_ACC_RAD调大了航线偏移严重

多旋翼的接受半径不是"到这就停"，是"到这就切换下一航点"。调大了飞机会提前转弯，航线内侧会切角。需要精度的场景（测绘、巡检）保持默认小值，需要平滑的航线（航拍）适当放大。

3. 围栏忘了开

GF_MAX_HOR_DIST和GF_MAX_VER_DIST默认都是0，等于没围栏。实际项目第一件事就是配上围栏，GF_ACTION至少设成Warning，推荐设成Return。

4. 固定翼降落滑道角超限

QGC规划降落航线时，盘旋点和降落点的3D位置差决定了滑道角。如果你把盘旋点高度设得很高、离降落点又近，角度很容易超FW_LND_ANG。上传时会被拒，但如果你在地面没测试直接起飞就麻烦了——提前验证任务可行性。

5. DO_JUMP循环计数不重置

手动在QGC里切换当前航点不会重置DO_JUMP的计数器。如果你想让任务真正从头开始，要么重新上传任务，要么解锁再上锁。

小结

Navigator是Commander和位置控制器之间的桥梁。Commander说"飞Mission"，Navigator就从SD卡读航点、算路径、检查围栏、发布目标位置。Commander说"返航"，Navigator就接手RTL逻辑。每种模式都有独立的模块处理，Navigator只做调度。

理解Navigator的关键在于理解position_setpoint_triplet——previous/current/next三航点构成了位置控制器需要的全部输入。Navigator所有的工作，本质上就是在不同模式下正确填充这个三元组。

下一篇写uLog——飞完之后怎么看数据。

本文代码为简化后的核心逻辑，方向正确但不是PX4真实代码。参数名和默认值已对照PX4官方文档校验。