first commit

reference-c++/visual_track_control/视觉跟踪控制律实现文档.md

@@ -0,0 +1,443 @@
+# 视觉跟踪控制律实现文档（visual_track_control）
+
+> 独立静态库子工程，实现 `../docs/物理追踪思路.md`（下称"规格"）定义的
+> 视觉跟踪阶段（S7 VISUAL_TRACKING）控制律：水平航向、垂直速度、
+> 距离 PID 三路控制 + 统一限幅 + Coast 清积分 / 判丢刹车 + 超龄降级，
+> 输出端随机体航向完成机体系→局部 NED 旋转，经 `MavlinkCommandGateway`
+> 打包为 MAVLink 字节流，交由注入的 `ICommandTransport` 实现送达。
+>
+> 本文档按 `../AGENTS.md`"实现文档规范"六要素组织。三份文档分工明确、
+> 互不重复：
+>
+> - `../docs/物理追踪思路.md`——控制律**物理与行为规格**（唯一权威），
+>   本文档不重复抄录公式推导；
+> - `../docs/视觉跟踪控制律技术路线.md`——**语言无关实现路线**：单周期八步
+>   伪代码、构件与状态清单、坐标系与符号约定、打包掩码取舍、重写指引；
+> - 本文档——**C++ 落地说明**：公共头一览、构建与测试方式、日志行为、
+>   排查与修改要点。
+>
+> 追踪库行为约定见 `../target_tracker/target_tracker.md`，同样只引用不复述。
+
+---
+
+## 一、功能职责与边界
+
+命名空间 `drone::vtc`，头文件保护一律 `#pragma once`。
+
+**做什么**：
+
+- **三路控制律**（`TrackingControlLaw::Update()` 单周期计算）：
+  - 水平航向（规格 §4）：像素偏移 → 水平视线角 β → 增益 → 变化率限幅 →
+    角速度限幅，按 `HeadingMode` 配置输出速率式 `yaw_rate_dps`（默认）
+    或位置式 `yaw_deg`（绝对 NED 航向，见 §3.8）；
+  - 垂直速度（规格 §5）：垂直视线角 α → 统一速度支路 → `vz_mps`
+    垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
+    原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
+  - 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
+    `vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级
+    （hold / slow_approach / exit）。
+- **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
+  `SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
+  全部在机体系完成。
+- **坐标系旋转**（规格 §2.3）：输出端用 `RotateBodyToNed()` 把机体系
+  速度（前/右/下）随快照航向 ψ 旋转为局部 NED（北/东/地），与网关的
+  `MAV_FRAME_LOCAL_NED` 一致；仅绕下轴旋转（滚转/俯仰对速度指令为
+  二阶小量，由飞控内环处理）。
+- **Coast 清积分**（规格 §6.4）：`is_predicted == true` 时清空距离 PID
+  积分项（`ResetIntegral()`），防积分饱和。
+- **判丢刹车**（规格 §6.4）：`tracked == false` 时输出 `kBrakeHover`
+  零速刹车，并清积分与全部限幅器历史（保证恢复跟踪后指令从零渐变）。
+- **超龄降级**（规格 §7.1、§9.2）：姿态过期/缺失 → `kDegradedHold`
+  （`kAttitudeStale`，禁止控制）；视觉结果超龄 → `kDegradedHold`
+  （`kVisionStale`）；判定器为无状态 `StalenessChecker::IsStale()`。
+- **MAVLink 打包**（规格 §2.2）：`MavlinkCommandGateway` 把
+  `ControlOutput` 打包为 `SET_POSITION_TARGET_LOCAL_NED`（ID 84）字节流。
+- **配置化**（规格 §8、需求分析 NFR-007）：全部阈值集中在
+  `TrackingConfig`，由 `TrackingConfig::LoadFromJson()` 启动期一次性加载，
+  代码不写死任何参数。
+
+**不做什么**（边界）：
+
+- **不做感知**：不运行检测/光流/雷达驱动，只消费调用方填入的
+  `ControlSnapshot`；
+- **不做状态机仲裁**：S6/S8 切换、目标丢失策略（`ITargetLostStrategy`）、
+  障碍接管均在状态机层，本库只按输入标志产生当拍输出；
+- **不写串口/传输**：库内无任何链路代码，字节流交给 `ICommandTransport`
+  实现方；
+- **不解析 MAVLink 输入**：只有打包发送方向，无 `mavlink_parse_*`；
+- **不做机体补偿**：`ControlSnapshot` 的 `roll_rad` / `pitch_rad` /
+  `yaw_rate_radps` / `velocity_*` 字段为预留输入（规格 §4.1）；当前仅
+  `yaw_rad` 参与计算（NED 旋转与位置式航向合成）；
+- **不做热更新**：配置启动期加载一次，调参重启生效。
+
+---
+
+## 二、接口与数据流
+
+### 2.1 公共头一览
+
+| 头文件 | 内容 |
+|--------|------|
+| `include/visual_track_control/vtc_types.h` | `ControlMode`、`DegradedReason`、`ControlSnapshot`、`ControlOutput` |
+| `include/visual_track_control/vtc_config.h` | `HeadingMode`、`NoDistanceAction`、各分组配置结构、`TrackingConfig` |
+| `include/visual_track_control/tracking_control_law.h` | `TrackingControlLaw`（主控律） |
+| `include/visual_track_control/pid_controller.h` | `PidParams`、`PidController` |
+| `include/visual_track_control/rate_limiter.h` | `Clamp()`、`SlewRateLimiter`（header-only） |
+| `include/visual_track_control/staleness_checker.h` | `StalenessChecker`（header-only） |
+| `include/visual_track_control/line_of_sight.h` | `PixelToAngle`（像素→视线角，全程针孔模型） |
+| `include/visual_track_control/frame_transform.h` | `RotateBodyToNed()`（机体系→局部 NED，header-only） |
+| `include/visual_track_control/i_command_transport.h` | `ICommandTransport`（传输抽象） |
+| `include/visual_track_control/mavlink_command_gateway.h` | `MavlinkCommandGateway`（MAVLink 打包） |
+
+### 2.2 关键字段（以代码为准）
+
+`enum class ControlMode`：`kVelocityHeading`（速度航向主输出）/
+`kBrakeHover`（零速刹车）/ `kDegradedHold`（降级保持，禁止正常控制）。
+（原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
+
+`enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
+`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。
+
+`ControlSnapshot`（输入，纯值类型）：
+
+| 字段 | 含义 |
+|------|------|
+| `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） |
+| `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** |
+| `radar_distance_m` | 机头前向单点距离（米） |
+| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） |
+| `radar_time_ms` | 雷达取样单调时钟 |
+| `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） |
+| `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 |
+| `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） |
+| `velocity_x_mps` / `velocity_y_mps` / `velocity_z_mps` | 自机速度分量（预留，接入主工程时由 `FlightStateSnapshot` 填充） |
+
+`ControlOutput`（输出，已限幅）：
+
+| 字段 | 含义 |
+|------|------|
+| `mode` / `valid` | 输出模式 / 指令是否可用于发送 |
+| `vx_mps` / `vy_mps` / `vz_mps` | **局部 NED 速度分量（北/东/地，米/秒）**：各通道在机体系计算并限幅后，输出端随 `snapshot.yaw_rad` 旋转得到；旋转为正交变换，合速度大小不变。机体系侧向恒为 0，旋转后 `vy_mps` 可非零 |
+| `yaw_deg` | 位置式目标**绝对航向**（度，NED，右转为正）：由当前航向 + 变化率限幅后的修正量合成，回绕到 [-180, 180)（ID 84 的 yaw 字段为绝对值） |
+| `yaw_rate_dps` | 速率式偏航角速度指令（度/秒，右转为正；天然是相对量，无坐标系歧义） |
+| `degraded_reason` | 降级原因（仅 `kDegradedHold` 有意义） |
+| `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
+
+`TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、
+`radar_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、
+`image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、
+`yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical`
+（`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、
+`kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、
+`approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、
+`no_distance_action`、`no_distance_approach_limit_mps`）、`accel_limit`
+（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
+`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
+相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
+
+### 2.3 ICommandTransport 契约
+
+```cpp
+class ICommandTransport {
+public:
+    virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
+};
+```
+
+- **只吃字节流**：串口、电台、回环打印均可注入实现；后续重写传输层
+  只换实现，不动打包与控制律；
+- 返回 `true` = 已交付底层链路，`false` = 发送失败（调用方可计数/告警）；
+- 实现方保证不长期持有 `data` 指针（同步拷贝或同步写出）。
+
+### 2.4 数据流向
+
+```text
+推理侧每帧: TargetTracker::Update(detections, frame_shape)
+控制线程(固定周期, 规格 §7.1):
+    TargetTracker::LastResult()          # 线程安全拷贝
+    → 取样瞬间用单调时钟打戳填入 ControlSnapshot.track_time_ms
+    → TrackingControlLaw::Update(snapshot, now_ms)   # 单周期计算
+    → 按 ControlOutput.mode 分发:
+        kVelocityHeading → MavlinkCommandGateway::SendVelocityHeading()
+        kBrakeHover      → MavlinkCommandGateway::SendBrakeHover()
+        kDegradedHold    → 不发送（仅告警/降级）
+    → mavlink_msg_to_send_buffer → ICommandTransport::Send(字节流)
+停止视觉闭环: TrackingControlLaw::Reset()（规格 §6.4）
+```
+
+---
+
+## 三、关键实现点
+
+### 3.1 传输接口只吃字节流的双重解耦
+
+打包（`MavlinkCommandGateway`）与传输（`ICommandTransport`）分离：
+网关只负责 `ControlOutput` → MAVLink 字节，不感知链路形态；传输实现
+只感知字节，不感知消息语义。对应规格 §2.2 控制单出口约束在独立库中的
+映射——控制律不直接写飞控。
+
+### 3.2 TrackResult 无时间戳，由调用方取样打戳
+
+`TrackResult` 是追踪库自有类型，本身不含时间戳（规格 §7.2）。约定：
+调用方在读取 `LastResult()` 的**瞬间**用单调时钟打戳填入
+`track_time_ms`；超龄判定一律用"取样时间 + `valid_for_ms`"
+（`StalenessChecker::IsStale(sample_time_ms, now_ms, valid_for_ms)`）：
+
+- `sample_time_ms <= 0` 视为从未取样，直接判超龄；
+- `now_ms < sample_time_ms`（时间戳不一致：时钟源错配/打戳错误）同样
+  判超龄——单一单调时钟下不应发生，宁可降级也不信任不可核实的取样。
+
+### 3.3 PID anti-windup 策略（条件积分）
+
+`PidController::Update()`（位置式）三层防护：
+
+1. **积分钳位**：候选积分值先钳制在 ±`integral_limit`；
+2. **anti-windup（条件积分）**：输出饱和（`|raw| > output_limit`）且
+   **本拍积分增量仍在推深饱和**（增量 `error·dt` 与原始输出 `raw`
+   同号）时，冻结本步积分累加（不采纳候选积分值）；误差反号后增量反向，
+   积分立即恢复回卷，无饱和残留。判据刻意基于"增量方向"而非"积分项与
+   输出同向"：后者在误差反号后仍会冻结回卷；也不用 `raw·error` 的
+   变体含混表述，实现即 `(error*dt_s) * raw > 0`；
+3. **输出限幅**：最终输出钳制在 ±`output_limit`。
+
+微分带一阶低通（`derivative_filter_coef ∈ [0,1)`，0 = 不滤波）；
+`dt_s <= 0` 时保持上次输出（异常节拍保护）。`ResetIntegral()` 只清积分，
+保留微分历史与上次输出；`Reset()` 清全部，均 O(1)（Coast 清积分，
+规格 §6.4）。
+
+### 3.4 SlewRateLimiter 兼作首条指令突变抑制
+
+`SlewRateLimiter` 保存上次输出，单步变化不超过 `rate_limit · dt_s`；
+构造时初值置 0，天然实现规格要求的"首条视觉速度设定值必须经过限幅"
+（状态机设计 S6）：切换进入视觉跟踪后首条指令从零按加速度限幅渐变，
+无需专门的切换分支。判丢进刹车时调用 `Reset()` 归零，恢复跟踪后同样
+从零渐变。主控律共 4 个限幅器：`yaw_deg_limiter_` / `yaw_rate_limiter_`
+（度/秒²）、`vx_limiter_` / `vz_limiter_`（米/秒²）。
+
+### 3.5 像素→角度：全程针孔模型
+
+`PixelToAngle` 构造期预计算针孔系数 `2·tan(θ/2)/size`，热路径仅一次
+乘加 + `atan`：`angle = atan(offset·2·tan(θ/2)/size)`（规格 §4.1/§5.1）。
+原"小偏移线性近似/大偏移针孔"双路切换已取消：线性式与针孔式在切换边界
+两侧不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入
+突变；`atan` 代价可忽略，全程采用精确针孔式。配置中 `linear_ratio` 键
+随之移除。水平 β（`PixelToAngle(W, θ_h)`）与垂直 α（`PixelToAngle(H, θ_v)`）
+各一个实例，构造期校验参数合法性（尺寸、视场角），非法抛
+`std::invalid_argument`。
+
+### 3.6 全工程仅 mavlink_command_gateway.cpp 一个 TU 包含 MAVLink 头
+
+c_library_v2 的 `mavlink_helpers.h` 中 **`MAVLINK_HELPER` 默认为空宏**，
+辅助函数直接以外部链接定义在头文件中；多个翻译单元包含 `common/mavlink.h`
+会产生**多重定义**链接错误。因此：
+
+- `common/mavlink.h` 只被 `src/mavlink_command_gateway.cpp` 包含
+  （该文件也是全库唯一的 MAVLink 知识落点）；单元测试同样**不得**包含
+  该头，按 MAVLink v2 线格式手工解码字节流断言（见 §5.3）；
+- 公共头 `mavlink_command_gateway.h` 不出现任何 `mavlink_*` 类型，
+  打包/发送缓冲用成员固定字节存储承载（`msg_storage_` / `tx_buffer_`，
+  各 512 字节，`.cpp` 中 `static_assert` 校验 ≥ `sizeof(mavlink_message_t)`
+  与 `MAVLINK_MAX_PACKET_LEN`）；
+- 打包用 `mavlink_msg_*_pack`（无通道状态、纯静态内联、可重入），
+  不使用 `_pack_chan` 通道 API；
+- CMake 中 MAVLink include 目录设为 `SYSTEM PRIVATE`：压第三方告警噪音，
+  且不泄漏给下游。
+
+同理，`nlohmann/json` 只被 `src/vtc_config.cpp` 一个 TU 包含。
+
+### 3.7 配置启动期一次加载，不做热更新
+
+`TrackingConfig::LoadFromJson()` 仅在启动期调用一次：解析结果拷贝进
+`TrackingConfig` 值对象后即与 JSON 脱钩，热路径绝不触碰 JSON 对象；
+调参重启即可。若未来需要热更新，扩展点为**原子指针交换**：持有
+`std::atomic<const TrackingConfig*>`，后台解析新配置成功后原子换指针，
+控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。
+
+### 3.8 主控律单周期流程
+
+`Update()` 按固定流程执行（`last_now_ms_` 推算 dt，首拍与异常节拍
+（时钟回拨/`now_ms` 未前进）用 `1.0 / control.frequency_hz` 兜底，
+dt 上限 `kMaxDtS = 0.5` 秒）：
+
+① 超龄判定（姿态过期/缺失 → `kDegradedHold`；视觉超龄 → `kDegradedHold`）
+→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
+→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
+→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
+→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按
+`no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
+随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
+
+两个设计决策记录：
+
+- **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
+  → ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
+  消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息类型
+  逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路：小误差经
+  `vz_gain` 线性映射为小垂直速度，行为连续、全周期单一 offboard 消息。
+- **位置式航向输出绝对值（A2）**：ID 84 的 `yaw` 字段是**绝对** NED
+  航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
+  位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式
+  （默认）输出角速度指令，天然相对量、无此问题。
+
+### 3.9 MAVLink 消息映射（规格 §2.2）
+
+全库仅 ID 84 `SET_POSITION_TARGET_LOCAL_NED` 一种消息（原 `kAttitudeTrim`
+→ ID 82 `SET_ATTITUDE_TARGET` 已随姿态微调支路移除）：
+
+| 模式 | 消息 | 要点 |
+|------|------|------|
+| `kVelocityHeading` | ID 84 | `coordinate_frame = MAV_FRAME_LOCAL_NED`（速度分量已是 NED 语义）。速度模式 `type_mask` 基础位 `kMaskVelocityBase = 0x01C7`（忽略位置与加速度），航向三分支：角速率非零 → `kMaskVelocityYawRate`（再忽略 yaw）；仅航向角非零 → `kMaskVelocityYaw`（再忽略 yaw_rate）；**两者均零 → 速率掩码配零角速率（保持当前航向）**——旧实现把零角速率落到航向掩码，等于误发绝对航向 0（指向正北） |
+| `kBrakeHover` | ID 84 | `kMaskBrake = 0x0DC7`：速度全零，位置/加速度/航向/角速率字段全部忽略 |
+
+---
+
+## 四、日志行为
+
+遵循 `../AGENTS.md` 日志纪律：**库内仅状态切换点打日志，热路径零日志、
+零堆分配**。
+
+| 位置 | 日志 | 触发条件 |
+|------|------|----------|
+| `tracking_control_law.cpp`（`Update()` 内 `FinishAndLog`） | `spdlog::info` "[ControlLaw] 状态切换: X -> Y (原因)" | **仅当 `ControlMode` 相对 `last_mode_` 发生变化**时打印（刹车进出、降级切换、进入/退出 Coast 跟踪）；同一模式连续多拍不重复打印 |
+| `tracking_control_law.cpp` | —— | 每拍正常计算（含三路通道数学）**不打任何日志** |
+| `mavlink_command_gateway.cpp` / `pid_controller.cpp` / `line_of_sight.cpp` / `vtc_config.cpp` | —— | 不打日志（配置错误走异常，见第六章） |
+
+示例程序（`examples/track_control_demo.cpp`，非生产代码）另有两处
+**演示观测日志**，均带节流防刷屏：
+
+- `LoggingTransport::Send()`：字节统计与包头十六进制摘要，**秒级节流**
+  （距上次打印 ≥ 1000 ms 才输出）；
+- 主循环状态摘要：**每 10 拍一次**（20 Hz 下即半秒一次）。
+
+---
+
+## 五、构建、测试与运行方式
+
+### 5.1 构建
+
+`../AGENTS.md` 规定构建环境为 WSL2 Ubuntu 24.04。**当前开发机 WSL 仅
+装有 Ubuntu 22.04（发行版名 `Ubuntu`）**，本库的构建与测试验证在
+22.04（cmake 3.22 / g++ 11.4）完成，行为与 24.04 无差异；24.04 发行版
+安装后应按其复核一遍（见 §6.4）。
+
+```bash
+cmake -S visual_track_control -B build_vtc -DBUILD_TESTING=OFF
+cmake --build build_vtc -j$(nproc)
+./build_vtc/track_control_demo
+```
+
+- **为什么 `-DBUILD_TESTING=OFF`**：本库以 `add_subdirectory` 零改动复用
+  `target_tracker`，而 `target_tracker/CMakeLists.txt` 中 `include(CTest)`
+  会把 `BUILD_TESTING` 默认置为 `ON`，从而连带构建追踪库的 3 个测试
+  可执行文件。本子工程不关心其测试，显式关闭以加快构建（追踪库自身的
+  测试方式见 `../target_tracker/target_tracker.md` §五）。
+- **本库自身测试不复用 `BUILD_TESTING`**：由独立选项 `VTC_BUILD_TESTS`
+  （默认 `ON`）控制，与上面的关闭互不干扰。
+- **示例无需 `LD_LIBRARY_PATH`**：`target_tracker` 为 SHARED 库，
+  构建期 `CMAKE_BUILD_RPATH` 已指向构建目录，`track_control_demo`
+  可直接运行。
+- 示例缺省配置路径由编译期宏 `VTC_DEMO_DEFAULT_CONFIG` 内置为
+  `config/track_control.json` 的绝对路径；也可命令行传入
+  `track_control_demo [配置文件路径]`。
+
+### 5.2 示例演示内容
+
+单线程模拟完整链路（合成运动检测框 → `TargetTracker::Update()` →
+`LastResult()` + 打戳 → `TrackingControlLaw::Update()` → Gateway →
+`LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
+（对应规格 §6.4）：
+
+1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达
+   关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
+   角速度随 β 收敛而减小）；
+2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
+   继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold：
+   前向清零）；
+3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
+   零速刹车。
+
+示例将追踪库判丢阈值放大为 `kDemoMaxLostFrames = 60` 帧（默认 8 帧在
+20 Hz 下 Coast 仅 0.4 秒不便观察），**仅演示专用**，正式参数以追踪库
+规格为准。程序跑完总拍数后自行调用 `TrackingControlLaw::Reset()` 并
+正常退出（返回 0），无需手动终止。
+
+### 5.3 单元测试（Google Test）
+
+测试源文件位于 `tests/`，覆盖 8 个测试套件：
+
+| 测试文件 | 覆盖要点 |
+|----------|----------|
+| `rate_limiter_test.cpp` | `Clamp` 边界；`SlewRateLimiter` 单步限幅、`dt<=0`/冻结保护、`Reset` |
+| `staleness_checker_test.cpp` | 从未取样、时钟回拨（`now < sample`）、恰在有效期边界（临界判新鲜） |
+| `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 |
+| `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 |
+| `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` |
+| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
+| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 |
+| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
+
+运行方式（构建后）：
+
+```bash
+./build_vtc/vtc_unit_tests                      # 直接跑
+ctest --test-dir build_vtc --output-on-failure  # 或经 ctest（gtest_discover_tests 注册）
+```
+
+依赖：系统安装 Google Test（`libgtest-dev`，FindGTest 模块模式，无需
+CMake config 包）；未找到时自动跳过测试构建并打印状态，不影响库与示例。
+`vtc_config_test` 通过编译期宏 `VTC_TEST_CONFIG` 读取真实配置文件
+`config/track_control.json`——**修改该配置文件时须同步核对测试断言**。
+
+### 5.4 部署提示
+
+`target_tracker` 为 SHARED 库（`libtarget_tracker.so.1`）：部署
+`track_control_demo` 或未来接入主工程时，必须将该 so 一并随带
+（目录式部署与 RPATH 约定见 `../target_tracker/target_tracker.md` §7）。
+本库 `visual_track_control` 自身为 STATIC 库，链接进可执行文件，无需随带。
+
+---
+
+## 六、排查 / 修改要点
+
+### 6.1 配置缺字段 / 非法值报错定位
+
+`LoadFromJson()` 缺字段、类型错误、非法取值均抛 `std::runtime_error`，
+**异常消息携带完整字段名**（如 `配置缺失字段或类型错误: heading.gain`、
+`配置非法(必须为正): control.frequency_hz`、
+`配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit`），
+按异常文本直接定位到 `config/track_control.json` 的对应分组与键即可；
+文件打开/解析失败则携带文件路径。`PixelToAngle` 构造期参数非法（尺寸
+非正、视场角不在 (0, π)）抛 `std::invalid_argument`，同样见消息文本。
+
+### 6.2 MAVLink 消息待实测适配项
+
+以下各项均以 **PX4 1.11.3 Offboard 实测为准**（规格 §10.1 待定项），
+全部集中在 `src/mavlink_command_gateway.cpp`，修改时只动这一个文件：
+
+| 适配项 | 位置 | 现状 |
+|--------|------|------|
+| `coordinate_frame` | `SendVelocityHeading()` / `SendBrakeHover()` 的 `mavlink_msg_set_position_target_local_ned_pack` 调用 | 固定取 `MAV_FRAME_LOCAL_NED`；速度分量已由主控律旋转为局部 NED 语义（§3.8 步骤⑧），语义是否匹配待实测确认 |
+| 速度模式 `type_mask` | 匿名命名空间 `kMaskVelocityBase` / `kMaskVelocityYaw` / `kMaskVelocityYawRate` / `kMaskBrake` | 掩码位组合待实测确认 |
+
+### 6.3 接入主工程时需补齐的事项
+
+1. **机体补偿（可选）**：机体系→NED 旋转已在库内完成（仅绕下轴按
+   `yaw_rad`，规格 §2.3）；`roll_rad` / `pitch_rad` / `yaw_rate_radps` /
+   `velocity_*` 字段仍为预留，规格 §4.1 的机体补偿启用时由姿态源填充；
+2. **`TrackedTarget` 主题接线**：`TrackResult` 尚未进入主工程
+   `include/common/types.h`；需先按 `../target_tracker/target_tracker.md`
+   §7 与规格 §7.2 正式定义 `TrackedTarget` 消息与 `kTrackedTarget` 主题，
+   由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
+3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
+   （`ICommandTransport` 注入），打包与控制律零改动；
+4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按
+   规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。
+
+### 6.4 红线声明
+
+- `config/track_control.json` 与 `TrackingConfig` 默认值中**所有数值均为
+  占位值**（对应规格第 8 章待定项与需求分析 §7 未标定前提）：相机内参/
+  畸变/外参未标定、PID 与限幅参数未整定。标定与实测整定完成前，本库
+  **仅限 SITL/台架录制数据回放验证数学链路，不得用于实飞**。
+- 构建环境差异：`../AGENTS.md` 规定 WSL2 Ubuntu 24.04，当前开发机仅
+  装有 Ubuntu 22.04，验证在 22.04 完成；安装 24.04 后需复核构建与测试。