# visual_track_control（Python 版）

视觉跟踪控制律：像素视线角 → 水平航向 / 垂直速度 / 距离 PID 三通道 + 统一限幅 +
状态机（正常 / Coast / 判丢刹车 / 降级保持）+ MAVSDK offboard 指令输出。
移植自 `reference-c++/visual_track_control/`（规格：docs/物理追踪思路.md）。

## 接口清单

| 名称 | 说明 |
| --- | --- |
| `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） |
| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit） |
| `TrackingControlLaw(cfg)` | 主控律 |
| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） |
| `.reset()` | 停止闭环时清全部内部状态 |
| `ControlSnapshot` | 输入快照（自带 `TrackState`，track_time_ms 由调用方打戳） |
| `ControlOutput` | 输出：mode / valid / vx·vy·vz（NED）/ vx_body_mps（机体系前向）/ yaw_deg / yaw_rate_dps / degraded_reason / coast_active |
| `PidController` / `PidParams` | 距离 PID（anti-windup 条件积分） |
| `SlewRateLimiter` / `clamp` | 变化率限幅（初值 0 → 首条指令渐变，S6） |
| `PixelToAngle` / `is_stale` / `rotate_body_to_ned` | 针孔视线角 / 超龄判定 / 机体→NED 旋转 |
| `CommandSender`（Protocol） | 指令输出抽象（供注入 mock） |
| `MavsdkCommandGateway(drone)` | MAVSDK offboard 网关（async：start / send_velocity_heading / send_brake_hover / stop） |

## 与 C++ 版的差异

- **MAVLink → MAVSDK 语义映射**（替代 C++ 的 ID 84 手工编码）：
  - 角速率非零 → `set_velocity_body(forward, 0, down, yawspeed)`；
  - 航向角非零（角速率为零）→ `set_velocity_ned(north, east, down, yaw)`；
  - 两者均零 → `set_velocity_body` 配零角速率（保持航向，A2 回归语义）；
  - 刹车 → `set_velocity_body(0,0,0,0)`（MAVSDK 无刹车掩码的行为等效适配）；
  - 降级保持不发送（由调用方判断模式）。
- **异步**：MAVSDK Python 为 asyncio 接口，网关方法均为协程；传输异常捕获后返回
  `False`（与 C++ 传输失败传播一致）。
- **解耦**：C++ 的 ControlSnapshot 内嵌 `target_tracker::TrackResult` 是唯一耦合点；
  Python 版自带字段对齐的 `TrackState`（box/raw 为 plain tuple），两包互不 import，
  转换在胶水层（`utils/demo_track_control.py` 的 `track_result_to_state`）。
- **新增字段** `ControlOutput.vx_body_mps`：旋转前的机体系前向速度，供速率式
  机体设定点直接使用（免反旋）。
- **日志**：spdlog → 标准 `logging`，仅状态切换点打印（热路径零日志）不变。
- 行为语义（姿态超龄优先于视觉、判丢进刹车清限幅器、Coast 清距离积分、
  dt 兜底 1/frequency_hz 上限 0.5s、WrapDeg180 回绕）与 C++ 一致，由移植测试锁定。