实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口

- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
2026-10-13 15:50:32 +08:00
parent 5a944af4f5
commit ded17d7995
30 changed files with 3552 additions and 27 deletions
@@ -0,0 +1,143 @@
+"""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