42 lines
3.0 KiB
Markdown
42 lines
3.0 KiB
Markdown
# 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++ 一致，由移植测试锁定。
|