精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 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。
This commit is contained in:
keita
+126
-5
@@ -6,15 +6,21 @@
|
||||
* 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
|
||||
低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
|
||||
* 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
|
||||
* 简洁任务简报（task_brief.log，纯文本）：一行一事件（阶段切换/地面站命令/
|
||||
飞控告警）+ 运行结束汇总（时长/最高/最近距目标/状态链），供人工阅读与归档；
|
||||
机器日志（JSONL/CSV）格式不变。
|
||||
|
||||
时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
|
||||
文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
|
||||
文件 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
|
||||
|
||||
@@ -60,13 +66,26 @@ class TaskLogConfig:
|
||||
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:
|
||||
"""结构化任务日志。用法：
|
||||
|
||||
@@ -86,12 +105,20 @@ class TaskLogger:
|
||||
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 时跳过。"""
|
||||
"""创建输出目录并打开日志文件；disabled 时跳过。"""
|
||||
if not self._cfg.enabled:
|
||||
return
|
||||
out_dir = Path(self._cfg.dir)
|
||||
@@ -103,11 +130,22 @@ class TaskLogger:
|
||||
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:
|
||||
"""flush 并关闭两个日志文件。"""
|
||||
"""写简报汇总并关闭全部日志文件。"""
|
||||
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()
|
||||
@@ -120,16 +158,32 @@ class TaskLogger:
|
||||
|
||||
# --- 记录 ---------------------------------------------------------------
|
||||
def log_event(self, now_ms: int, event_type: str, **fields) -> None:
|
||||
"""追加一条 JSONL 事件。"""
|
||||
"""追加一条 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 节流）。"""
|
||||
"""追加一行遥测（按 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
|
||||
@@ -137,8 +191,75 @@ class TaskLogger:
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user