双目测距方案重构与其他小修改

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

@@ -34,8 +34,9 @@
     垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
     原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
   - 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
-    `vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级
-    （hold / slow_approach / exit）。
+    `vx_mps`；目标距离无效/超龄时按 `NoDistanceAction` 配置降级
+    （hold / slow_approach / exit，exit 需持续无距离达
+    `no_distance_exit_grace_ms` 门限，见 §3.8）。
 - **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
   `SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
   全部在机体系完成。
@@ -96,7 +97,8 @@
 （原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
 
 `enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
-`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。
+`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（持续无距离达门限且配置
+退出近距操作）。
 
 `ControlSnapshot`（输入，纯值类型）：
 
@@ -104,9 +106,9 @@
 |------|------|
 | `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` | 雷达取样单调时钟 |
+| `target_distance_m` | 目标距离（米，双目立体测距前向分量 Z） |
+| `distance_valid` | 距离有效标志；**无效不得当目标距离**（规格 §6.3、需求分析 FR-041）。距离来自目标检测框自身的双目测距，无跨模态关联问题 |
+| `distance_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） |
@@ -124,15 +126,18 @@
 | `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
 
 `TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、
-`radar_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、
+`distance_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`
+`no_distance_action`、`no_distance_approach_limit_mps`、
+`no_distance_exit_grace_ms`）、`accel_limit`
 （`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
 `config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
+`distance.no_distance_action` 默认 `slow_approach`（双目有效域外无距离
+是常态，对齐 D4 远场定速接近策略，见《双目测距方案讨论纪要》S1）；
 相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
 
 ### 2.3 ICommandTransport 契约
@@ -263,11 +268,11 @@ dt 上限 `kMaxDtS = 0.5` 秒）：
 → ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
 → ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
 → ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
-→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按
+→ ⑦ 距离通道（`distance_valid` 且未超龄才可用；否则清积分并按
 `no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
 随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
 
-两个设计决策记录：
+三个设计决策记录：
 
 - **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
   → ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
@@ -278,6 +283,14 @@ dt 上限 `kMaxDtS = 0.5` 秒）：
   航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
   位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式
   （默认）输出角速度指令，天然相对量、无此问题。
+- **kExit 持续无距离门限（纪要 S1）**：换双目后有效域外无距离是常态
+  （纪要 D4 远场定速接近），首个无效拍即退出会频繁降级。`kExit` 现需
+  连续无距离达 `no_distance_exit_grace_ms` 才降级（语义收敛为"疑似
+  传感器级故障"），grace 期间按 `kSlowApproach` 行为执行（经加速度
+  限幅，vx 平滑过渡），任一有效距离帧重置计时；`no_distance_exit_grace_ms
+  = 0` 保留旧"立即退出"行为。`no_distance_action` 默认值随之
+  `kHold` → `kSlowApproach`。判丢刹车期间不计无距离时长，`Reset()`
+  清计时状态。
 
 ### 3.9 MAVLink 消息映射（规格 §2.2）
 
@@ -347,12 +360,12 @@ cmake --build build_vtc -j$(nproc)
 `LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
 （对应规格 §6.4）：
 
-1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达
-   关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
+1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，双目
+   距离有效，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
    角速度随 β 收敛而减小）；
 2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
-   继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold：
-   前向清零）；
+   继续预测跟踪；同时置距离无效，演示 §6.3 无距离降级路径（配置为
+   `slow_approach`：按限速值缓速前向）；
 3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
    零速刹车。
 
@@ -372,8 +385,8 @@ cmake --build build_vtc -j$(nproc)
 | `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 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 |
+| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道无距离降级动作（含 kExit 门限、grace 有效帧重置、零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
+| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、默认降级动作为 slow_approach、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界/grace 为负均抛异常且消息携带字段名 |
 | `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
 
 运行方式（构建后）：
@@ -430,8 +443,11 @@ CMake config 包）；未找到时自动跳过测试构建并打印状态，不
    由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
 3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
    （`ICommandTransport` 注入），打包与控制律零改动；
-4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按
-   规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。
+4. **距离有效标志来源**：`distance_valid` 由双目链路（StereoRanger）
+   给出——距离来自目标检测框自身，控制律不自行判定；多目标场景下
+   调用方填 `ControlSnapshot` 时应校验 `StereoTargetDistance.track_id`
+   与 `TrackResult.track_id` 一致（v1.0 单目标为调用方约定，纪要 S3，
+   库内不加代码）。
 
 ### 6.4 红线声明