code-test-drone/docs/视觉跟踪控制律技术路线.md

视觉跟踪控制律（visual_track_control）技术路线

本文档把 visual_track_control/ 子项目已实现的控制律抽象为与编程语言无关的 技术路线：只讲逻辑、数据结构、算法与实现方式，不依赖 C++ 语法特性。 可作为任意语言（C++/Rust/Python 仿真/嵌入式 C）重新实现或移植的规格说明。

文档关系（三份文档各司其职，不重复抄录）：

文档	定位	权威性
docs/物理追踪思路.md	控制律物理与行为规格（为什么这样控）	数学与行为的唯一权威
docs/视觉跟踪控制律技术路线.md（本文）	语言无关实现路线（逻辑怎么落地）	实现结构与算法流程的权威
visual_track_control/视觉跟踪控制律实现文档.md	C++ 实现说明（构建、日志、排查）	当前 C++ 落地的权威

文中出现的字段名（如 track_time_ms、yaw_rate_dps）取自当前实现，仅作为 跨文档可追溯的标识符，不构成语言绑定；重写时可自由改名，但语义与单位必须一致。

⚠️ 全部数值均为占位值：相机内参/畸变未标定、增益与 PID 未整定。标定完成前 本路线仅限回放/SITL 验证数学链路，不得用于实飞（见第 11 章红线）。

---

目录

1. 概述与系统定位
2. 输入输出接口定义
3. 核心构件与状态清单
4. 坐标系与符号约定
5. 单周期控制流程（主流程伪代码）
6. 各通道算法细节
7. 安全语义：超龄、判丢、Coast、无距离
8. 输出打包与链路
9. 配置参数全量表
10. 设计取舍记录（为什么不是别的做法）
11. 实现与重写指引
12. 验证路线与待定项

---

1. 概述与系统定位

1.1 模块职责

本模块是视觉跟踪阶段（状态机 S7 VISUAL_TRACKING）的单周期控制律，职责可归纳为 "三算一限一降级一打包"：

1. 三路控制律计算（每拍一次，固定周期）
   • 水平航向：目标像素偏移 → 水平视线角 β → 增益 → 偏航指令；
   • 垂直高度：目标像素偏移 → 垂直视线角 α → 垂直速度指令；
   • 距离：双目目标距离与期望射程之差 Δd → PID → 前向速度指令。
2. 统一限幅：每个通道内联完成"变化率限幅 + 幅值限幅"，任何输出都不允许 跳过限幅直达链路（含切换后的首条指令）。
3. 安全降级：输入超龄、目标判丢、距离不可用时，输出降级意图而不是继续 用旧数据算正常指令。
4. 单出口打包：控制意图 → MAVLink 字节流 → 注入的传输实现；控制律本身 不认识串口、电台或飞控。

1.2 在系统中的位置

┌─────────────┐   检测框    ┌──────────────┐  追踪结果   ┌───────────────────┐
│ YOLO 识别   │────────────▶│ TargetTracker│───────────▶│  本模块：控制律    │
│ （感知线程）│  每帧       │ （滤波器）   │  主目标+    │  TrackingControlLaw│
└─────────────┘             └──────────────┘  Coast标志  └─────────┬─────────┘
                                                                   │
┌─────────────┐  目标距离                                          │ 控制意图
│ 双目立体测距│──────────────────────┐                             ▼
└─────────────┘                      │              ┌──────────────────────┐
┌─────────────┐  姿态/航向/速度       ├─────────────▶│ 打包网关             │
│ PX4 链路    │──────────────────────┘   控制快照    │ （控制意图→MAVLink） │
└─────────────┘   （由调用方采集打戳）                └─────────┬────────────┘
      ▲                                                         │ 字节流
      │                    MAVLink offboard 指令流               ▼
      └──────────────────────────────────────────────  ┌──────────────────┐
                                                       │ 传输抽象实现     │
                                                       │ （串口/电台/回环）│
                                                       └──────────────────┘

上游依赖：追踪库提供主目标中心与 tracked / is_predicted 两个行为标志 （追踪库技术路线见 docs/视觉追踪技术路线.md）；双目模块提供目标距离 （检测框自身的立体测距，前向分量 Z）与有效标志 distance_valid；飞控链路 提供自机姿态（至少航向角）。

方位源（接入层约定）：双目模组两枚相机分列模组中线两侧（各偏 ±B/2）， 单目像素存在近场视差偏差（3m 处约 0.57°）。匹配成功时接入层应以虚拟双目 中心像素 x_mid = (xL + xR) / 2 填充 track.center（推导见 docs/双目深度估计思路.md §11.3）。虚拟中心相机与单目共用同一 FOV 配置， 本模块"像素偏移 → 视线角"逻辑与配置零改动。

下游依赖：只有一个——传输抽象。打包与传输分离，控制律不直接写飞控。

上层归属：状态机（S6/S7/S8 切换、目标丢失策略、障碍接管）在本模块之外。 本模块只按输入标志产生"当拍应该做什么"，不决定"要不要继续视觉闭环"。

1.3 设计特点

特点	含义	带来的约束
纯值输入 / 纯值输出	每拍传入一个不可变快照，返回一个输出值对象	无隐藏输入、无回调、可离线回放
单线程固定周期	由控制线程按配置频率调用，构件内部无锁	不得从多线程并发调用同一实例
热路径零动态分配	所有系数、缓冲在构造期备妥；每拍只做算术	便于实时性分析与嵌入式移植
热路径零日志	仅在输出模式发生切换时打一条日志	日志量与状态变化次数成正比，不与频率成正比
配置全量外置	所有阈值/增益/限幅来自启动期加载的配置对象	代码内不得出现任何写死的控制参数
无状态纯函数构件	视线角转换、超龄判定、坐标旋转均为纯函数	可独立单元测试，无需夹具
单一 offboard 消息类型	全周期只发一种消息（ID 84）	避免消息类型逐拍翻转账扰动飞控（见 §10.1）

1.4 明确不做的事（边界）

• 不做感知：不跑检测/光流/双目立体匹配，只消费调用方填入的快照；
• 不做距离有效性判定：distance_valid 由双目链路自身的一致性校验给出， track_id 一致性校验为调用方约定（§12.2），本模块只读取标志；
• 不做状态机仲裁：不判断何时进入/退出视觉跟踪，不实现目标丢失策略；
• 不做链路 IO：不打开串口、不管理重连、不解析任何入向 MAVLink 消息；
• 不做配置热更新：启动期加载一次，调参重启生效；
• 不做多目标：只处理追踪库选定的主目标。

---

2. 输入输出接口定义

2.1 输入：控制快照（每拍一个，由调用方采集并打戳）

分组	字段	单位 / 类型	语义与约束
视觉	track.center	像素 (x, y)	主目标中心；原点为画面左上角，x 右正、y 下正；双目匹配成功时由接入层填虚拟双目中心 x_mid（§1.2）
视觉	track.tracked	布尔	是否存在有效轨道；false → 判丢刹车
视觉	track.is_predicted	布尔	本拍是否为滤波器预测值（Coast）；true → 清距离积分
视觉	track_time_ms	单调时钟 ms	取样瞬间打戳；<= 0 视为从未取样，直接判超龄
距离	target_distance_m	m	双目目标距离（前向分量 Z）；<= 0 视为不可用
距离	distance_valid	布尔	本帧测距可用标志：距离来自目标检测框自身的双目测距；无效帧的距离不得当作目标距离
距离	distance_time_ms	单调时钟 ms	双目距离取样瞬间打戳
姿态	attitude_present	布尔	姿态源当前是否可用；false → 禁止控制
姿态	attitude_time_ms	单调时钟 ms	姿态取样瞬间打戳
姿态	yaw_rad	rad (NED)	自机航向；参与计算：位置式航向合成 + 输出端坐标旋转
姿态	roll_rad / pitch_rad	rad (FRD)	预留（机体补偿），当前不参与计算
姿态	yaw_rate_radps	rad/s	预留（机体补偿），当前不参与计算
自机速度	velocity_x/y/z_mps	m/s	预留，接入主工程后由飞控状态填充

关键约定：追踪结果自身不带时间戳。 追踪库输出的是"最新一帧的结果"， 调用方必须在读取结果的那一瞬间用单调时钟打戳，写入 track_time_ms。 所有超龄判定一律采用"取样时刻 + 有效期"模型（§7.1），不使用"消息序号"或 "到达时刻"。

时钟要求：单一单调时钟源。 快照内三类时间戳与 Update() 的 now_ms 必须来自同一个不受墙钟调整影响的时钟（如系统启动后毫秒数）。混用墙钟会 导致 now < sample 的不可核实情形，实现按"宁可降级"处理（§7.1）。

2.2 输出：控制意图（每拍一个，已限幅）

字段	单位	语义
mode	枚举	kVelocityHeading / kBrakeHover / kDegradedHold
valid	布尔	是否可交付链路发送；kDegradedHold 恒为 false
vx_mps	m/s	局部 NED 北向速度（机体系前向经航向旋转得到）
vy_mps	m/s	局部 NED 东向速度（机体系侧向恒 0，旋转后可非零）
vz_mps	m/s	NED 下正垂直速度
yaw_deg	deg (NED)	绝对目标航向，回绕到 [-180, 180)；仅位置式模式非零
yaw_rate_dps	deg/s	偏航角速度指令（相对量）；仅速率式模式非零
degraded_reason	枚举	kNone / kAttitudeStale / kVisionStale / kNoDistanceExit
coast_active	布尔	本拍处于 Coast 预测跟踪

输出坐标系已在库内完成旋转：速度分量对外一律是局部 NED（北/东/地）， 与打包时声明的坐标框一致。调用方不得再做二次旋转。

2.3 输出模式与可发送性矩阵

模式	valid	速度分量	航向字段	链路动作
kVelocityHeading	true	三路限幅后旋转值	按配置的航向模式二选一非零	发送速度+航向指令
kBrakeHover	true	全零	全零（掩码忽略 yaw 与 yaw_rate）	发送零速刹车
kDegradedHold	false	全零	全零	不发送，仅告警/记录降级原因

2.4 传输抽象契约（唯一出向依赖）

接口 CommandTransport:
    Send(data: 字节序列, len: 长度) -> 布尔
        返回 true  = 已交付底层链路
        返回 false = 发送失败（调用方可计数/告警，控制律不重试）

契约要点：

• 只吃字节流：串口、电台、回环打印、文件录制均可注入实现；换传输层 不改动打包与控制律；
• 同步交付：实现方不得长期持有传入的字节指针（同步拷贝或同步写出）， 因此打包侧可以复用同一块固定缓冲；
• 无反向通道：本模块不接收任何入向消息，传输实现无需提供接收能力。

---

3. 核心构件与状态清单

控制律由 1 个主控律 + 6 个构件组成。下表列出每个构件需要持久化的状态， 这是重写/移植时最容易漏掉的部分——状态漏了，行为就变了。

构件	职责	构造期预计算	运行期持久状态	复位入口
主控律	八步流程编排	各构件实例、配置值拷贝	上拍时刻 last_now_ms、上拍模式 last_mode、无距离计时（exit 门限用，§7.5）	Reset() 清全部
视线角转换器 ×2	像素偏移 → β / α	针孔系数 2·tan(θ/2)/size	无（纯函数）	不需要
距离 PID	Δd → 前向速度	PID 参数、输出限幅	积分累计、上拍误差、低通后微分值、上次输出、"是否存在有效微分历史"标志	ResetIntegral() 只清积分；Reset() 清全部
变化率限幅器 ×4	限制每拍变化量	变化率上限	上次输出值	Reset(value)
静态限幅	幅值钳制	无	无（纯函数）	不需要
超龄判定器	输入是否过期	无	无（纯函数）	不需要
坐标旋转	机体系 → NED	无	无（纯函数）	不需要

四个变化率限幅器分别守护什么：

限幅器	守护量	单位	生效通道
航向角限幅器	位置式航向修正量的变化率	deg/s（见 §12.2 量纲待澄清项）	水平航向（位置式）
航向角速率限幅器	偏航角速度指令的变化率	deg/s²	水平航向（速率式，默认）
前向速度限幅器	前向加速度	m/s²	距离通道
垂直速度限幅器	垂直加速度	m/s²	垂直通道

为什么正好是 4 个：机体系侧向速度恒为 0（本控制律不做侧移），因此 ay_mps2 配置项当前为预留、无对应限幅器实例。

---

4. 坐标系与符号约定

4.1 三个坐标系

坐标系	轴定义	用途
像素系	原点左上角，x 右正、y 下正	追踪库输出的原始语义
机体系 (FRD)	x 前、y 右、z 下	控制律内部计算（三路通道各自算出机体量）
局部 NED	x 北、y 东、z 地	对外输出与打包声明

4.2 像素偏移与视线角符号

x_offset = center.x − W/2      # 目标偏右 → 正
y_offset = center.y − H/2      # 目标偏下 → 正

β = 水平视线角（弧度）：与 x_offset 同号   # 目标偏右 → β 正 → 需右转
α = 垂直视线角（弧度）：与 y_offset 同号   # 目标偏下 → α 正

垂直符号链（NED 下正约定）：目标偏低 → α 为正 → 期望向下运动 → NED 垂直 速度 vz 为正（下正）。因此垂直通道不需要额外取反，增益为正值即可。

水平符号链（速率式）：目标偏右 → β 为正 → 偏航角速度指令为正。按 NED 航向"自北顺时针（右转为正）"约定，正角速度即右转，趋近目标。因此水平通道 同样不需要额外取反。

4.3 机体系 → NED 旋转（只随航向角）

给定机体速度 (v_fwd, v_right, v_down) 与自机航向 ψ（rad，自北顺时针为正）：

v_north = v_fwd·cos ψ − v_right·sin ψ
v_east  = v_fwd·sin ψ + v_right·cos ψ
v_down  = v_down                     # 绕下轴旋转不影响垂直分量

为什么只随航向旋转、不含滚转/俯仰：速度设定值的姿态耦合项由飞控内环 处理，这是 offboard 速度设定值的标准做法；滚转/俯仰对水平速度指令是二阶小量。

正方向自检（实现时必须通过的两条断言）：

输入	ψ	期望输出	说明
机体前向 (1, 0, 0)	90°（机头朝东）	(0, 1, 0) = 东	前向随航向转向东 ✓
机体右向 (0, 1, 0)	0°（机头朝北）	(0, 1, 0) = 东	机体右侧即地理东 ✓

4.4 限幅与旋转的先后顺序（正确性论证）

顺序：限幅在机体系完成 → 旋转在输出端执行。

论证：绕下轴旋转是正交变换，保持向量范数不变，即 √(v_north² + v_east²) = √(v_fwd² + v_right²)。因此机体系内满足的幅值约束 在旋转后仍然成立，先限幅后旋转不破坏限幅语义；反过来（先旋转后限幅） 则会让限幅作用在地理轴上，与"机体能力限制"的物理含义不符。

推论：输出的 vy_mps 可以非零（机体系侧向恒 0，旋转后东向分量非零）， 这不是缺陷，调用方不得把它当作异常。

4.5 姿态不可用时的处理

旋转与位置式航向合成都依赖 yaw_rad。因此姿态过期/缺失必须在任何坐标 运算之前拦截（流程第 ① 步），直接进入降级；不允许"用上一拍航向凑合旋转"， 因为那会产生坐标系标注与实际不符的输出（规格明令禁止混用未标注坐标系数据）。

---

5. 单周期控制流程（主流程伪代码）

5.1 节拍推算

函数 计算dt(now_ms) -> 秒:
    dt ← 1.0 / 配置.control.frequency_hz          # 默认：配置频率兜底
    若 last_now_ms > 0 且 now_ms > last_now_ms:    # 时钟正常前进才信任差值
        dt ← min( (now_ms − last_now_ms) / 1000.0, DT_MAX )
    last_now_ms ← now_ms
    返回 dt

常量 DT_MAX ← 0.5 秒                              # 异常长间隔上限

三条兜底规则的动机：

情形	处理	动机
首拍（last_now_ms == 0）	用配置频率推算 dt	无上拍参考，不允许 dt=0 触发除零
时钟回拨或同值（now <= last）	用配置频率推算 dt	差值不可信；不引入额外状态去纠错
间隔异常长（> 0.5 s）	截断到 0.5 s	防止限幅器一步放过巨大跳变

5.2 八步主流程

函数 Update(快照, now_ms) -> 控制意图:
    dt ← 计算dt(now_ms)
    输出 ← 新建控制意图()
    输出.mode ← kVelocityHeading          # 正常路径默认；分支可覆盖为降级/刹车

    # ---- ① 超龄判定（最高优先级：数据不可信就不算） ----
    若 (非 快照.attitude_present) 或 已超龄(快照.attitude_time_ms, now_ms, 配置.attitude_stale_ms):
        输出.mode ← kDegradedHold; 输出.degraded_reason ← kAttitudeStale
        返回 收尾并记录模式切换(输出, "姿态过期/缺失，禁止控制")

    若 已超龄(快照.track_time_ms, now_ms, 配置.visual_stale_ms):
        输出.mode ← kDegradedHold; 输出.degraded_reason ← kVisionStale
        返回 收尾并记录模式切换(输出, "视觉追踪结果超龄")

    # ---- ② 判丢 → 零速刹车 ----
    若 非 快照.track.tracked:
        无距离计时.清除()                              # 刹车期间不计 kExit 时长
        若 last_mode ≠ kBrakeHover:                    # 仅在"进入沿"清状态
            距离PID.全清();  四个限幅器.复位(0)
        输出.mode ← kBrakeHover; 输出.valid ← true      # 速度/航向字段保持全零
        返回 收尾并记录模式切换(输出, "判丢，零速刹车")

    # ---- ③ Coast → 清距离积分 ----
    若 快照.track.is_predicted:
        距离PID.清积分()                                # 只清积分，保留微分历史
        输出.coast_active ← true
        # 航向通道为纯比例映射，无积分项，无需清理

    # ---- ④ 像素偏移 → 视线角 ----
    x_offset ← 快照.track.center.x − 配置.image_width  / 2
    y_offset ← 快照.track.center.y − 配置.image_height / 2
    β_deg ← 水平视线角转换器(x_offset) × 弧度转度        # 针孔模型，见 §6.1
    α_deg ← 垂直视线角转换器(y_offset) × 弧度转度

    # ---- ⑤ 水平航向通道 ----
    若 配置.heading.mode == 速率式:
        目标角速率 ← 配置.heading.gain × β_deg
        限速后     ← 航向角速率限幅器.推进(目标角速率, dt)      # 先变化率
        输出.yaw_rate_dps ← 钳制(限速后, ±配置.yaw_rate_limit_dps)  # 再幅值
    否则:  # 位置式
        目标修正量 ← 配置.heading.gain × β_deg
        修正量     ← 航向角限幅器.推进(目标修正量, dt)
        # ID 84 的 yaw 字段是"绝对航向"，必须与当前航向合成，不能直发修正量
        输出.yaw_deg ← 回绕180( 快照.yaw_rad × 弧度转度 + 修正量 )

    # ---- ⑥ 垂直速度通道（统一速度支路） ----
    目标垂直速度 ← 配置.vertical.vz_gain_mps_per_deg × α_deg
    限速后       ← 垂直速度限幅器.推进(目标垂直速度, dt)        # 加速度限幅
    输出.vz_mps  ← 钳制(限速后, ±配置.vertical.vz_limit_mps)

    # ---- ⑦ 距离通道 ----
    距离可用 ← 快照.distance_valid
              且 快照.target_distance_m > 0
              且 非 已超龄(快照.distance_time_ms, now_ms, 配置.distance_stale_ms)

    若 距离可用:
        无距离计时.清除()                              # 任一有效帧重置 kExit 计时
        Δd ← 快照.target_distance_m − 配置.distance.d_exp_m     # 正=偏远，需接近
        前向原始 ← 距离PID.更新(Δd, dt)
        限速后   ← 前向速度限幅器.推进(前向原始, dt)             # 加速度限幅
        # 非对称限幅：接近可以快，后退必须慢
        输出.vx_mps ← 钳制(限速后, −配置.retreat_velocity_limit_mps,
                                  +配置.approach_velocity_limit_mps)
    否则:
        距离PID.清积分()                                        # 防饱和
        按 配置.distance.no_distance_action 分支:
            保持:        输出.vx_mps ← 0; 前向速度限幅器.复位(0)
            限速接近:    输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
            退出近距操作:
                若 非 无距离计时.进行中: 无距离计时.启动(now_ms)  # 首个无效拍起计时
                若 now_ms − 无距离计时.起始时刻 ≥ 配置.no_distance_exit_grace_ms:
                    输出.mode ← kDegradedHold
                    输出.degraded_reason ← kNoDistanceExit
                    返回 收尾并记录模式切换(输出, "持续无距离达门限，退出近距操作")
                # grace 期内按"限速接近"执行（复用加速度限幅，vx 平滑无跳变）
                输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)

    # ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ----
    旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad)

    输出.valid ← true
    返回 收尾并记录模式切换(输出, 输出.coast_active ? "Coast 预测跟踪" : "正常跟踪")

5.3 提前返回的语义差异（易错点）

三条提前返回路径跳过第 ⑧ 步旋转，这是安全的：这些路径的速度分量全为零， 零向量旋转后仍是零向量。但重写时必须注意：

返回路径	是否清 PID	是否清限幅器	速度是否旋转	valid
姿态超龄降级	否	否	否	false
视觉超龄降级	否	否	否	false
判丢刹车	是（进入沿）	是（进入沿）	否	true
无距离退出（grace 期满）	是（仅积分）	否	否	false
正常/Coast	视情况清积分	否	是	true

降级路径不清状态的理由：降级只是"这一拍不输出"，输入恢复后应当接着上次的 积分/限幅历史平滑续接；若每次降级都清状态，输入抖动会导致指令反复从零起爬。

刹车路径必须清状态的理由：判丢是一次真实的控制中断，恢复跟踪时的首条指令 必须从零渐变（避免位置→速度切换突变），因此积分与全部限幅器历史归零。

5.4 模式切换日志（唯一日志点）

函数 收尾并记录模式切换(输出, 原因文本) -> 控制意图:
    若 输出.mode ≠ last_mode:
        记录 INFO 日志: "[ControlLaw] 状态切换: {旧模式名} -> {新模式名} ({原因文本})"
        last_mode ← 输出.mode
    返回 输出

同一模式连续多拍不重复打印。这是本模块唯一的运行期日志；每拍的三路通道 数学计算不打任何日志。

---

6. 各通道算法细节

6.1 像素 → 视线角：全程针孔模型

构造期（水平、垂直各一个实例）:
    校验 size_px > 0，否则报参数错误
    校验 0 < fov_rad < π，否则报参数错误
    系数 ← 2 · tan(fov_rad / 2) / size_px        # 预计算，热路径不再算 tan

每拍:
    视线角(弧度) ← atan( offset_px × 系数 )

通道	size	fov	含义
水平	image_width (W)	fov_h_deg (θ_h)	偏移 → β
垂直	image_height (H)	fov_v_deg (θ_v)	偏移 → α

为什么不用线性近似：线性式 angle ≈ offset · θ/size 只在小偏移成立。 若做"小偏移走线性、大偏移走针孔"的双路切换，两式在切换边界函数值不相等 （θ=60°、边界比例 0.2 处约 8% 角度台阶），目标跨越边界的那一拍会向控制环路 注入一次角度突变 → 限幅器被迫吸收 → 机体抽动。atan 的计算代价在此频率下 可忽略，故全程使用精确针孔式，配置项也随之少一个（原 linear_ratio 已移除）。

未建模项（红线相关）：镜头畸变、相机安装外参（俯仰/偏航安装角）、 像素非正方形均未补偿。标定完成前，视线角存在系统性偏差。

6.2 水平航向通道

两种输出映射，二选一，由配置决定：

模式	输出字段	量纲	优点	风险
速率式（默认）	yaw_rate_dps	(deg/s)/deg = 1/s	指令天然是相对量，无坐标系歧义；不依赖自机航向精度	稳态需 β→0 才停转，存在小幅极限环可能
位置式	yaw_deg	deg/deg（无量纲）	稳态无累积漂移	输出是绝对 NED 航向，必须与当前航向合成；依赖新鲜姿态

限幅顺序：先变化率、后幅值（不可颠倒）：

目标 ← 增益 × β_deg
限速 ← 变化率限幅器.推进(目标, dt)      # 限制"每拍能变多少"
输出 ← 钳制(限速, ±幅值上限)            # 限制"最大能到多少"

颠倒顺序会导致：幅值限幅先切掉超限部分，变化率限幅器记录的"上次输出"变成 被切过的值，下一步以此为基准继续推进，实际变化率可超过配置上限。

位置式必须合成绝对航向：链路消息的 yaw 字段语义是绝对 NED 航向。 若直接把修正量填进去，等于命令机体"转向修正角那个方向"（例如修正量 5° 会被 理解为"转向北偏东 5°"），与当前航向无关——这是典型的坐标系语义误用。正确做法：

输出.yaw_deg ← 回绕180( 当前航向(度) + 限幅后修正量 )

函数 回绕180(deg):
    d ← fmod(deg + 180, 360)
    若 d < 0: d ← d + 360
    返回 d − 180                       # 结果落在 [-180, 180)

6.3 垂直通道：统一速度支路

目标垂直速度 ← vz_gain_mps_per_deg × α_deg
限速         ← 垂直速度限幅器.推进(目标垂直速度, dt)   # 即垂直加速度限幅
输出.vz_mps  ← 钳制(限速, ±vz_limit_mps)

小误差自然映射为小垂直速度：α = 1° 且增益 0.1 → vz = 0.1 m/s，行为连续， 不存在"小误差走一条支路、大误差走另一条支路"的模式翻转。原设计的双支路 （小误差发姿态微调消息）已取消，理由见 §10.1。

6.4 距离通道

Δd ← 实测距离 − 期望射程 d_exp
    Δd > 0：目标比期望远 → 前向接近（vx 为正）
    Δd < 0：目标比期望近 → 减速/后退（vx 为负）

vx ← 非对称钳制( 加速度限幅( PID(Δd) ), −后退限速, +接近限速 )

非对称限幅的物理动机：接近方向允许较快（追击需要），后退方向必须保守 （后退意味着放弃射程优势，且后方通常未观测）。默认占位值 3 m/s vs 1 m/s。

PID 输出限幅取两者较大值：PID 内部用对称限幅 ±max(接近, 后退) 做 anti-windup 判据，外部再做非对称钳制。这样内部限幅不会先于外部约束生效， 保证 anti-windup 判据基于"真实饱和边界"。

可用性三重门（缺一不可）：distance_valid 成立 且 距离为正 且 未超龄。 双目测距取自目标检测框自身，不存在"回波打到别处"的跨模态关联问题；标志为 false 表示本帧测距不可用（有效域外/未匹配/一致性校验失败），不得把任何数值 当作目标距离。

6.5 PID 内部算法

函数 更新(error, dt) -> 输出:
    若 dt <= 0: 返回 上次输出                       # 异常节拍保护

    # ---- 积分：候选值先钳位 ----
    积分候选 ← 积分累计 + error × dt
    若 积分限幅 > 0: 积分候选 ← 钳制(积分候选, ±积分限幅)

    # ---- 微分：一阶低通 ----
    若 存在有效微分历史:
        微分原始 ← (error − 上拍误差) / dt
        微分 ← 滤波系数 × 上次滤波微分 + (1 − 滤波系数) × 微分原始
    否则:
        微分 ← 0                                    # 首拍不做微分（避免冲击）
    上次滤波微分 ← 微分; 上拍误差 ← error; 存在有效微分历史 ← true

    # ---- 位置式合成 ----
    原始输出 ← kp·error + ki·积分候选 + kd·微分

    # ---- anti-windup：条件积分 ----
    已饱和       ← (输出限幅 > 0) 且 |原始输出| > 输出限幅
    增量推深饱和 ← (error × dt) × 原始输出 > 0       # 本拍积分增量方向与饱和同向
    若 非 (已饱和 且 增量推深饱和):
        积分累计 ← 积分候选                          # 否则冻结本拍累加

    # ---- 输出限幅 ----
    上次输出 ← (输出限幅 > 0) ? 钳制(原始输出, ±输出限幅) : 原始输出
    返回 上次输出

条件积分判据的三种候选与取舍（这是 anti-windup 最容易写错的地方）：

判据	饱和时行为	误差反号后	结论
积分项与输出同向	冻结	仍冻结（积分残留同号）→ 无法回卷	✗ 会留下饱和残留
原始输出 × error > 0	冻结	可回卷，但大 kp 项会掩盖饱和方向 → 回卷被拖延	△ 不够干脆
(error × dt) × 原始输出 > 0	冻结	增量立即反向 → 首拍即回卷	✓ 采用

两种复位的语义差别（必须分开实现）：

方法	清空内容	保留内容	使用场景
清积分	积分累计	微分历史、上次输出	Coast 每拍、距离不可用每拍
全清	积分、上拍误差、滤波微分、上次输出、微分历史标志	参数	判丢进入刹车、停止视觉闭环

Coast 只清积分而不清微分：预测值仍在连续演化，微分历史有效，清掉会让微分项 在恢复跟踪瞬间失去参考、产生一次冲击。

6.6 变化率限幅器

函数 推进(target, dt) -> 输出:
    若 dt <= 0 或 变化率上限 <= 0: 返回 当前输出      # 异常节拍/冻结保护
    最大步长 ← 变化率上限 × dt
    当前输出 ← 钳制(target, 当前输出 − 最大步长, 当前输出 + 最大步长)
    返回 当前输出

两个必须知道的性质：

1. 不可能超调：输出被钳制在以"上次输出"为中心、±最大步长 为半径的区间内， 因此单步变化严格不超过配置上限。调试时若观察到"限幅后数值超过目标"， 一定是别处的问题，不是限幅器超调。
2. 初值为 0 即天然实现首条指令渐变：构造时输出置 0，切换进入视觉跟踪后的 第一条指令会从 0 按加速度限幅爬升，不需要专门写切换分支。这也是"判丢 刹车时把限幅器复位到 0"的原因——复位后恢复跟踪同样从零爬升。

---

7. 安全语义：超龄、判丢、Coast、无距离

7.1 超龄判定（纯函数，三条判据）

函数 已超龄(取样时刻, 当前时刻, 有效期) -> 布尔:
    若 取样时刻 <= 0:            返回 true      # 从未取样
    若 当前时刻 < 取样时刻:      返回 true      # 时钟源错配/打戳错误，年龄不可信
    返回 (当前时刻 − 取样时刻) > 有效期

第二条判据是"宁可降级"的显式表达：单一单调时钟下不应出现 now < sample。 一旦出现，说明取样与判定用了不同时钟源（或打戳位置错误），此时年龄不可核实， 不能因为"看起来没超龄"就继续用它算正常指令。

三个有效期独立配置，反映各输入的更新特性：

输入	有效期占位值	依据
视觉追踪结果	200 ms	与推理帧率/追踪输出节拍匹配，约 4 拍（20 Hz）
自机姿态	300 ms	姿态链路更新较快，但缺失即禁止坐标运算
双目距离	500 ms	双目随推理帧更新，且距离不可用有独立降级动作

7.2 降级优先级（顺序即语义）

姿态不可用  >  视觉超龄  >  判丢  >  无距离（exit 档，持续达门限）

优先级	检查	理由
1	姿态过期/缺失	连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制）
2	视觉超龄	视觉是"是否判丢"的唯一来源；数据过期时连 tracked 标志本身都不可信，因此必须先于判丢处理
3	判丢	视觉可信且明确报告无轨道 → 零速刹车
4	无距离	只有在"视觉可信且正在跟踪"前提下才谈得上距离降级；exit 档还需持续无距离达门限（§7.5），默认档为 slow_approach（§10.8）

7.3 判丢刹车与状态清理

触发: tracked == false（且视觉未超龄、姿态可用）
动作: 输出 kBrakeHover（速度全零、航向全零）、valid = true
      进入沿清：距离 PID 全清 + 四个限幅器复位到 0

为什么用"进入沿"而不是每拍清：每拍清会把限幅器输出反复钉在 0，虽然结果 相同，但会掩盖"进入刹车"这一事件；用进入沿判定还能保证只清一次，语义清晰。 判定依据是"上拍模式 ≠ 刹车"，因此不需要额外的布尔标志。

为什么刹车要 valid = true：刹车是一条真实要发出去的指令（零速）， 与降级"什么都不发"有本质区别。

7.4 Coast（预测跟踪）

触发: is_predicted == true（追踪库用滤波器预测值补帧）
动作: 输出照常计算（目标中心为预测值），仅清距离 PID 积分；coast_active = true

为什么 Coast 期间仍照常算航向与垂直：Coast 是短暂补帧（数帧量级），预测中心 连续可用，此时停止控制反而会造成机体停顿—恢复的抖动。清积分的理由是：预测期间 误差不反映真实世界，积分累加会把预测偏差固化成饱和量。

Coast 期间距离通常不可用：目标短暂消失时双目失去匹配（distance_valid == false），第 ⑦ 步走无距离分支；默认 slow_approach 下前向按低速定速推进， 方位闭环继续（§7.5）。这是设计上的自然降级，不需要额外分支。

7.5 无距离三档动作

配置值	行为	适用场景
slow_approach（默认）	以配置的低速限幅缓速接近（仍经加速度限幅）	默认档：双目是机载唯一距离源，有效域外无距离是常态而非异常；即远场策略"方位闭环 + 定速接近"（D4）
hold	前向速度清零，限幅器复位到 0	安全占位语义：不知道距离就不前冲
exit	持续无距离达 no_distance_exit_grace_ms 才整体降级保持（kDegradedHold / kNoDistanceExit），不发送	距离是安全前提（如避撞）时的可配置安全策略；持续无距离 ≈ 传感器级故障

三档中只有 exit 会中断整拍输出，且延迟到 grace 期满；其余情况（含 exit 的 grace 期）仍输出航向与垂直通道（继续把目标保持在画面中心）。

exit 门限语义（替代旧版"首个无效拍即退出"）：

• distance_available 首次为 false 起计时，任一有效距离帧重置计时；
• grace 期内前向按 slow_approach 低速推进，复用既有加速度限幅，vx 平滑 过渡无跳变；
• 判丢刹车期间不计时（每拍清计时状态），Reset() 同样清除；
• no_distance_exit_grace_ms = 0 保留旧版行为（首个无效拍即退出）。

---

8. 输出打包与链路

8.1 控制意图 → 消息映射

模式	消息	关键填法
kVelocityHeading	ID 84 SET_POSITION_TARGET_LOCAL_NED（速度模式）	位置三字段填 0 并被掩码忽略；加速度三字段填 0 并被掩码忽略；速度填 NED 三分量；航向按掩码三分支二选一
kBrakeHover	ID 84（同上，全零速度）	速度全零，位置/加速度/航向/航向角速率字段全部被掩码忽略
kDegradedHold	——	不打包、不发送

当前实现已收敛为只使用一种消息类型（ID 84）。原设计中的姿态目标消息 （ID 82）随姿态微调支路一并取消（§10.1）。

8.2 忽略掩码语义与位定义

掩码语义：置位 = 忽略该字段。基础位组合（速度模式必备）：

基础掩码 = 忽略位置 x/y/z + 忽略加速度 afx/afy/afz
        = 1 + 2 + 4 + 64 + 128 + 256 = 455 = 0x01C7

航向两位取自本仓库随附的 MAVLink 定义（v3，common.xml / common.h）：

枚举	值	位序
YAW_IGNORE	1024 = 0x0400	bit 10
YAW_RATE_IGNORE	2048 = 0x0800	bit 11

⚠️ 版本偏移风险（必须实测确认）：这两个值之所以是 1024/2048，是因为该 版本的枚举在 AZ_IGNORE=256 之后插入了 FORCE_SET=512。较旧的 MAVLink 版本没有 FORCE_SET，其 YAW_IGNORE=512、YAW_RATE_IGNORE=1024， 位序整体低一位。若飞控端（PX4 1.11.3）随附的是旧枚举，本模块发出的掩码会被 解读为"使用 yaw、忽略 yaw_rate"，即把速率式指令误解为绝对航向 0（指向正北）。 上线前必须在飞控端核对枚举值，或在链路侧抓包比对行为。

8.3 三个掩码与三分支选择

掩码	组成	语义
速度+航向角	基础 | 0x0800	使用 yaw，忽略 yaw_rate（位置式）
速度+角速率	基础 | 0x0400	使用 yaw_rate，忽略 yaw（速率式，默认）
刹车	基础 | 0x0C00	yaw 与 yaw_rate 均忽略

分支选择规则（含"两者皆零"的处理）：

有角速率 ← |yaw_rate_dps| > 极小阈值
有航向角 ← |yaw_deg|      > 极小阈值

若 (非 有角速率) 且 有航向角:  用 速度+航向角掩码
否则:                          用 速度+角速率掩码

为什么"两者皆零"必须落到角速率掩码：此时若选航向角掩码，yaw = 0 会被 飞控当作**有效绝对航向 0（正北）**执行——机体无故转向正北。落到角速率掩码配 零角速率，语义是"保持当前航向"，才是期望行为。这是速率式默认配置下的常态 路径（β≈0 时角速率自然为 0），影响面很大。

8.4 单位换算清单（打包边界上的全部换算）

量	控制律内部	链路上	换算
航向角	deg	rad	× π/180
偏航角速率	deg/s	rad/s	× π/180
速度	m/s	m/s	无
时间戳	单调 ms	系统启动后 ms	直接传入（由调用方保证语义一致）

内部统一用度、边界统一转弧度的理由：控制参数（视场角、增益、限幅）在 度单位下便于人工整定与阅读；弧度只是链路协议要求。换算点集中在打包层， 控制律内部不出现弧度制航向。

8.5 打包与传输分离

控制律 ──控制意图──▶ 打包网关 ──字节流──▶ 传输抽象 ──▶ 物理链路
                     （懂协议）            （只懂字节）

分离带来的三个好处：

1. 可回放/可录制：把传输实现换成"写文件"即可离线录制全量指令流， 换成"回环打印"即可台架观测，控制律零改动；
2. 链路可替换：串口 ↔ 电台 ↔ 网络透明切换；
3. 协议知识集中：所有 MAVLink 细节只存在于打包层一处，控制律与配置层 完全不感知协议。

与实现语言相关的工程约束（重写时可忽略，但当前 C++ 落地必须遵守）： 随附的 MAVLink C 库把辅助函数以外部链接直接定义在头文件中（MAVLINK_HELPER 默认为空宏），多个编译单元包含同一协议头会产生重复定义链接错误。因此 协议头只允许被打包层那一个编译单元包含，对外头文件不得出现任何协议类型， 打包/发送缓冲以固定尺寸字节存储承载并在编译期断言尺寸足够。

8.6 发送节奏

• 控制频率由配置给定（占位 20 Hz），每拍一条指令；
• offboard 模式要求指令流持续：断流超过飞控侧超时阈值会触发 offboard 失效保护。因此 kDegradedHold（不发送）会让飞控自行进入失效保护—— 这是有意的"交还控制权"语义，但见 §12.2 的待确认项；
• 本模块不做重传、不做发送失败重试；发送失败仅返回布尔由调用方计数告警。

---

9. 配置参数全量表

配置在启动期一次性加载，运行期只读。缺字段、类型错误、非法取值一律 在加载期报错并携带完整字段路径（如 heading.gain），不允许带着半套 默认值进入运行期。

9.1 control 组：周期与超龄

键	单位	占位值	作用
frequency_hz	Hz	20.0	控制频率；同时作为 dt 兜底值
visual_stale_ms	ms	200	视觉结果超龄阈值
distance_stale_ms	ms	500	双目距离超龄阈值
attitude_stale_ms	ms	300	自机姿态超龄阈值

9.2 camera 组：画面与视场

键	单位	占位值	作用
image_width	px	1920	画面宽 W（β 计算与 x 偏移基准）
image_height	px	1080	画面高 H（α 计算与 y 偏移基准）
fov_h_deg	deg	60.0	水平视场角 θ_h
fov_v_deg	deg	35.0	垂直视场角 θ_v

画面尺寸必须与追踪库实际使用的画面尺寸一致；若推理在缩放图上运行， 这里填缩放后的尺寸，否则视线角会系统性偏差。

9.3 heading 组：水平航向

键	单位	占位值	作用
mode	枚举	rate	rate = 速率式（默认）/ position = 位置式
gain	见 §6.2	1.0	视线角 → 航向指令增益
yaw_rate_limit_dps	deg/s	30.0	偏航角速度幅值上限
yaw_slew_limit_dps2	deg/s²	60.0	航向指令变化率上限（两种模式共用，量纲待澄清）

9.4 vertical 组：垂直

键	单位	占位值	作用
vz_gain_mps_per_deg	(m/s)/deg	0.1	视线角 → 垂直速度增益
vz_limit_mps	m/s	2.0	垂直速度幅值上限

原双支路所需的"切换阈值"与"姿态微调增益/限幅"三项已随支路取消而移除。

9.5 distance 组：距离

键	单位	占位值	作用
d_exp_m	m	10.0	期望射程（由拦截设备接口给定）
kp / ki / kd	——	0.5 / 0.05 / 0.1	PID 三增益
integral_limit	m·s	2.0	积分项限幅（<=0 表示不限幅）
derivative_filter_coef	∈[0,1)	0.2	微分一阶低通系数（0 = 不滤波）
approach_velocity_limit_mps	m/s	3.0	接近速度上限
retreat_velocity_limit_mps	m/s	1.0	后退速度上限
no_distance_action	枚举	slow_approach	hold / slow_approach / exit（默认档依据见 §10.8）
no_distance_approach_limit_mps	m/s	0.5	slow_approach 档（含 exit 的 grace 期）的限速值
no_distance_exit_grace_ms	ms	3000	exit 档持续无距离门限（0 = 首个无效拍即退出）

9.6 accel_limit 组：加速度

键	单位	占位值	作用
ax_mps2	m/s²	2.0	前向加速度上限（距离通道限幅器）
ay_mps2	m/s²	2.0	侧向（预留：本控制律不做侧移，无对应限幅器）
az_mps2	m/s²	1.5	垂直加速度上限（垂直通道限幅器）

---

10. 设计取舍记录（为什么不是别的做法）

10.1 取消垂直"姿态微调"双支路，统一走速度支路

原设计：垂直误差小于切换阈值时发姿态目标消息做微调，大于阈值时发 速度设定值。

取消理由（三条，任一条都足以否决）：

1. 丢弃同周期的距离通道输出：姿态消息里没有前向速度字段。一旦本拍切成 姿态消息，距离通道算出的 vx 就被整拍丢弃 → 目标在画面中心附近时逼近 停滞（而"目标居中"恰恰是最常见的稳态）。
2. 消息类型逐拍翻转扰动飞控：目标在切换阈值附近抖动时，offboard 会收到 交替的两种消息类型，飞控侧设定值来源反复切换，行为难以预测。
3. 需要额外整定一套参数：切换阈值、微调增益、姿态角限幅、姿态角速率限幅 四项全部需要标定，而收益仅是"小误差时更细腻"。

替代方案的效果：小误差经增益线性映射为小垂直速度（1° → 0.1 m/s），行为 连续、无模式翻转、全周期单一消息类型，且少 4 个待标定参数。

10.2 航向默认改为速率式

理由：速率式指令是相对量，不存在"这是绝对航向还是修正量"的坐标系歧义； 位置式的 yaw 字段是绝对 NED 航向，一旦合成环节出错（漏加当前航向、姿态过期 用了旧航向），后果是机体转向一个完全无关的地理方向——故障模式危险且不直观。 速率式最坏情况是转速不对，不会指向错误方位。

代价：稳态需 β→0 才停转，可能有小幅极限环（可通过死区或降低增益抑制）。 位置式保留为可配置选项，但姿态接线未验证前不得用于实飞。

10.3 输出端补齐机体系 → NED 旋转

原状态：速度分量以机体系语义直发，而打包声明的坐标框是局部 NED—— 声明与实际不符。机头不朝北时，"前向 3 m/s"会被飞控理解为"北向 3 m/s"， 机体航向一变，速度方向就错。

修正：在控制律输出端（第 ⑧ 步）随快照航向旋转为 NED，使对外语义与打包 声明一致。限幅仍在机体系完成（正交变换保范，见 §4.4）。

10.4 视线角全程针孔、取消线性双路

见 §6.1：双路切换边界处约 8% 角度台阶，会向控制环注入突变；atan 代价可忽略。 同时消除了一个待标定参数。

10.5 PID anti-windup 采用"增量方向"条件积分

见 §6.5 判据对比表：另两种判据要么导致误差反号后无法回卷（积分残留）， 要么被大比例项掩盖饱和方向、拖延回卷。

10.6 限幅器初值为 0 兼作首条指令突变抑制

见 §6.6：不需要为"位置模式 → 速度模式切换"写专门分支，限幅器从零起步天然 满足"首条视觉速度设定值必须经过限幅"的要求；判丢刹车时复位到 0，恢复跟踪 同样从零爬升。

10.7 降级路径不清状态、刹车路径清状态

见 §5.3：降级是"这一拍不输出"，恢复后应平滑续接；刹车是真实控制中断， 恢复后必须从零渐变。两者语义不同，清理策略必须不同。

10.8 距离输入从雷达切换为双目：语义简化、默认动作与退出门限（2026-09-10）

背景：目标距离的机载唯一来源改为双目立体测距（决策 D2，替代激光雷达； 见 docs/双目深度估计思路.md）。

1. 关联标志语义简化（S2）：associated_to_target（相机—雷达空间关联） → distance_valid（本帧测距可用）。双目距离来自目标检测框自身，"回波未 打到目标"的跨模态关联问题消失，标志退化为纯粹的本帧可用性（域外/未匹配/ 一致性校验失败）；track_id 一致性校验转为调用方约定（§12.2）。
2. 默认无距离动作 hold → slow_approach（D4）：单雷达时代无距离多为 瞬时丢关联，hold 占位安全；双目时代有效域外无距离是常态，hold 会让 远场逼近停滞。slow_approach 即"方位闭环 + 定速接近"远场策略的库内承载。
3. exit 增加持续门限（S1）：旧版首个无效拍即降级，会把单帧丢检误判为 传感器级故障；改为持续无距离达 no_distance_exit_grace_ms 才退出，grace 期 按 slow_approach 行为执行（复用加速度限幅，无跳变），任一有效帧重置。 exit 语义收敛为"持续无距离 ≈ 传感器级故障"。

---

11. 实现与重写指引

11.1 必须持久化的状态（漏一个就变行为）

状态	所属	初值	何时改变
上拍时刻	主控律	0（表示首拍）	每拍写入当前时刻
上拍输出模式	主控律	降级保持	仅模式切换时写入（并触发日志）
无距离计时（进行中 + 起始时刻）	主控律	未计时	首个无距离拍启动（exit 档）；有效距离帧 / 判丢刹车 / 复位() 清除
积分累计	PID	0	每拍（除冻结）；清积分/全清时归零
上拍误差	PID	0	每拍
低通后微分	PID	0	每拍
上次输出	PID	0	每拍
微分历史有效标志	PID	false	首次更新后置 true；全清时回 false
上次输出 ×4	四个限幅器	0	每拍；复位时归零

构造期预计算量（不应在热路径重复计算）：针孔系数 ×2、PID 参数拷贝、 限幅器变化率上限 ×4、配置值拷贝。

11.2 时序与线程模型

推理侧（每帧，独立线程）:
    追踪库.更新(检测框, 画面尺寸)         # 内部维护轨道与主目标

控制线程（固定周期，配置频率）:
    结果 ← 追踪库.取最新结果()            # 线程安全拷贝
    快照 ← 采集(结果, 双目距离, 姿态)
    快照.track_time_ms ← 当前单调时钟()   # 就在取结果的那一刻打戳
    意图 ← 控制律.更新(快照, 当前单调时钟())
    按 意图.mode 分发:
        速度航向 → 网关.发送速度航向(意图, 启动后毫秒)
        零速刹车 → 网关.发送刹车(启动后毫秒)
        降级保持 → 不发送（记录降级原因/告警）

停止视觉闭环（状态机决定）:
    控制律.复位()                          # 清积分、清限幅器、清节拍与模式历史、清无距离计时

约束：

• 控制律实例只由控制线程调用，构件无锁；
• 采集快照与打戳必须尽量贴近（打戳晚于采集会把采集耗时算进"新鲜度"）；
• 追踪库取结果必须是拷贝而非引用，避免推理线程并发改写；
• 复位与更新不得并发。

11.3 数值与精度注意事项

事项	处理方式	理由
角度回绕	位置式合成后回绕到 [-180, 180)	链路侧与人工读数都按此区间
浮点"是否为零"	用极小阈值（如 1e-9）比较，不用 == 0	掩码分支选择依赖"航向/角速率是否有效"
dt 异常	dt <= 0 → 保持上次输出；dt > 0.5 s → 截断	除零保护与限幅器保护
首拍微分	不做微分（无历史）	避免首拍微分冲击
除法基准	画面尺寸、dt 均在构造期/入口校验为正	防除零
配置校验	加载期全量校验并抛错（含字段路径）	不允许带非法参数进入运行期

11.4 资源与日志纪律

• 热路径零动态分配：所有缓冲与系数在构造期备妥（若实现语言有 GC， 也应避免每拍新建对象造成抖动）；
• 热路径零日志：仅模式切换打一条 INFO；
• 异常日志节流：若实现中加入异常日志，必须按"第 1 次与每满 N 次"节流， 20 Hz 下不节流会在数秒内刷满日志；
• RAII / 确定性释放：传输实现、配置源在构造期注入，生命周期由调用方保证。

11.5 可测试性设计（构件边界即测试边界）

构件	是否纯函数	测试要点
超龄判定	是	三条判据各一例；边界（恰好等于有效期不超龄）
坐标旋转	是	§4.3 两条正方向自检；旋转保范；垂直分量不变
视线角转换	是	中心偏移为 0；针孔结果与线性近似明显偏离（证明未走线性）；非法参数报错
静态限幅	是	上/下界、界内不变
变化率限幅器	否（有状态）	单步不超限；不可超调；复位后从零爬升；dt<=0 保持
PID	否（有状态）	各项独立；积分钳位；误差反号后能回卷（旧错误判据在此会留在同侧饱和值）；两种复位语义
主控律	否（有状态）	八步流程各分支；降级优先级；模式切换日志只打一次
打包网关	否	字节头（起始字节、长度、消息 ID）；掩码三分支；单位换算；两者皆零走角速率掩码

测试策略：构件级用纯函数/小状态测试覆盖数学与边界；主控律级用构造快照 驱动分支覆盖；打包级直接解析字节验证线格式（不依赖协议库的解码函数， 避免"用同一份错误理解自我验证"）。

---

12. 验证路线与待定项

12.1 分阶段验证路线

阶段	目标	方法	通过判据
1. 构件级	数学正确	单元测试（无硬件）	全部用例通过
2. 控制律级	分支与时序正确	构造快照驱动 + 端到端合成场景回放	三分支（正常/Coast/判丢）行为符合 §7
3. 链路级	线格式正确	注入"回环打印/写文件"传输实现，解析字节	帧头、消息 ID、掩码、单位换算符合 §8
4. SITL	闭环行为	仿真飞控 + 回放/合成目标	目标居中收敛、无抖动、无积分饱和
5. 台架	实链路	真实串口/电台 + 飞控上电不转桨	offboard 建立、指令被接受、无失效保护
6. 实飞	标定整定	逐项标定增益与限幅	仅在 1–5 全部通过后

当前已完成阶段 1–3（构建零告警、单元测试全绿、端到端示例三阶段完整运行）。 阶段 4 起需要飞控与标定数据，属后续工作。

12.2 待定项清单

项	现状	需要谁确认	风险
全部数值参数	占位值	标定 + 台架整定	高：未标定不得实飞
忽略掩码位序	按随附 MAVLink v3 枚举（YAW_IGNORE=1024、YAW_RATE_IGNORE=2048）	核对飞控端 PX4 1.11.3 随附枚举	高：旧枚举无 FORCE_SET=512，位序低一位 → 速率指令被误解为绝对航向 0（§8.2）
坐标框取值	局部 NED	台架实测	中：与旋转语义必须一致
降级时是否应主动刹车	降级不发送，依赖飞控 offboard 断流失效保护	与安全策略负责人确认	中：断流后飞控行为（悬停/返航）取决于其配置，不由本模块决定
yaw_slew_limit_dps2 量纲	两种航向模式共用一个键；速率式下含义为 deg/s²，位置式下实际含义为 deg/s	启用位置式前拆分为两个键或重命名	低（默认速率式，位置式当前休眠）
侧向通道	ay_mps2 预留、机体侧向恒 0	若需要侧移则新增通道	低
机体补偿	快照的滚转/俯仰/角速度字段预留未用	接入主工程姿态源后评估	低
相机标定	内参/畸变/安装外参未标定，针孔模型无畸变补偿	标定	中：视线角系统性偏差
track_id 一致性校验	调用方约定：装配层校验双目距离与追踪主目标的 track_id 一致并告警，库内不实现（S3）	主程序装配层落地时加一处校验	中：错配会把其他目标的距离当作本目标，距离通道朝错误目标调射程

12.3 红线

1. 未标定不得实飞：配置与代码默认值中所有数值均为占位，标定与实测整定 完成前，仅限 SITL / 台架 / 录制数据回放验证数学链路。
2. 不得混用未标注坐标系的数据：对外输出的速度分量已声明为局部 NED， 任何改动都必须同步更新声明与打包侧坐标框。
3. 不得用超龄数据生成正常指令：超龄只能触发降级或告警。
4. 不得绕过限幅：任何新增通道都必须内联"变化率 + 幅值"双重限幅， 包括切换后的首条指令。
5. distance_valid == false 的距离不得当作目标距离（v1.0 仅单目检出时 不发布距离，方位退化为该目偏移，见 docs/双目深度估计思路.md §11.3）。

---

附：与追踪库的行为衔接速查

追踪库输出	本模块响应	位置
tracked == true、is_predicted == false	三路正常计算	§5.2
tracked == true、is_predicted == true	三路照常计算 + 清距离积分 + coast_active	§7.4
tracked == false	零速刹车 + 清积分与限幅器历史	§7.3
结果超过 visual_stale_ms 未更新	降级保持（视觉超龄），不刹车（标志不可信）	§7.2
主目标切换（追踪库内部选举）	本模块无感知：像素偏移自然跳变，由限幅器吸收	§6.6