keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
271 lines
11 KiB
Python
271 lines
11 KiB
Python
"""log.py —— 任务日志模块：结构化记录任务全流程（事件 + 周期遥测）。
|
||
|
||
预定流程 B 阶段：让任意一次 SITL / 实飞都能回溯出完整阶段轨迹与异常。
|
||
|
||
分层：
|
||
* 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
|
||
低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
|
||
* 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
|
||
* 简洁任务简报（task_brief.log，纯文本）：一行一事件（阶段切换/地面站命令/
|
||
飞控告警）+ 运行结束汇总（时长/最高/最近距目标/状态链），供人工阅读与归档；
|
||
机器日志（JSONL/CSV）格式不变。
|
||
|
||
时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
|
||
文件 IO 集中在 open / close；热路径只追加（并立即 flush 落盘——缓冲期间文件不可见，
|
||
地面站页面运行中需要读到最新状态与遥测，2026-10-14 实测）。
|
||
"""
|
||
|
||
import csv
|
||
import json
|
||
import math
|
||
import time
|
||
from dataclasses import dataclass
|
||
from datetime import datetime
|
||
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" # 周期遥测日志
|
||
brief_file: str = "task_brief.log" # 人类可读任务简报（追加）
|
||
telemetry_every_n: int = 1 # 每 N 周期记一次遥测（1 = 每周期）
|
||
telemetry_flush_ms: int = 1000 # 遥测落盘间隔（毫秒）；0 = 每行立即 flush
|
||
|
||
|
||
def _monotonic_ms() -> int:
|
||
return int(time.monotonic() * 1000)
|
||
|
||
|
||
_STATE_CN = {"wait": "待命", "arm_takeoff": "起飞", "goto": "坐标引导",
|
||
"search": "搜索", "track": "追踪靠近", "approach": "接近悬停",
|
||
"rtl": "返航"}
|
||
|
||
|
||
def _fmt_t_s(t_s: float) -> str:
|
||
"""相对时间 → mm:ss.s（简报用）。"""
|
||
m, s = divmod(max(0.0, float(t_s or 0.0)), 60.0)
|
||
return f"{int(m):02d}:{s:04.1f}"
|
||
|
||
|
||
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._brief_fp = None
|
||
self._t0_ms: Optional[int] = None
|
||
self._tick = 0
|
||
self._last_tel_flush_ms: Optional[int] = None # 遥测上次落盘时刻
|
||
# 简报汇总统计（随遥测行/事件更新，close 时写一行汇总）
|
||
self._n_events = 0
|
||
self._last_t_s = 0.0
|
||
self._max_rel_alt_m: Optional[float] = None
|
||
self._min_dist_m: Optional[float] = None
|
||
self._state_chain: list = []
|
||
|
||
# --- 生命周期 -----------------------------------------------------------
|
||
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._brief_fp = (out_dir / self._cfg.brief_file).open(
|
||
"a", encoding="utf-8")
|
||
self._brief_fp.write(
|
||
f"===== 任务开始 {datetime.now().strftime('%Y-%m-%d %H:%M:%S')} =====\n")
|
||
self._last_tel_flush_ms = None
|
||
self._event_fp.flush()
|
||
self._tel_fp.flush()
|
||
self._brief_fp.flush()
|
||
|
||
def close(self) -> None:
|
||
"""写简报汇总并关闭全部日志文件。"""
|
||
if self._brief_fp is not None:
|
||
self._brief_fp.write(self._brief_summary() + "\n\n")
|
||
self._brief_fp.flush()
|
||
self._brief_fp.close()
|
||
self._brief_fp = None
|
||
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 事件并**立即落盘**（事件低频且语义关键：地面站要实时看阶段）。
|
||
|
||
注意：遥测行不在这里 flush——见 log_telemetry 的节流落盘说明。
|
||
"""
|
||
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")
|
||
self._event_fp.flush()
|
||
self._n_events += 1
|
||
if self._brief_fp is not None:
|
||
self._brief_fp.write(self._brief_text(record) + "\n")
|
||
self._brief_fp.flush()
|
||
|
||
def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
|
||
"""追加一行遥测（按 telemetry_every_n 节流）。
|
||
|
||
落盘策略：**写入靠 Python 缓冲、按 telemetry_flush_ms 定期 flush**，
|
||
而不是每行 fsync 式 flush。原因：本方法在主控制循环里以 20Hz 调用，
|
||
每行都 flush 等于每秒 20 次同步磁盘 IO，在树莓派 SD 卡上会阻塞事件
|
||
循环（实测 MAVSDK 回调队列因此积压告警）；事件行仍即时落盘，故页面
|
||
的阶段显示不受影响，只有图表数据最多滞后 telemetry_flush_ms。
|
||
close() 会做最后一次 flush，进程正常退出不丢数据。
|
||
"""
|
||
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])
|
||
self._flush_telemetry(now_ms)
|
||
# 简报汇总统计（只随真正落盘的行更新）
|
||
self._last_t_s = rec.t_s
|
||
if math.isfinite(rec.relative_altitude_m):
|
||
if (self._max_rel_alt_m is None
|
||
or rec.relative_altitude_m > self._max_rel_alt_m):
|
||
self._max_rel_alt_m = rec.relative_altitude_m
|
||
dist = rec.distance_to_target_m
|
||
if math.isfinite(dist) and dist > 0:
|
||
if self._min_dist_m is None or dist < self._min_dist_m:
|
||
self._min_dist_m = dist
|
||
if rec.state and (not self._state_chain
|
||
or self._state_chain[-1] != rec.state):
|
||
if len(self._state_chain) < 32:
|
||
self._state_chain.append(rec.state)
|
||
|
||
# --- 简报（人类可读，一行一事件）----------------------------------------
|
||
@staticmethod
|
||
def _brief_text(record: dict) -> str:
|
||
t = _fmt_t_s(record.get("t_s", 0.0))
|
||
etype = record.get("type", "?")
|
||
if etype == "state":
|
||
to = _STATE_CN.get(record.get("to_state"),
|
||
record.get("to_state", "?"))
|
||
if record.get("from_state") == record.get("to_state"):
|
||
return f"[{t}] 任务启动（当前: {to}）"
|
||
frm = _STATE_CN.get(record.get("from_state"),
|
||
record.get("from_state", "?"))
|
||
return f"[{t}] 阶段: {frm} → {to}（{record.get('reason', '')}）"
|
||
if etype == "command":
|
||
return f"[{t}] 地面站命令: {record.get('name', '')}"
|
||
if etype == "fc_status":
|
||
return (f"[{t}] 飞控 {record.get('severity', '')}: "
|
||
f"{str(record.get('text', '')).strip()}")
|
||
if etype == "takeoff":
|
||
return f"[{t}] 起飞至 {record.get('altitude_m', '?')} m"
|
||
if etype == "takeoff_failed":
|
||
return f"[{t}] 起飞失败: {record.get('error', '')}"
|
||
if etype == "rtl":
|
||
return f"[{t}] 开始返航"
|
||
fields = [f"{k}={v}" for k, v in record.items()
|
||
if k not in ("type", "ts_ms", "t_s")]
|
||
return f"[{t}] {etype}: {', '.join(fields)}"
|
||
|
||
def _brief_summary(self) -> str:
|
||
parts = [f"时长 {self._last_t_s:.1f}s"]
|
||
if self._max_rel_alt_m is not None:
|
||
parts.append(f"最高 {self._max_rel_alt_m:.1f}m")
|
||
if self._min_dist_m is not None:
|
||
parts.append(f"最近距目标 {self._min_dist_m:.1f}m")
|
||
parts.append(f"事件 {self._n_events} 条")
|
||
if self._state_chain:
|
||
parts.append("状态链 " + "→".join(
|
||
_STATE_CN.get(s, s) for s in self._state_chain))
|
||
return "----- 本次运行汇总: " + " · ".join(parts) + " -----"
|
||
|
||
# --- 内部 ---------------------------------------------------------------
|
||
def _flush_telemetry(self, now_ms: int) -> None:
|
||
"""按 telemetry_flush_ms 节流落盘遥测（0 = 每行立即落盘）。"""
|
||
interval = self._cfg.telemetry_flush_ms
|
||
if interval <= 0:
|
||
self._tel_fp.flush()
|
||
self._last_tel_flush_ms = now_ms
|
||
return
|
||
if (self._last_tel_flush_ms is None
|
||
or now_ms - self._last_tel_flush_ms >= interval):
|
||
self._tel_fp.flush()
|
||
self._last_tel_flush_ms = now_ms
|
||
|
||
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
|