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

---

## 1. 概述与系统定位

### 1.1 模块职责

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

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

### 1.2 在系统中的位置

```text
┌─────────────┐   检测框    ┌──────────────┐  追踪结果   ┌───────────────────┐
│ 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 传输抽象契约（唯一出向依赖）

```text
接口 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 像素偏移与视线角符号

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

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

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

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

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

```text
给定机体速度 (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 节拍推算

```text
函数 计算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 八步主流程

```text
函数 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 模式切换日志（唯一日志点）

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

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

---

## 6. 各通道算法细节

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

```text
构造期（水平、垂直各一个实例）:
    校验 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 航向**，必须与当前航向合成；依赖新鲜姿态 |

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

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

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

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

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

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

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

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

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

### 6.4 距离通道

```text
Δ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 内部算法

```text
函数 更新(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 变化率限幅器

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

**两个必须知道的性质**：

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

---

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

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

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

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

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

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

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

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

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

### 7.3 判丢刹车与状态清理

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

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

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

### 7.4 Coast（预测跟踪）

```text
触发: 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 忽略掩码语义与位定义

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

```text
基础掩码 = 忽略位置 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` 均忽略 |

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

```text
有角速率 ← |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 打包与传输分离

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

分离带来的三个好处：

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 时序与线程模型

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

控制线程（固定周期，配置频率）:
    结果 ← 追踪库.取最新结果()            # 线程安全拷贝
    快照 ← 采集(结果, 双目距离, 姿态)
    快照.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 |