源码主线 06｜把估计、模式与任务连成一个完整系统

全系统设计不是让每个模块都有算法，而是让每个输出都拥有清楚的物理语义、时间语义和控制权。到这里，可以把一次“航点接近→视觉捕获→环绕→人工接管”拆成可解释的状态转换，而不是一个不断发送速度的大循环。

一条端到端的信息链

目标来源 → 任务状态与租约 → 路线／图像制导 → 带坐标单位和有效期的参考 → 飞控模式 → 位置／速度／姿态闭环 → 执行器映射与混控 → 机器人运动 → IMU／DVL／深度／相机。反馈并非只在最后闭合：估计有效性会限制可选模式，执行器能力会限制上层速度，视觉身份和年龄会决定任务是否继续。

“零”不是一种足够清楚的状态

意图	需要定义的输出	物理含义
停止追踪	不再消费视觉任务目标	机器人仍可由其他模式控制
速度为零	速度反馈把实际速度拉回零	有水流时通常仍需要推力
推力为零	归一化执行命令归零	机器人可能继续惯性运动或漂移
定点保持	保存位置目标并闭位置／速度环	需要可用位置与速度估计
解除武装	走明确的电机使能状态转换	不同于普通任务暂停

项目当前GUIDED中，有效非零XY命令进入速度闭环；有效零XY命令清水平推力并重置相关PID；超时或无有效指令则捕获位置进入保持。这个具体例子说明：同一“停止发送速度”的上层意图，可能先清推力，随后又进入保持。设计协议时应以明确状态表达意图，而不是期待所有模块对数值零做相同猜测。

恢复不是重新开始，也不是无条件接着走

在PosHold恢复中，需要区分三个状态：旧任务目标、当前位置估计、控制器内部积分。保留旧目标回答“还想去哪里”；用当前估计初始化回答“从哪里重新控制”；重置或平滑积分回答“如何避免输出跳变”。若底层初始化顺手把位置目标改成当前值，上层“继续原目标”的意图就可能被覆盖。把目标设置放在显式恢复事务中，可以使这三个决定有固定顺序。

教学恢复流程：先确认估计重新可用 → 记录当前位置 → 根据任务决定保留旧目标或捕获新目标 → 初始化控制器动态状态 → 显式写入选定目标 → 用受限参考逐步接入。这里是设计用伪流程，不是声称原代码逐行如此执行。最有价值的测试，是分别观察目标、积分和最终输出，而不只观察模式名称。

安全看门狗为什么是分层的

DVL滤波器关心观测是否连续有效；EKF关心自己是否仍有足够约束；任务层关心位置、心跳、图像与命令是否新鲜；执行层关心输出是否仍由活跃控制源更新。各层时间门限不同并非天然矛盾，但它们必须组成明确策略：传感器短失效时可以惯性传播，持续失效时任务应降低依赖定位的行为。

桥接的BridgeState.accept管理任务模式租约，_select_outgoing检查期限、模式、armed与遥测新鲜度。一个任务持有租约时，另一个任务不应通过偶发UDP包抢占；人工接管则应通过明确优先级和模式切换抢占自动任务。收到MANUAL_CONTROL报文本身与自动模式已经退出，是两个不同事件。

用契约表取代“接口应该兼容”的猜测

契约维度	教学例
物理量	速度m/s，还是归一化推力命令
坐标	LOCAL_NED，body FRD，或内部NEU
时间	采样时间、生成时间、接收时间、TTL
状态	命令有效、估计有效、模式匹配、已武装
所有权	来源、session、seq、租约及人工优先级
结束行为	暂停、清推力、定点保持、切换模式分别定义

为每个发送端和接收端填同一张表，随后用真实序列化与解析函数做回环。正常命令、重复序号、旧会话、过期图像、坐标不符、模式不符、零命令、断流各走一次。断言的是最终目标与输出，不只是“成功收包”。这就是从局部算法测试走向系统级可解释行为。

最终综合练习：一个可逐层解释的任务

在软件中建立简化机器人：IMU高频预测、DVL低频速度约束、压力约束深度；A*或直接航点经LOS生成NED速度；靠近目标后让视觉中心／面积误差接管制导；侧移与yaw前馈形成环绕。先保持传感器理想，再依次加入横流、DVL短失效、图像延迟和命令断流。每次只改变一个条件，说明任务状态、估计不确定度、控制参考与推进器命令各如何变化。能完整讲清这四层，你就真正理解了这套GNC，而不只是记住函数名。