code-test-drone/utils/visual_track_control/README.md

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，默认 slow_approach；exit 需持续无距离达 no_distance_exit_grace_ms 才降级）
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++ 一致，由移植测试锁定。