first commit

utils/visual_track_control/README.md

@@ -0,0 +1,41 @@
+# 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++ 一致，由移植测试锁定。

utils/visual_track_control/__init__.py

@@ -0,0 +1,55 @@
+"""视觉跟踪控制包（visual_track_control Python 版）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/。本包与 target_tracker
+包互不依赖（TrackState 为字段对齐的独立值类型，胶水层负责转换）。
+
+分层：
+  纯算法构件   rate_limiter / staleness_checker / line_of_sight /
+               frame_transform / pid_controller
+  配置         config（TrackingConfig + load_from_json）
+  主控律       tracking_control_law（TrackingControlLaw）
+  指令输出     command_sender（抽象 Protocol）+ mavsdk_gateway（MAVSDK offboard）
+"""
+
+from .command_sender import CommandSender
+from .config import (AccelLimitConfig, CameraGroupConfig, ControlGroupConfig,
+                     DistanceGroupConfig, HeadingGroupConfig, HeadingMode,
+                     NoDistanceAction, TrackingConfig, VerticalGroupConfig,
+                     load_from_json)
+from .frame_transform import rotate_body_to_ned
+from .line_of_sight import PixelToAngle
+from .mavsdk_gateway import MavsdkCommandGateway
+from .pid_controller import PidController, PidParams
+from .rate_limiter import SlewRateLimiter, clamp
+from .staleness_checker import is_stale
+from .tracking_control_law import TrackingControlLaw
+from .types import (ControlMode, ControlOutput, ControlSnapshot, DegradedReason,
+                    TrackState)
+
+__all__ = [
+    "CommandSender",
+    "MavsdkCommandGateway",
+    "TrackingControlLaw",
+    "TrackingConfig",
+    "load_from_json",
+    "HeadingMode",
+    "NoDistanceAction",
+    "ControlGroupConfig",
+    "CameraGroupConfig",
+    "HeadingGroupConfig",
+    "VerticalGroupConfig",
+    "DistanceGroupConfig",
+    "AccelLimitConfig",
+    "ControlMode",
+    "DegradedReason",
+    "TrackState",
+    "ControlSnapshot",
+    "ControlOutput",
+    "PidController",
+    "PidParams",
+    "SlewRateLimiter",
+    "clamp",
+    "is_stale",
+    "PixelToAngle",
+    "rotate_body_to_ned",
+]

utils/visual_track_control/command_sender.py

@@ -0,0 +1,32 @@
+"""控制指令发送抽象（语义接口，对应 C++ ICommandTransport + 网关门面）。
+
+C++ 版的 ICommandTransport 是字节流传输抽象（Send(bytes)），MAVLink 编码
+在网关内完成；Python 版采用 MAVSDK 语义映射后，"传输"被抽象为设定点级
+指令发送。本 Protocol 仅声明门面接口，供胶水层依赖注入与 mock 测试。
+
+异常约定：实现不得向外抛传输层异常，失败一律返回 False（网关已捕获）。
+"""
+
+from typing import Optional, Protocol, runtime_checkable
+
+from .types import ControlOutput
+
+
+@runtime_checkable
+class CommandSender(Protocol):
+    async def start(self) -> bool:
+        """进入 offboard 模式。失败返回 False。"""
+        ...
+
+    async def send_velocity_heading(self, output: ControlOutput,
+                                    time_boot_ms: Optional[int] = None) -> bool:
+        """发送速度+航向意图（ControlMode.VELOCITY_HEADING）。失败返回 False。"""
+        ...
+
+    async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
+        """发送零速刹车/悬停意图（ControlMode.BRAKE_HOVER）。失败返回 False。"""
+        ...
+
+    async def stop(self) -> None:
+        """退出 offboard 模式（结束时调用）。"""
+        ...

utils/visual_track_control/config.py

@@ -0,0 +1,278 @@
+"""视觉跟踪控制律配置（TrackingConfig + load_from_json）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/vtc_config.cpp
+（规格：docs/物理追踪思路.md §8：全部阈值集中配置，NFR-007）。
+
+启动期一次性解析：缺字段 / 类型错误 / 非法取值抛 ValueError 并携带完整
+字段名（如 "heading.gain"）；解析结果与 JSON 脱钩，热路径不做热更新。
+"""
+
+import json
+from dataclasses import dataclass, field
+from enum import Enum
+from typing import Any
+
+from .types import ControlMode  # noqa: F401  (re-export 便利性)
+
+
+class HeadingMode(Enum):
+    """航向通道模式（§4.2）。默认速率式：yaw_rate 天然是相对量，无坐标系歧义。"""
+
+    RATE = "rate"          # β → 偏航角速度，输出 yaw_rate_dps
+    POSITION = "position"  # β → 修正量，与当前航向合成绝对航向输出 yaw_deg
+
+
+class NoDistanceAction(Enum):
+    """雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
+
+    HOLD = "hold"                  # 保持：前向速度指令清零
+    SLOW_APPROACH = "slow_approach"  # 限速接近：按 no_distance_approach_limit_mps 缓速
+    EXIT = "exit"                  # 退出近距操作：输出降级保持
+
+
+@dataclass
+class ControlGroupConfig:
+    frequency_hz: float = 20.0
+    visual_stale_ms: int = 200
+    radar_stale_ms: int = 500
+    attitude_stale_ms: int = 300
+
+
+@dataclass
+class CameraGroupConfig:
+    image_width: int = 1920
+    image_height: int = 1080
+    fov_h_deg: float = 60.0
+    fov_v_deg: float = 35.0
+
+
+@dataclass
+class HeadingGroupConfig:
+    mode: HeadingMode = HeadingMode.RATE
+    gain: float = 1.0
+    yaw_rate_limit_dps: float = 30.0
+    yaw_slew_limit_dps2: float = 60.0
+
+
+@dataclass
+class VerticalGroupConfig:
+    vz_gain_mps_per_deg: float = 0.1
+    vz_limit_mps: float = 2.0
+
+
+@dataclass
+class DistanceGroupConfig:
+    d_exp_m: float = 10.0
+    kp: float = 0.0
+    ki: float = 0.0
+    kd: float = 0.0
+    integral_limit: float = 0.0
+    derivative_filter_coef: float = 0.0
+    approach_velocity_limit_mps: float = 3.0
+    retreat_velocity_limit_mps: float = 1.0
+    no_distance_action: NoDistanceAction = NoDistanceAction.HOLD
+    no_distance_approach_limit_mps: float = 0.5
+
+
+@dataclass
+class AccelLimitConfig:
+    ax_mps2: float = 2.0
+    ay_mps2: float = 2.0
+    az_mps2: float = 1.5
+
+
+@dataclass
+class TrackingConfig:
+    control: ControlGroupConfig = field(default_factory=ControlGroupConfig)
+    camera: CameraGroupConfig = field(default_factory=CameraGroupConfig)
+    heading: HeadingGroupConfig = field(default_factory=HeadingGroupConfig)
+    vertical: VerticalGroupConfig = field(default_factory=VerticalGroupConfig)
+    distance: DistanceGroupConfig = field(default_factory=DistanceGroupConfig)
+    accel_limit: AccelLimitConfig = field(default_factory=AccelLimitConfig)
+
+
+# ---------------------------------------------------------------------------
+# JSON 解析与校验（错误消息携带完整字段名，与 C++ 版文本一致）
+# ---------------------------------------------------------------------------
+
+def _require_section(root: dict, section: str) -> dict:
+    value = root.get(section)
+    if not isinstance(value, dict):
+        raise ValueError(f"配置缺失分组或类型错误: {section}")
+    return value
+
+
+def _is_number(value: Any) -> bool:
+    return isinstance(value, (int, float)) and not isinstance(value, bool)
+
+
+def _is_integer(value: Any) -> bool:
+    return isinstance(value, int) and not isinstance(value, bool)
+
+
+def _require_number(section: dict, section_name: str, key: str) -> float:
+    value = section.get(key)
+    if not _is_number(value):
+        raise ValueError(f"配置缺失字段或类型错误: {section_name}.{key}")
+    return float(value)
+
+
+def _require_integer(section: dict, section_name: str, key: str) -> int:
+    value = section.get(key)
+    if not _is_integer(value):
+        raise ValueError(f"配置缺失字段或类型错误(需整数): {section_name}.{key}")
+    return int(value)
+
+
+def _require_string(section: dict, section_name: str, key: str) -> str:
+    value = section.get(key)
+    if not isinstance(value, str):
+        raise ValueError(f"配置缺失字段或类型错误(需字符串): {section_name}.{key}")
+    return value
+
+
+def _require_positive(value: float, field_name: str) -> float:
+    if not (value > 0.0):
+        raise ValueError(f"配置非法(必须为正): {field_name}")
+    return value
+
+
+def _require_non_negative(value: float, field_name: str) -> float:
+    if value < 0.0:
+        raise ValueError(f"配置非法(不得为负): {field_name}")
+    return value
+
+
+def _require_range(value: float, lo: float, hi: float, field_name: str) -> float:
+    if not (lo < value < hi):
+        raise ValueError(f"配置非法(超出范围): {field_name}")
+    return value
+
+
+def _load_control(root: dict) -> ControlGroupConfig:
+    s = _require_section(root, "control")
+    c = ControlGroupConfig()
+    c.frequency_hz = _require_positive(
+        _require_number(s, "control", "frequency_hz"), "control.frequency_hz")
+    c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
+    c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms")
+    c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
+    if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0:
+        raise ValueError("配置非法: control.*_stale_ms 必须为正")
+    return c
+
+
+def _load_camera(root: dict) -> CameraGroupConfig:
+    s = _require_section(root, "camera")
+    c = CameraGroupConfig()
+    c.image_width = int(_require_positive(
+        float(_require_integer(s, "camera", "image_width")), "camera.image_width"))
+    c.image_height = int(_require_positive(
+        float(_require_integer(s, "camera", "image_height")), "camera.image_height"))
+    c.fov_h_deg = _require_range(_require_number(s, "camera", "fov_h_deg"),
+                                 0.0, 180.0, "camera.fov_h_deg")
+    c.fov_v_deg = _require_range(_require_number(s, "camera", "fov_v_deg"),
+                                 0.0, 180.0, "camera.fov_v_deg")
+    return c
+
+
+def _load_heading(root: dict) -> HeadingGroupConfig:
+    s = _require_section(root, "heading")
+    h = HeadingGroupConfig()
+    mode = _require_string(s, "heading", "mode")
+    if mode == "position":
+        h.mode = HeadingMode.POSITION
+    elif mode == "rate":
+        h.mode = HeadingMode.RATE
+    else:
+        raise ValueError("配置非法: heading.mode 仅支持 position / rate")
+    h.gain = _require_non_negative(_require_number(s, "heading", "gain"), "heading.gain")
+    h.yaw_rate_limit_dps = _require_positive(
+        _require_number(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps")
+    h.yaw_slew_limit_dps2 = _require_positive(
+        _require_number(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2")
+    return h
+
+
+def _load_vertical(root: dict) -> VerticalGroupConfig:
+    s = _require_section(root, "vertical")
+    v = VerticalGroupConfig()
+    v.vz_gain_mps_per_deg = _require_non_negative(
+        _require_number(s, "vertical", "vz_gain_mps_per_deg"),
+        "vertical.vz_gain_mps_per_deg")
+    v.vz_limit_mps = _require_positive(
+        _require_number(s, "vertical", "vz_limit_mps"), "vertical.vz_limit_mps")
+    return v
+
+
+def _load_distance(root: dict) -> DistanceGroupConfig:
+    s = _require_section(root, "distance")
+    d = DistanceGroupConfig()
+    d.d_exp_m = _require_positive(_require_number(s, "distance", "d_exp_m"),
+                                  "distance.d_exp_m")
+    d.kp = _require_non_negative(_require_number(s, "distance", "kp"), "distance.kp")
+    d.ki = _require_non_negative(_require_number(s, "distance", "ki"), "distance.ki")
+    d.kd = _require_non_negative(_require_number(s, "distance", "kd"), "distance.kd")
+    d.integral_limit = _require_non_negative(
+        _require_number(s, "distance", "integral_limit"), "distance.integral_limit")
+    # 容许 -0.0：范围下界略低于 0，读入后归一化为 0
+    d.derivative_filter_coef = _require_range(
+        _require_number(s, "distance", "derivative_filter_coef"),
+        -0.000001, 1.0, "distance.derivative_filter_coef")
+    if d.derivative_filter_coef < 0.0:
+        d.derivative_filter_coef = 0.0
+    d.approach_velocity_limit_mps = _require_positive(
+        _require_number(s, "distance", "approach_velocity_limit_mps"),
+        "distance.approach_velocity_limit_mps")
+    d.retreat_velocity_limit_mps = _require_positive(
+        _require_number(s, "distance", "retreat_velocity_limit_mps"),
+        "distance.retreat_velocity_limit_mps")
+    action = _require_string(s, "distance", "no_distance_action")
+    if action == "hold":
+        d.no_distance_action = NoDistanceAction.HOLD
+    elif action == "slow_approach":
+        d.no_distance_action = NoDistanceAction.SLOW_APPROACH
+    elif action == "exit":
+        d.no_distance_action = NoDistanceAction.EXIT
+    else:
+        raise ValueError(
+            "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit")
+    d.no_distance_approach_limit_mps = _require_non_negative(
+        _require_number(s, "distance", "no_distance_approach_limit_mps"),
+        "distance.no_distance_approach_limit_mps")
+    return d
+
+
+def _load_accel_limit(root: dict) -> AccelLimitConfig:
+    s = _require_section(root, "accel_limit")
+    a = AccelLimitConfig()
+    a.ax_mps2 = _require_positive(_require_number(s, "accel_limit", "ax_mps2"),
+                                  "accel_limit.ax_mps2")
+    a.ay_mps2 = _require_positive(_require_number(s, "accel_limit", "ay_mps2"),
+                                  "accel_limit.ay_mps2")
+    a.az_mps2 = _require_positive(_require_number(s, "accel_limit", "az_mps2"),
+                                  "accel_limit.az_mps2")
+    return a
+
+
+def load_from_json(path: str) -> TrackingConfig:
+    """从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。"""
+    try:
+        with open(path, "r", encoding="utf-8") as fp:
+            root = json.load(fp)
+    except OSError:
+        raise ValueError(f"配置文件打开失败: {path}") from None
+    except json.JSONDecodeError as exc:
+        raise ValueError(f"配置文件解析失败: {path} ({exc})") from None
+
+    if not isinstance(root, dict):
+        raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)")
+
+    cfg = TrackingConfig()
+    cfg.control = _load_control(root)
+    cfg.camera = _load_camera(root)
+    cfg.heading = _load_heading(root)
+    cfg.vertical = _load_vertical(root)
+    cfg.distance = _load_distance(root)
+    cfg.accel_limit = _load_accel_limit(root)
+    return cfg

utils/visual_track_control/frame_transform.py

@@ -0,0 +1,25 @@
+"""机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h
+（规格：docs/物理追踪思路.md §2.3）。
+
+速度设定值只随航向角 ψ（绕下轴旋转）旋转：滚转/俯仰对速度指令为二阶
+小量，由飞控内环处理。旋转为正交变换：合速度大小不变，因此"限幅在机
+体系完成、旋转在输出端执行"不破坏 §8 限幅约束。
+
+正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
+  机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东；
+  机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东。
+"""
+
+import math
+
+
+def rotate_body_to_ned(v_forward: float, v_right: float, v_down: float,
+                       yaw_rad: float) -> tuple[float, float, float]:
+    """绕下轴按航向角 ψ 旋转，返回 (北, 东, 地)；垂直分量不变。"""
+    c = math.cos(yaw_rad)
+    s = math.sin(yaw_rad)
+    north = v_forward * c - v_right * s
+    east = v_forward * s + v_right * c
+    return north, east, v_down

utils/visual_track_control/line_of_sight.py

@@ -0,0 +1,25 @@
+"""像素偏移 → 视线角（针孔模型）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/line_of_sight.cpp
+（规格：docs/物理追踪思路.md §4.1 水平 β / §5.1 垂直 α）。
+
+全程使用精确针孔模型 angle = atan(offset · 2·tan(θ/2) / size)：原线性近似
+双路切换在边界两侧不连续（θ=60°、边界比例 0.2 处约 8% 台阶），会向控制
+环路注入突变；atan 代价可忽略。构造期预计算针孔系数，Convert 仅一次乘加。
+"""
+
+import math
+
+
+class PixelToAngle:
+    def __init__(self, size_px: float, fov_rad: float) -> None:
+        # 构造期校验：非法参数直接抛异常（初始化抛异常规范）
+        if not (size_px > 0.0):
+            raise ValueError("PixelToAngle: size_px 必须为正")
+        if not (0.0 < fov_rad < math.pi):
+            raise ValueError("PixelToAngle: fov_rad 必须在 (0, π) 内")
+        self._pinhole_scale = 2.0 * math.tan(fov_rad / 2.0) / size_px
+
+    def convert(self, offset_px: float) -> float:
+        """偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）。"""
+        return math.atan(offset_px * self._pinhole_scale)

utils/visual_track_control/mavsdk_gateway.py

@@ -0,0 +1,98 @@
+"""MAVSDK offboard 网关 —— 控制意图 → MAVSDK 设定点（语义映射）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/mavlink_command_gateway.cpp
+（C++ 版手工编码 MAVLink ID 84 字节流；Python 版改用 MAVSDK offboard API，
+按 A2 语义映射三分支）：
+
+  速率式（yaw_rate_dps ≠ 0）→ set_velocity_body(机体前向, 0, vz, yaw_rate)
+  位置式（yaw_deg ≠ 0 且角速率为零）→ set_velocity_ned(NED 速度 + 绝对 yaw)
+  两者均零 → set_velocity_body 配零角速率（保持当前航向，A2 回归语义）
+  刹车 → set_velocity_body(0, 0, 0, 0)（MAVSDK 无刹车掩码的行为等效适配）
+
+降级保持（DEGRADED_HOLD）不发送任何指令，由调用方（胶水层）判断模式。
+MAVSDK Python API 为 asyncio 异步接口，本网关方法均为协程；传输异常
+捕获后返回 False（与 C++ 传输失败传播一致），不向外抛异常。
+"""
+
+import logging
+from typing import Any, Optional
+
+from .types import ControlOutput
+
+logger = logging.getLogger(__name__)
+
+
+class MavsdkCommandGateway:
+    """drone：mavsdk System（仅取 .offboard 属性，duck-typed 便于注入 mock）。"""
+
+    def __init__(self, drone: Any) -> None:
+        self._offboard = drone.offboard
+        self._started = False
+
+    async def start(self) -> bool:
+        """进入 offboard：PX4 要求先流式发送首条设定点再请求启动。"""
+        try:
+            await self._offboard.set_velocity_body(
+                self._velocity_body(0.0, 0.0, 0.0, 0.0))
+            await self._offboard.start()
+        except Exception as exc:  # OffboardError / 连接异常统一视为启动失败
+            logger.warning("[MavsdkGateway] offboard 启动失败: %s", exc)
+            return False
+        self._started = True
+        logger.info("[MavsdkGateway] offboard 已启动")
+        return True
+
+    async def send_velocity_heading(self, output: ControlOutput,
+                                    time_boot_ms: Optional[int] = None) -> bool:
+        try:
+            if output.yaw_rate_dps != 0.0:
+                # 速率式：机体系速度 + 偏航角速度（NED 分量不再使用）
+                await self._offboard.set_velocity_body(
+                    self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps,
+                                        output.yaw_rate_dps))
+            elif output.yaw_deg != 0.0:
+                # 位置式：NED 速度 + 绝对航向（A2：绝对航向合成在控制律完成）
+                await self._offboard.set_velocity_ned(
+                    self._velocity_ned(output.vx_mps, output.vy_mps,
+                                       output.vz_mps, output.yaw_deg))
+            else:
+                # 两者均零：机体系速度 + 零角速率 = 保持当前航向（A2 回归语义）
+                await self._offboard.set_velocity_body(
+                    self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps,
+                                        0.0))
+        except Exception as exc:
+            logger.warning("[MavsdkGateway] 速度航向指令发送失败: %s", exc)
+            return False
+        return True
+
+    async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool:
+        try:
+            await self._offboard.set_velocity_body(
+                self._velocity_body(0.0, 0.0, 0.0, 0.0))
+        except Exception as exc:
+            logger.warning("[MavsdkGateway] 刹车指令发送失败: %s", exc)
+            return False
+        return True
+
+    async def stop(self) -> None:
+        try:
+            await self._offboard.stop()
+        except Exception as exc:
+            logger.warning("[MavsdkGateway] offboard 停止失败: %s", exc)
+        self._started = False
+        logger.info("[MavsdkGateway] offboard 已停止")
+
+    @staticmethod
+    def _velocity_body(forward: float, right: float, down: float,
+                       yawspeed: float) -> Any:
+        from mavsdk import offboard as _mavsdk_offboard
+        return _mavsdk_offboard.VelocityBodyYawspeed(
+            forward_m_s=forward, right_m_s=right, down_m_s=down,
+            yawspeed_deg_s=yawspeed)
+
+    @staticmethod
+    def _velocity_ned(north: float, east: float, down: float,
+                      yaw: float) -> Any:
+        from mavsdk import offboard as _mavsdk_offboard
+        return _mavsdk_offboard.VelocityNedYaw(
+            north_m_s=north, east_m_s=east, down_m_s=down, yaw_deg=yaw)

utils/visual_track_control/pid_controller.py

@@ -0,0 +1,95 @@
+"""位置式 PID 控制器（纯算法构件）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/pid_controller.cpp
+（规格：docs/物理追踪思路.md §6.2 距离控制律、§6.4 Coast 清积分）。
+
+anti-windup（条件积分）：输出饱和且本拍积分增量（error·dt）仍在推深饱和
+时冻结本步积分累加；判据刻意基于"增量方向"而非"积分项与输出同向"——
+后者在误差反号后仍会冻结回卷，导致饱和残留。
+"""
+
+from dataclasses import dataclass
+
+from .rate_limiter import clamp
+
+
+@dataclass
+class PidParams:
+    """PID 参数（全部来自配置，代码不写死任何数值）。"""
+
+    kp: float = 0.0
+    ki: float = 0.0
+    kd: float = 0.0
+    integral_limit: float = 0.0         # 积分项限幅（<=0 表示不限幅）
+    derivative_filter_coef: float = 0.0  # 微分一阶低通系数 ∈ [0,1)：0 = 不滤波
+    output_limit: float = 0.0           # 输出限幅（<=0 表示不限幅）
+
+
+class PidController:
+    def __init__(self, params: PidParams) -> None:
+        self._params = params
+        self._integral = 0.0
+        self._prev_error = 0.0
+        self._filtered_derivative = 0.0
+        self._last_output = 0.0
+        self._has_prev_error = False
+
+    def update(self, error: float, dt_s: float) -> float:
+        """单步更新：输入误差与采样间隔（秒），返回限幅后的输出。
+
+        dt_s <= 0 时保持上次输出（异常节拍保护）。
+        """
+        if dt_s <= 0.0:
+            return self._last_output
+
+        # 积分：候选值先钳位（积分限幅）
+        integral_candidate = self._integral + error * dt_s
+        if self._params.integral_limit > 0.0:
+            integral_candidate = clamp(integral_candidate,
+                                       -self._params.integral_limit,
+                                       self._params.integral_limit)
+
+        # 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波）
+        derivative = 0.0
+        if self._has_prev_error:
+            derivative = (error - self._prev_error) / dt_s
+            coef = self._params.derivative_filter_coef
+            derivative = coef * self._filtered_derivative + (1.0 - coef) * derivative
+        self._filtered_derivative = derivative
+        self._prev_error = error
+        self._has_prev_error = True
+
+        # 位置式合成
+        integral_term = self._params.ki * integral_candidate
+        raw = self._params.kp * error + integral_term + self._params.kd * derivative
+
+        # anti-windup（条件积分）：输出饱和且本拍积分增量仍在推深饱和时冻结
+        saturated = (self._params.output_limit > 0.0
+                     and abs(raw) > self._params.output_limit)
+        increment_pushes_deeper = (error * dt_s) * raw > 0.0
+        if not (saturated and increment_pushes_deeper):
+            self._integral = integral_candidate
+
+        # 输出限幅
+        if self._params.output_limit > 0.0:
+            self._last_output = clamp(raw, -self._params.output_limit,
+                                      self._params.output_limit)
+        else:
+            self._last_output = raw
+        return self._last_output
+
+    def reset_integral(self) -> None:
+        """清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出。"""
+        self._integral = 0.0
+
+    def reset(self) -> None:
+        """清空全部内部状态（积分、微分历史、上次输出）。"""
+        self._integral = 0.0
+        self._prev_error = 0.0
+        self._filtered_derivative = 0.0
+        self._last_output = 0.0
+        self._has_prev_error = False
+
+    @property
+    def last_output(self) -> float:
+        return self._last_output

utils/visual_track_control/rate_limiter.py

@@ -0,0 +1,37 @@
+"""限幅与变化率限制（纯算法构件）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h
+（规格：docs/物理追踪思路.md §8、状态机设计 S6 首条指令限幅）。
+"""
+
+def clamp(v: float, lo: float, hi: float) -> float:
+    """静态限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）。"""
+    return min(max(v, lo), hi)
+
+
+class SlewRateLimiter:
+    """变化率限幅器：限制输出每秒最大变化量（单位由调用方决定）。
+
+    初值置 0 时天然实现"首条指令突变抑制"（S6）：切换进入视觉跟踪后
+    首条指令从零按加速度限幅渐变。
+    """
+
+    def __init__(self, rate_limit: float, initial: float = 0.0) -> None:
+        self._rate_limit = rate_limit
+        self._output = initial
+
+    def limit(self, target: float, dt_s: float) -> float:
+        """朝 target 推进一个节拍；dt_s <= 0 或 rate_limit <= 0 时保持当前输出。"""
+        if dt_s <= 0.0 or self._rate_limit <= 0.0:
+            return self._output
+        max_step = self._rate_limit * dt_s
+        self._output = clamp(target, self._output - max_step, self._output + max_step)
+        return self._output
+
+    def reset(self, value: float = 0.0) -> None:
+        """复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）。"""
+        self._output = value
+
+    @property
+    def last_output(self) -> float:
+        return self._output

utils/visual_track_control/staleness_checker.py

@@ -0,0 +1,19 @@
+"""输入超龄判定（单调毫秒时钟）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h
+（规格：docs/物理追踪思路.md §7.1：超龄数据只能触发降级或告警）。
+"""
+
+
+def is_stale(sample_time_ms: int, now_ms: int, valid_for_ms: int) -> bool:
+    """输入超龄判定。
+
+    sample_time_ms <= 0 视为从未取样，直接判超龄；now_ms < sample_time_ms
+    （时间戳不一致：时钟源错配/打戳错误）同样判超龄——单一单调时钟下不应
+    发生，宁可降级也不信任不可核实的取样。
+    """
+    if sample_time_ms <= 0:
+        return True
+    if now_ms < sample_time_ms:
+        return True
+    return (now_ms - sample_time_ms) > valid_for_ms

utils/visual_track_control/tracking_control_law.py

@@ -0,0 +1,220 @@
+"""视觉跟踪主控律（TrackingControlLaw）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/src/tracking_control_law.cpp
+（规格：docs/物理追踪思路.md §2.3/§4/§5/§6/§7/§8/§9）。
+
+单周期八步流程：
+  ① 超龄判定（姿态优先于视觉，超龄 → 降级保持）
+  ② 判丢 → 零速刹车（进入时清积分与限幅器历史）
+  ③ Coast（is_predicted）→ 清距离积分防饱和
+  ④ 像素偏移 → 视线角 β/α（全程针孔模型）
+  ⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向）
+  ⑥ 垂直统一速度支路（A1：不切姿态微调模式）
+  ⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit）
+  ⑧ 机体系 → 局部 NED 旋转（A4）
+
+日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。
+Python 扩展：ControlOutput.vx_body_mps 记录旋转前的机体系前向分量，
+供 MAVSDK 机体速度设定点使用。
+"""
+
+import logging
+import math
+
+from .config import HeadingMode, NoDistanceAction, TrackingConfig
+from .frame_transform import rotate_body_to_ned
+from .line_of_sight import PixelToAngle
+from .pid_controller import PidController, PidParams
+from .rate_limiter import SlewRateLimiter, clamp
+from .staleness_checker import is_stale
+from .types import ControlMode, ControlOutput, ControlSnapshot, DegradedReason
+
+logger = logging.getLogger(__name__)
+
+_K_PI = 3.14159265358979323846
+_K_RAD_TO_DEG = 180.0 / _K_PI
+_K_DEG_TO_RAD = _K_PI / 180.0
+_K_MAX_DT_S = 0.5  # 单步 dt 上限（秒）：异常节拍保护
+
+_MODE_NAMES = {
+    ControlMode.VELOCITY_HEADING: "速度航向",
+    ControlMode.BRAKE_HOVER: "零速刹车",
+    ControlMode.DEGRADED_HOLD: "降级保持",
+}
+
+
+def _wrap_deg_180(deg: float) -> float:
+    """角度回绕到 [-180, 180)（位置式绝对航向合成用）。"""
+    d = math.fmod(deg + 180.0, 360.0)
+    if d < 0.0:
+        d += 360.0
+    return d - 180.0
+
+
+def _build_distance_pid_params(cfg: TrackingConfig) -> PidParams:
+    """由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者。"""
+    return PidParams(
+        kp=cfg.distance.kp,
+        ki=cfg.distance.ki,
+        kd=cfg.distance.kd,
+        integral_limit=cfg.distance.integral_limit,
+        derivative_filter_coef=cfg.distance.derivative_filter_coef,
+        output_limit=max(cfg.distance.approach_velocity_limit_mps,
+                         cfg.distance.retreat_velocity_limit_mps),
+    )
+
+
+class TrackingControlLaw:
+    def __init__(self, cfg: TrackingConfig) -> None:
+        self._cfg = cfg
+        self._pixel_to_beta = PixelToAngle(cfg.camera.image_width,
+                                           cfg.camera.fov_h_deg * _K_DEG_TO_RAD)
+        self._pixel_to_alpha = PixelToAngle(cfg.camera.image_height,
+                                            cfg.camera.fov_v_deg * _K_DEG_TO_RAD)
+        self._distance_pid = PidController(_build_distance_pid_params(cfg))
+        self._yaw_deg_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2)
+        self._yaw_rate_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2)
+        self._vx_limiter = SlewRateLimiter(cfg.accel_limit.ax_mps2)
+        self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2)
+        self._last_now_ms = 0
+        self._last_mode = ControlMode.DEGRADED_HOLD
+
+    def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput:
+        # ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
+        # 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率。
+        dt_s = 1.0 / self._cfg.control.frequency_hz
+        if self._last_now_ms > 0 and now_ms > self._last_now_ms:
+            dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S)
+        self._last_now_ms = now_ms
+
+        out = ControlOutput()
+        out.mode = ControlMode.VELOCITY_HEADING  # 正常路径默认模式，分支可覆盖
+
+        # ---- ① 超龄判定（§7.1 / §9.2） ----
+        # 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2）
+        if (not snapshot.attitude_present
+                or is_stale(snapshot.attitude_time_ms, now_ms,
+                            self._cfg.control.attitude_stale_ms)):
+            out.mode = ControlMode.DEGRADED_HOLD
+            out.degraded_reason = DegradedReason.ATTITUDE_STALE
+            return self._finish_and_log(out, "姿态过期/缺失，禁止控制")
+        # 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
+        if is_stale(snapshot.track_time_ms, now_ms,
+                    self._cfg.control.visual_stale_ms):
+            out.mode = ControlMode.DEGRADED_HOLD
+            out.degraded_reason = DegradedReason.VISION_STALE
+            return self._finish_and_log(out, "视觉追踪结果超龄")
+
+        # ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ----
+        if not snapshot.track.tracked:
+            if self._last_mode is not ControlMode.BRAKE_HOVER:
+                # 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
+                self._distance_pid.reset()
+                self._yaw_deg_limiter.reset()
+                self._yaw_rate_limiter.reset()
+                self._vx_limiter.reset()
+                self._vz_limiter.reset()
+            out.mode = ControlMode.BRAKE_HOVER  # 速度分量保持默认全零
+            out.valid = True
+            return self._finish_and_log(out, "判丢，零速刹车")
+
+        # ---- ③ Coast：is_predicted → 清积分防饱和（§6.4） ----
+        if snapshot.track.is_predicted:
+            self._distance_pid.reset_integral()
+            # 航向通道为纯比例增益映射（§4.2），无积分项，无需清理
+            out.coast_active = True
+
+        # ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ----
+        # x_offset = cx − W/2（右正），y_offset = cy − H/2（下正）
+        x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5
+        y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5
+        beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG
+        alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG
+
+        # ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ----
+        if self._cfg.heading.mode is HeadingMode.RATE:
+            # 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033）
+            target_rate = self._cfg.heading.gain * beta_deg
+            slewed = self._yaw_rate_limiter.limit(target_rate, dt_s)
+            out.yaw_rate_dps = clamp(slewed, -self._cfg.heading.yaw_rate_limit_dps,
+                                     self._cfg.heading.yaw_rate_limit_dps)
+        else:
+            # 位置式：β → 航向修正量，变化率限幅（FR-033）后与当前航向合成
+            # 绝对 NED 航向——ID 84 的 yaw 字段是绝对值，直接发修正量会让
+            # 机体转向"修正角"方向（A2 修复）。姿态已在 ① 通过超龄校验。
+            target_yaw = self._cfg.heading.gain * beta_deg
+            correction_deg = self._yaw_deg_limiter.limit(target_yaw, dt_s)
+            out.yaw_deg = _wrap_deg_180(snapshot.yaw_rad * _K_RAD_TO_DEG
+                                        + correction_deg)
+
+        # ---- ⑥ 垂直速度通道（§5：统一速度支路） ----
+        # 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。
+        # 输出模式保持 VELOCITY_HEADING，距离通道前向速度不会被丢弃（A1 修复）。
+        vz_target = self._cfg.vertical.vz_gain_mps_per_deg * alpha_deg
+        slewed = self._vz_limiter.limit(vz_target, dt_s)  # 加速度限幅（§8）
+        out.vz_mps = clamp(slewed, -self._cfg.vertical.vz_limit_mps,
+                           self._cfg.vertical.vz_limit_mps)
+
+        # ---- ⑦ 距离通道（§6.2 / §6.3） ----
+        # 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
+        distance_available = (snapshot.associated_to_target
+                              and snapshot.radar_distance_m > 0.0
+                              and not is_stale(snapshot.radar_time_ms, now_ms,
+                                               self._cfg.control.radar_stale_ms))
+        if distance_available:
+            # Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
+            delta_d = snapshot.radar_distance_m - self._cfg.distance.d_exp_m
+            vx_raw = self._distance_pid.update(delta_d, dt_s)
+            slewed = self._vx_limiter.limit(vx_raw, dt_s)  # 加速度限幅（§8）
+            out.vx_mps = clamp(slewed,
+                               -self._cfg.distance.retreat_velocity_limit_mps,
+                               self._cfg.distance.approach_velocity_limit_mps)
+        else:
+            # 无可用目标距离：清积分防饱和，按配置降级动作（§6.3）
+            self._distance_pid.reset_integral()
+            action = self._cfg.distance.no_distance_action
+            if action is NoDistanceAction.HOLD:
+                # 保持：前向指令清零（安全占位语义）
+                out.vx_mps = 0.0
+                self._vx_limiter.reset(0.0)
+            elif action is NoDistanceAction.SLOW_APPROACH:
+                # 限速接近：缓速前向，同样经加速度限幅
+                out.vx_mps = self._vx_limiter.limit(
+                    self._cfg.distance.no_distance_approach_limit_mps, dt_s)
+            else:  # NoDistanceAction.EXIT
+                # 退出近距操作：整体降级保持
+                out.mode = ControlMode.DEGRADED_HOLD
+                out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
+                return self._finish_and_log(out, "距离未关联，退出近距操作")
+
+        # ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
+        # 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束
+        # 不被破坏）；输出端随快照航向旋转为北/东/地。旋转前记录机体系前向
+        # 分量，供 MAVSDK 机体速度设定点直接使用（免反旋）。
+        out.vx_body_mps = out.vx_mps
+        out.vx_mps, out.vy_mps, out.vz_mps = rotate_body_to_ned(
+            out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad)
+
+        out.valid = True
+        return self._finish_and_log(out,
+                                    "Coast 预测跟踪" if out.coast_active
+                                    else "正常跟踪")
+
+    def reset(self) -> None:
+        """停止视觉闭环时清全部状态（§6.4）。"""
+        self._distance_pid.reset()
+        self._yaw_deg_limiter.reset()
+        self._yaw_rate_limiter.reset()
+        self._vx_limiter.reset()
+        self._vz_limiter.reset()
+        self._last_now_ms = 0
+        self._last_mode = ControlMode.DEGRADED_HOLD
+
+    def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput:
+        """仅模式变化时打印状态切换日志（刹车进出、降级切换等）。"""
+        if out.mode is not self._last_mode:
+            logger.info("[ControlLaw] 状态切换: %s -> %s (%s)",
+                        _MODE_NAMES[self._last_mode], _MODE_NAMES[out.mode],
+                        reason)
+            self._last_mode = out.mode
+        return out

utils/visual_track_control/types.py

@@ -0,0 +1,95 @@
+"""视觉跟踪控制律的中立输入/输出结构。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/vtc_types.h。
+
+解耦说明：C++ 版的 ControlSnapshot 直接内嵌 target_tracker::TrackResult；本包为保持
+与 target_tracker 包互不依赖，自带字段对齐的 TrackState 值类型。调用方（胶水层）
+负责把 target_tracker.TrackResult 转换为 TrackState（字段均为 plain tuple/标量）。
+"""
+
+from dataclasses import dataclass, field
+from enum import Enum, auto
+from typing import Optional
+
+
+class ControlMode(Enum):
+    """控制输出模式（§2.2 控制意图语义）。"""
+
+    VELOCITY_HEADING = auto()  # 速度与航向意图
+    BRAKE_HOVER = auto()       # 零速刹车/悬停：判丢等场景
+    DEGRADED_HOLD = auto()     # 降级保持：超龄/退出近距操作，禁止正常控制
+
+
+class DegradedReason(Enum):
+    """降级原因（仅 DEGRADED_HOLD 模式有意义）。"""
+
+    NONE = auto()
+    ATTITUDE_STALE = auto()   # PX4 姿态过期：禁止空间转换和控制（§9.2）
+    VISION_STALE = auto()     # 视觉追踪结果超龄（§7.1）
+    NO_DISTANCE_EXIT = auto() # 无距离关联且配置为退出近距操作（§6.3）
+
+
+@dataclass
+class TrackState:
+    """追踪结果视图（字段语义与 target_tracker.TrackResult 对齐，但独立定义）。"""
+
+    tracked: bool = False
+    primary_id: Optional[int] = None
+    center: tuple[float, float] = (0.0, 0.0)
+    box: Optional[tuple[float, float, float, float]] = None
+    confidence: float = 0.0
+    is_predicted: bool = False
+    lost_frames: int = 0
+    n_active: int = 0
+    raw: Optional[tuple[float, float, float, float]] = None
+
+
+@dataclass
+class ControlSnapshot:
+    """控制律输入快照：调用方在同一控制周期内采集并打戳。
+
+    TrackState 本身不含时间戳（§7.2），调用方必须在取样瞬间用单调时钟
+    打戳填入 track_time_ms；超龄判定一律使用"取样时间 + valid_for_ms"。
+    """
+
+    track: TrackState = field(default_factory=TrackState)
+    track_time_ms: int = 0            # 取样单调时钟（毫秒），<=0 视为从未取样
+
+    radar_distance_m: float = 0.0     # 机头前向单点距离（米）
+    associated_to_target: bool = False  # 相机—雷达空间关联成立标志（未关联不得当目标距离）
+    radar_time_ms: int = 0
+
+    attitude_present: bool = False    # 姿态源当前是否可用（过期 → 禁止控制）
+    attitude_time_ms: int = 0
+    yaw_rad: float = 0.0              # 航向（弧度，NED）；参与 NED 旋转与位置式航向合成
+    roll_rad: float = 0.0             # 以下为预留字段（机体补偿未实现，§4.1）
+    pitch_rad: float = 0.0
+    yaw_rate_radps: float = 0.0
+    velocity_x_mps: float = 0.0
+    velocity_y_mps: float = 0.0
+    velocity_z_mps: float = 0.0
+
+
+@dataclass
+class ControlOutput:
+    """控制律输出：限幅后的控制意图。
+
+    坐标语义：vx/vy/vz 为局部 NED（北/东/地）分量——各通道在机体系内计算
+    并限幅后，随 snapshot.yaw_rad 旋转得到（旋转为正交变换，合速度不变）。
+    vx_body_mps 为旋转前的机体系前向分量，供 MAVSDK 速度机体设定点使用
+    （机体系侧向恒为 0，故不另设右向字段；vz 两系相同）。
+    """
+
+    mode: ControlMode = ControlMode.DEGRADED_HOLD
+    valid: bool = False
+
+    vx_mps: float = 0.0   # 北向（§6.2）
+    vy_mps: float = 0.0   # 东向（机体系恒 0，航向旋转后可非零）
+    vz_mps: float = 0.0   # 垂直（NED 下正，§5.2）
+    vx_body_mps: float = 0.0  # 机体系前向分量（旋转前，MAVSDK 机体设定点用）
+
+    yaw_deg: float = 0.0        # 位置式目标绝对航向（度，NED，右转为正，回绕 [-180,180)）
+    yaw_rate_dps: float = 0.0   # 速率式偏航角速度指令（度/秒，右转为正）
+
+    degraded_reason: DegradedReason = DegradedReason.NONE
+    coast_active: bool = False