实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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,149 @@
+"""log.py —— 任务日志模块：结构化记录任务全流程（事件 + 周期遥测）。
+
+预定流程 B 阶段：让任意一次 SITL / 实飞都能回溯出完整阶段轨迹与异常。
+
+分层：
+  * 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
+    低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
+  * 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
+
+时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
+文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
+"""
+
+import csv
+import json
+import time
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Optional
+
+_TELEMETRY_FIELDS = (
+    "t_s", "state",
+    "pos_valid", "fix_type", "num_satellites",
+    "latitude_deg", "longitude_deg", "relative_altitude_m", "yaw_deg",
+    "target_injected", "distance_to_target_m", "target_locked",
+    "off_target_x_deg", "off_target_y_deg",
+    "mode", "yaw_rate_dps", "vx_body_mps", "vz_mps",
+)
+
+
+@dataclass
+class TelemetryRecord:
+    """单周期遥测记录（字段顺序即 CSV 列序）。"""
+
+    t_s: float = 0.0
+    state: str = ""
+    pos_valid: bool = False
+    fix_type: int = 0
+    num_satellites: int = 0
+    latitude_deg: float = 0.0
+    longitude_deg: float = 0.0
+    relative_altitude_m: float = 0.0
+    yaw_deg: float = 0.0
+    target_injected: bool = False
+    distance_to_target_m: float = float("inf")
+    target_locked: bool = False
+    off_target_x_deg: float = 0.0
+    off_target_y_deg: float = 0.0
+    mode: str = ""
+    yaw_rate_dps: float = 0.0
+    vx_body_mps: float = 0.0
+    vz_mps: float = 0.0
+
+
+@dataclass
+class TaskLogConfig:
+    """任务日志配置（全外置）。"""
+
+    enabled: bool = True
+    dir: str = "logs"                       # 输出目录（相对工作目录）
+    event_file: str = "task_events.jsonl"  # 事件日志
+    telemetry_file: str = "task_telemetry.csv"  # 周期遥测日志
+    telemetry_every_n: int = 1             # 每 N 周期记一次遥测（1 = 每周期）
+
+
+def _monotonic_ms() -> int:
+    return int(time.monotonic() * 1000)
+
+
+class TaskLogger:
+    """结构化任务日志。用法：
+
+        logger = TaskLogger(cfg, now_ms_fn)
+        logger.open()                    # 建目录 + 写 CSV 表头
+        logger.log_event(now_ms, "state", from_state="wait", ...)
+        logger.log_telemetry(now_ms, record)
+        logger.close()
+
+    enabled=False 或未 open 时，所有 log 方法为空操作（不抛异常、不写文件）。
+    """
+
+    def __init__(self, cfg: TaskLogConfig = TaskLogConfig(),
+                 now_ms_fn=_monotonic_ms) -> None:
+        self._cfg = cfg
+        self._now_ms = now_ms_fn
+        self._event_fp = None
+        self._tel_fp = None
+        self._tel_writer = None
+        self._t0_ms: Optional[int] = None
+        self._tick = 0
+
+    # --- 生命周期 -----------------------------------------------------------
+    def open(self) -> None:
+        """创建输出目录并打开两个日志文件；disabled 时跳过。"""
+        if not self._cfg.enabled:
+            return
+        out_dir = Path(self._cfg.dir)
+        out_dir.mkdir(parents=True, exist_ok=True)
+        self._event_fp = (out_dir / self._cfg.event_file).open(
+            "a", encoding="utf-8")
+        self._tel_fp = (out_dir / self._cfg.telemetry_file).open(
+            "a", encoding="utf-8", newline="")
+        self._tel_writer = csv.writer(self._tel_fp)
+        if (out_dir / self._cfg.telemetry_file).stat().st_size == 0:
+            self._tel_writer.writerow(_TELEMETRY_FIELDS)
+        self._event_fp.flush()
+        self._tel_fp.flush()
+
+    def close(self) -> None:
+        """flush 并关闭两个日志文件。"""
+        if self._event_fp is not None:
+            self._event_fp.flush()
+            self._event_fp.close()
+            self._event_fp = None
+        if self._tel_fp is not None:
+            self._tel_fp.flush()
+            self._tel_fp.close()
+            self._tel_fp = None
+            self._tel_writer = None
+
+    # --- 记录 ---------------------------------------------------------------
+    def log_event(self, now_ms: int, event_type: str, **fields) -> None:
+        """追加一条 JSONL 事件。"""
+        if self._event_fp is None:
+            return
+        record = {"type": event_type, "ts_ms": now_ms,
+                  "t_s": self._rel_s(now_ms)}
+        record.update(fields)
+        self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
+
+    def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
+        """追加一行遥测（按 telemetry_every_n 节流）。"""
+        if self._tel_writer is None:
+            return
+        self._tick += 1
+        if self._tick % max(1, self._cfg.telemetry_every_n) != 0:
+            return
+        rec.t_s = self._rel_s(now_ms)
+        self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS])
+
+    # --- 内部 ---------------------------------------------------------------
+    def _rel_s(self, now_ms: int) -> float:
+        if self._t0_ms is None:
+            self._t0_ms = now_ms
+        return (now_ms - self._t0_ms) / 1000.0
+
+    @property
+    def fieldnames(self) -> tuple:
+        return _TELEMETRY_FIELDS