code-test-drone/utils/pod_bridge/virtual_pod.py

"""virtual_pod.py —— 模拟吊舱（SITL/台架工具，非飞行组件；类比 sim_fc.py 模拟飞控）。

按"无电机吊舱"的实测语义合成 表2-2 遥测，实现 PodLink 接口可直插 PodBridge：
  * 相机刚性固连机体朝前 → 框架角(frame_roll/pitch/yaw)恒 0；
  * 无激光 → laser_range_m 恒 None；
  * off_target 按目标相对机体的视线角合成，符号与 S1 台架标定一致：
      目标偏右 → off_target_x_deg 为正（右正）
      目标偏下 → off_target_y_deg 为负（上正）
  * 目标不可见（身后 / 超视场 / 被 set_visible(False)）→ 状态字节1 bit2=0（搜索态）
    且脱靶量为 0（对齐真实吊舱"丢失时脱靶量恒 0、锁定位不复位"的坑）。

载机状态由注入的 own_state_fn 提供（CarrierView：NED 位置 + 航向），与 PodBridge
解耦——SITL 脚本注入遥测 hub，单测注入固定值。
"""

import math
import time
from dataclasses import dataclass

from .protocol import Telemetry


@dataclass
class CarrierView:
    """虚拟吊舱所需的载机视角状态（NED 位置 + 航向）。"""

    north_m: float = 0.0
    east_m: float = 0.0
    down_m: float = 0.0
    yaw_rad: float = 0.0


@dataclass
class VirtualTargetConfig:
    """虚拟目标放置与运动参数（锚点机体系偏移 + 水平圆弧）。"""

    ahead_m: float = 30.0
    right_m: float = 15.0
    down_m: float = 0.0
    arc_radius_m: float = 12.0
    arc_speed_mps: float = 1.0
    arc_ccw: bool = True


class VirtualTarget:
    """虚拟目标：按锚点机头朝向放置，随后在 NED 水平面做匀速圆弧运动。

    几何与 test.py 的 VirtualTarget 一致（去掉了针孔投影部分——吊舱路径
    直接给角度，不需要检测框）。
    """

    def __init__(self, cfg: VirtualTargetConfig,
                 anchor_ned: tuple[float, float, float],
                 anchor_yaw_rad: float) -> None:
        cos_y = math.cos(anchor_yaw_rad)
        sin_y = math.sin(anchor_yaw_rad)

        self._start_n = anchor_ned[0] + cfg.ahead_m * cos_y - cfg.right_m * sin_y
        self._start_e = anchor_ned[1] + cfg.ahead_m * sin_y + cfg.right_m * cos_y
        self._start_d = anchor_ned[2] + cfg.down_m

        self._radius_m = max(0.1, float(cfg.arc_radius_m))
        self._omega = (float(cfg.arc_speed_mps) / self._radius_m
                       * (1.0 if cfg.arc_ccw else -1.0))

        right_n = -sin_y
        right_e = cos_y
        self._center_n = self._start_n + self._radius_m * right_n
        self._center_e = self._start_e + self._radius_m * right_e
        self._theta0 = math.atan2(self._start_e - self._center_e,
                                  self._start_n - self._center_n)

        self._pos_n = self._start_n
        self._pos_e = self._start_e
        self._pos_d = self._start_d
        self._t0 = time.monotonic()
        self._visible = True

    @property
    def visible(self) -> bool:
        return self._visible

    def set_visible(self, visible: bool) -> None:
        self._visible = visible

    def update_position(self) -> None:
        elapsed = max(0.0, time.monotonic() - self._t0)
        theta = self._theta0 + self._omega * elapsed
        self._pos_n = self._center_n + self._radius_m * math.cos(theta)
        self._pos_e = self._center_e + self._radius_m * math.sin(theta)
        self._pos_d = self._start_d

    @property
    def pos_ned(self) -> tuple[float, float, float]:
        return self._pos_n, self._pos_e, self._pos_d


class VirtualPod:
    """模拟吊舱：poll() 按当前载机视角合成一帧 表2-2 遥测（锁定/搜索态）。"""

    def __init__(self, target: VirtualTarget, own_state_fn,
                 fov_h_deg: float = 60.0, fov_v_deg: float = 35.0) -> None:
        self._target = target
        self._own = own_state_fn
        self._fov_h = fov_h_deg
        self._fov_v = fov_v_deg
        self.attitude_frames_rx = 0   # 收到的表3 帧数（诊断）
        self.closed = False

    def poll(self) -> list:
        self._target.update_position()
        own = self._own()
        n, e, d = self._target.pos_ned
        rel_n = n - own.north_m
        rel_e = e - own.east_m
        rel_d = d - own.down_m
        cos_y = math.cos(own.yaw_rad)
        sin_y = math.sin(own.yaw_rad)
        fwd = rel_n * cos_y + rel_e * sin_y
        right = -rel_n * sin_y + rel_e * cos_y

        off_x = math.degrees(math.atan2(right, fwd))   # 右正
        off_y = math.degrees(math.atan2(-rel_d, fwd))  # 上正（S1 标定）
        locked = (self._target.visible
                  and fwd > 0.5
                  and abs(off_x) < self._fov_h / 2.0
                  and abs(off_y) < self._fov_v / 2.0)

        return [Telemetry(
            status1=0b00000100 if locked else 0,   # bit2 锁定 / 搜索
            off_target_x_deg=off_x if locked else 0.0,
            off_target_y_deg=off_y if locked else 0.0,
            frame_roll_deg=0.0,                    # 无电机：框架角恒 0
            frame_pitch_deg=0.0,
            frame_yaw_deg=0.0,
            laser_range_m=None,                    # 无激光：恒无效
        )]

    def send(self, data: bytes) -> None:
        self.attitude_frames_rx += 1   # 表3 载机姿态下发：计数即可，不解析

    def close(self) -> None:
        self.closed = True