新增sim_fc.py: 模拟飞控喂表3验证吊舱解算(第1阶段)
- 伪造载机姿态/位置/时戳持续喂表3,单进程收发一体观测表2-2/表4 - 回答Q1(无激光是否仍解算表4)/Q2(表4实际频率)/Q3(丢失后行为) - 自动判读「目标贴地」假设: 表4相对高度是否约等于喂入的载机高度 - 退出码0/1/2/3便于脚本串测; --json落逐帧采样供阶段2聚类分析 - 默认参数已敲定: lat=39.9042 lon=116.4074 rel-alt=50 rate=20Hz - 纯软单测21条(注入fake serial), 全量回归372 passed
This commit is contained in:
keita
@@ -0,0 +1,457 @@
|
||||
"""sim_fc.py —— 模拟飞控喂 表3,验证吊舱目标解算能力(台架工具,非飞行组件)。
|
||||
|
||||
用途:在**没有飞控、机体不动**的条件下,由树莓派单独向吊舱持续喂一套伪造的
|
||||
表3 载机姿态/位置/时间,看吊舱能否据此解算出目标经纬度(表4),用于回答
|
||||
docs/吊舱解算验证方案-模拟飞控喂表3.md 的三个问题:
|
||||
|
||||
Q1 无激光时吊舱是否仍然解算并输出 表4
|
||||
Q2 表4 的实际发送频率(决定 target_stale_ms / --auto-unlock SEC)
|
||||
Q3 目标丢失后 表4 是停止更新还是继续吐旧值
|
||||
|
||||
设计约束(务必遵守):
|
||||
* **单进程收发一体**。/tx 与 rx 是同一个串口,不能一边跑 listen_pod.py
|
||||
一边跑本脚本 —— 后者会 EBUSY,前者会读到错帧。要看仪表盘请在结束后看汇总;
|
||||
* **不碰飞控**。本脚本只走吊舱串口,不产生任何控制输出,是纯开环观测;
|
||||
* 核心逻辑(Feeder / Collector / 判定)与串口隔离,可纯软单测,见
|
||||
utils/tests/pod_bridge/test_sim_fc.py。
|
||||
|
||||
用法:
|
||||
python -m pod_bridge.sim_fc /dev/ttyAMA0 --seconds 60
|
||||
python -m pod_bridge.sim_fc /dev/ttyAMA0 --detect drone --auto-lock --seconds 60
|
||||
python -m pod_bridge.sim_fc /dev/ttyAMA0 --rel-alt 80 --json run.json
|
||||
|
||||
退出码(便于脚本串测):
|
||||
0 = 收到 表4 且经纬度有效(Q1 通过,可进阶段2)
|
||||
1 = 完全未收到 表4(Q1 失败)
|
||||
2 = 收到 表4 但经纬度恒为 0(有帧无解算)
|
||||
3 = 串口链路错误(接线/控制台占用等)
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Sequence
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1])) # utils/ 入 sys.path
|
||||
|
||||
from pod_bridge.pod_serial import PodSerial # noqa: E402
|
||||
from pod_bridge.protocol import (CarrierAttitude, ControlCode, # noqa: E402
|
||||
ControlCommand, TargetGeo, Telemetry,
|
||||
encode_attitude, encode_control)
|
||||
|
||||
# 退出码:见模块 docstring
|
||||
EXIT_OK = 0
|
||||
EXIT_NO_TARGET_FRAME = 1
|
||||
EXIT_ZERO_GEO = 2
|
||||
EXIT_LINK_ERROR = 3
|
||||
|
||||
# 表2-1 0x07 目标识别开:参数X 0=检测人或车,1=检测无人机(规格 §2.2.1)
|
||||
_DETECT_PARAM_X = {"person": 0, "drone": 1}
|
||||
|
||||
_M_PER_DEG_LAT = 111320.0 # 纬度每度约合米(局部平面近似,够台架判读)
|
||||
|
||||
|
||||
# --- 纯逻辑:不碰串口,便于单测 ---------------------------------------------
|
||||
class Feeder:
|
||||
"""按拍生成伪造的 表3 载机数据。"""
|
||||
|
||||
def __init__(self, lat_deg: float, lon_deg: float, abs_alt_m: float,
|
||||
rel_alt_m: float, roll_deg: float = 0.0, pitch_deg: float = 0.0,
|
||||
yaw_deg: float = 0.0, yaw_rate_dps: float = 0.0,
|
||||
ground_speed_mps: float = 0.0,
|
||||
freeze_time: bool = False) -> None:
|
||||
self.lat = lat_deg
|
||||
self.lon = lon_deg
|
||||
self.abs_alt = abs_alt_m
|
||||
self.rel_alt = rel_alt_m
|
||||
self.base_roll = roll_deg
|
||||
self.base_pitch = pitch_deg
|
||||
self.base_yaw = yaw_deg
|
||||
self.yaw_rate = yaw_rate_dps
|
||||
self.ground_speed = ground_speed_mps
|
||||
self.freeze_time = freeze_time
|
||||
if freeze_time:
|
||||
self._frozen = self._utc_fields(datetime.now(timezone.utc))
|
||||
|
||||
@staticmethod
|
||||
def _utc_fields(now: datetime) -> dict:
|
||||
return {
|
||||
"year": now.year, "month": now.month, "day": now.day,
|
||||
"hour": now.hour, "minute": now.minute, "second": now.second,
|
||||
"centisecond": now.microsecond // 10000,
|
||||
}
|
||||
|
||||
def build(self, elapsed_s: float, now: Optional[datetime] = None) -> CarrierAttitude:
|
||||
"""生成 elapsed_s 时刻的一帧 表3(默认 yaw 与时戳随时间推进)。"""
|
||||
fields = (self._frozen if self.freeze_time
|
||||
else self._utc_fields(now or datetime.now(timezone.utc)))
|
||||
yaw = (self.base_yaw + self.yaw_rate * elapsed_s) % 360.0
|
||||
# 地速不为 0 时让位置沿 yaw 方向做匀速直线推进(阶段3 用)
|
||||
d_north = d_east = 0.0
|
||||
if self.ground_speed:
|
||||
rad = math.radians(self.base_yaw)
|
||||
dist = self.ground_speed * elapsed_s
|
||||
d_north = dist * math.cos(rad) / _M_PER_DEG_LAT
|
||||
d_east = dist * math.sin(rad) / (_M_PER_DEG_LAT
|
||||
* math.cos(math.radians(self.lat)))
|
||||
return CarrierAttitude(
|
||||
roll_deg=self.base_roll,
|
||||
pitch_deg=self.base_pitch,
|
||||
yaw_deg=yaw,
|
||||
latitude_deg=self.lat + d_north,
|
||||
longitude_deg=self.lon + d_east,
|
||||
abs_alt_m=self.abs_alt,
|
||||
rel_alt_m=self.rel_alt,
|
||||
airspeed_mps=self.ground_speed,
|
||||
ground_speed_mps=self.ground_speed,
|
||||
**fields,
|
||||
)
|
||||
|
||||
|
||||
class Collector:
|
||||
"""接收侧统计。热路径只调用 update(),不打日志。"""
|
||||
|
||||
def __init__(self, ref_lat_deg: float, ref_lon_deg: float) -> None:
|
||||
self.ref_lat = ref_lat_deg
|
||||
self.ref_lon = ref_lon_deg
|
||||
self.n_tele = 0
|
||||
self.n_tgt = 0
|
||||
self.n_other = 0
|
||||
self.n_loopback = 0 # 疑似自发自收(收到自己发的 表3)
|
||||
self.first_tgt_s: Optional[float] = None
|
||||
self.last_tgt_s: Optional[float] = None
|
||||
self.first_lock_s: Optional[float] = None
|
||||
self.n_lock_events = 0
|
||||
self._last_locked: Optional[bool] = None
|
||||
self._lats: List[float] = []
|
||||
self._lons: List[float] = []
|
||||
self._abs_alts: List[float] = []
|
||||
self._rel_alts: List[float] = []
|
||||
self._east: List[float] = []
|
||||
self._north: List[float] = []
|
||||
self.records: List[dict] = []
|
||||
|
||||
# --- 每帧更新(热路径) ---
|
||||
def update(self, frame, now_s: float) -> None:
|
||||
if isinstance(frame, Telemetry):
|
||||
self.n_tele += 1
|
||||
locked = frame.is_target_locked
|
||||
if self._last_locked is not None and locked != self._last_locked:
|
||||
self.n_lock_events += 1
|
||||
if locked and self.first_lock_s is None:
|
||||
self.first_lock_s = now_s
|
||||
self._last_locked = locked
|
||||
elif isinstance(frame, TargetGeo):
|
||||
self.n_tgt += 1
|
||||
if self.first_tgt_s is None:
|
||||
self.first_tgt_s = now_s
|
||||
self.last_tgt_s = now_s
|
||||
east, north = offset_m(self.ref_lat, self.ref_lon,
|
||||
frame.latitude_deg, frame.longitude_deg)
|
||||
self._lats.append(frame.latitude_deg)
|
||||
self._lons.append(frame.longitude_deg)
|
||||
self._abs_alts.append(frame.abs_alt_m)
|
||||
self._rel_alts.append(frame.rel_alt_m)
|
||||
self._east.append(east)
|
||||
self._north.append(north)
|
||||
self.records.append({
|
||||
"t_s": round(now_s, 3),
|
||||
"lat": frame.latitude_deg, "lon": frame.longitude_deg,
|
||||
"abs_alt_m": frame.abs_alt_m, "rel_alt_m": frame.rel_alt_m,
|
||||
"east_m": east, "north_m": north,
|
||||
"locked": self._last_locked,
|
||||
})
|
||||
elif isinstance(frame, CarrierAttitude):
|
||||
self.n_loopback += 1
|
||||
else:
|
||||
self.n_other += 1
|
||||
|
||||
@property
|
||||
def has_valid_geo(self) -> bool:
|
||||
"""表4 是否出现过非零经纬度。"""
|
||||
return any(lat != 0.0 or lon != 0.0
|
||||
for lat, lon in zip(self._lats, self._lons))
|
||||
|
||||
def summary(self, elapsed_s: float, n_sent: int,
|
||||
ref_abs_alt: float, ref_rel_alt: float) -> dict:
|
||||
"""生成汇总 dict(elapsed_s <= 0 时频率按 0 处理)。"""
|
||||
def _hz(n: int) -> float:
|
||||
return n / elapsed_s if elapsed_s > 0 else 0.0
|
||||
|
||||
jitter = (ptpm(self._east), ptpm(self._north))
|
||||
return {
|
||||
"elapsed_s": round(elapsed_s, 2),
|
||||
"has_valid_geo": self.has_valid_geo,
|
||||
"attitude_sent": n_sent,
|
||||
"attitude_hz": round(_hz(n_sent), 2),
|
||||
"telemetry_frames": self.n_tele,
|
||||
"telemetry_hz": round(_hz(self.n_tele), 2),
|
||||
"lock_events": self.n_lock_events,
|
||||
"first_lock_s": self.first_lock_s,
|
||||
"target_frames": self.n_tgt,
|
||||
"target_hz": round(_hz(self.n_tgt), 2),
|
||||
"first_target_s": self.first_tgt_s,
|
||||
"last_target_s": self.last_tgt_s,
|
||||
"target_stale_s": (round(elapsed_s - self.last_tgt_s, 2)
|
||||
if self.last_tgt_s is not None else None),
|
||||
"lat_mean": _mean(self._lats),
|
||||
"lon_mean": _mean(self._lons),
|
||||
"lat_stdev_deg": round(stddev(self._lats), 9),
|
||||
"lon_stdev_deg": round(stddev(self._lons), 9),
|
||||
"east_mean_m": round(_mean(self._east), 2),
|
||||
"north_mean_m": round(_mean(self._north), 2),
|
||||
"spread_east_m": round(jitter[0], 2),
|
||||
"spread_north_m": round(jitter[1], 2),
|
||||
"abs_alt_mean_m": round(_mean(self._abs_alts), 2),
|
||||
"rel_alt_mean_m": round(_mean(self._rel_alts), 2),
|
||||
"ref_abs_alt_m": ref_abs_alt,
|
||||
"ref_rel_alt_m": ref_rel_alt,
|
||||
"loopback_frames": self.n_loopback,
|
||||
"other_frames": self.n_other,
|
||||
}
|
||||
|
||||
|
||||
def offset_m(ref_lat: float, ref_lon: float, lat: float,
|
||||
lon: float) -> tuple:
|
||||
"""目标相对参考点的水平偏移(east_m, north_m),局部平面近似。"""
|
||||
north = (lat - ref_lat) * _M_PER_DEG_LAT
|
||||
east = (lon - ref_lon) * _M_PER_DEG_LAT * math.cos(math.radians(ref_lat))
|
||||
return east, north
|
||||
|
||||
|
||||
def _mean(values: Sequence[float]) -> float:
|
||||
return sum(values) / len(values) if values else 0.0
|
||||
|
||||
|
||||
def stddev(values: Sequence[float]) -> float:
|
||||
"""总体标准差(空序列返回 0)。"""
|
||||
if len(values) < 2:
|
||||
return 0.0
|
||||
mu = _mean(values)
|
||||
return math.sqrt(sum((v - mu) ** 2 for v in values) / len(values))
|
||||
|
||||
|
||||
def ptpm(values: Sequence[float]) -> float:
|
||||
"""峰峰值 max-min,空序列返回 0。"""
|
||||
return max(values) - min(values) if values else 0.0
|
||||
|
||||
|
||||
def verdict(summary: dict, link_error: Optional[BaseException]) -> tuple:
|
||||
"""判读:(退出码, 结论文本列表)。"""
|
||||
lines = []
|
||||
if link_error is not None:
|
||||
lines.append(f"❌ 串口链路中断: {link_error}")
|
||||
lines.append(" 排查: ① 是否另有进程占用该串口(listen_pod/pod_console/bridge); "
|
||||
"② 内核控制台未清(serial-getty / cmdline.txt 的 console=); "
|
||||
"③ TX/RX 是否交叉、共地、波特率一致")
|
||||
return EXIT_LINK_ERROR, lines
|
||||
if summary["loopback_frames"]:
|
||||
lines.append(f"⚠ 收到 {summary['loopback_frames']} 帧 表3 回环:"
|
||||
" TX/RX 疑似短接或自发自收,请查接线")
|
||||
|
||||
n_tgt = summary["target_frames"]
|
||||
if n_tgt == 0:
|
||||
lines.append("❌ Q1 失败: 完全未收到 表4(EE 18)。可能原因:")
|
||||
lines.append(" ① 未锁定目标 —— 加 --detect person/drone --auto-lock 重试;")
|
||||
lines.append(" ② 吊舱无激光,确实不做目标地理解算 —— 转自有双目视距链路;")
|
||||
lines.append(" ③ 需要先给表3 喂若干秒后吊舱才开始解算(已跑 %.0fs)"
|
||||
% summary["elapsed_s"])
|
||||
return EXIT_NO_TARGET_FRAME, lines
|
||||
if not summary["has_valid_geo"]:
|
||||
lines.append("⚠ Q1 存疑: 收到 %d 帧 表4 但经纬度恒为 0 —— 有帧无解算。" % n_tgt)
|
||||
lines.append(" 建议: 换更高的 --rel-alt、确认目标已锁定,"
|
||||
"并准备向厂商确认「无激光时如何解算目标绝对/相对高度」")
|
||||
return EXIT_ZERO_GEO, lines
|
||||
|
||||
lines.append("✅ Q1 通过: 无激光仍能解算 表4,"
|
||||
f"实测 {summary['target_hz']:.2f} Hz")
|
||||
if summary["first_target_s"] is not None:
|
||||
lines.append(f" Q2 表4 频率 {summary['target_hz']:.2f}Hz,"
|
||||
f"首次出现于 {summary['first_target_s']:.1f}s")
|
||||
if summary["first_lock_s"] is not None:
|
||||
lines.append(f" 首次锁定 {summary['first_lock_s']:.1f}s →"
|
||||
f" 表4 滞后 {summary['first_target_s'] - summary['first_lock_s']:.1f}s")
|
||||
lines.append(f" 目标相对我方: 东 {summary['east_mean_m']:+.1f}m"
|
||||
f" 北 {summary['north_mean_m']:+.1f}m,"
|
||||
f"散布 E/W {summary['spread_east_m']:.2f}m"
|
||||
f" N/S {summary['spread_north_m']:.2f}m")
|
||||
# 「目标贴地」假设的证据:表4 高度是否等于我方喂进去的载机高度
|
||||
if abs(summary["rel_alt_mean_m"] - summary["ref_rel_alt_m"]) < 1.0:
|
||||
lines.append(" ⚠ 表4 相对高度 ≈ 我方喂入的载机相对高度"
|
||||
f"({summary['ref_rel_alt_m']:.0f}m)"
|
||||
" → 吊舱很可能假设「目标在地面」,该假设对空中/楼顶目标失效")
|
||||
return EXIT_OK, lines
|
||||
|
||||
|
||||
# --- 命令行入口 -------------------------------------------------------------
|
||||
def main(argv: Optional[Sequence[str]] = None) -> int:
|
||||
ap = argparse.ArgumentParser(description="模拟飞控喂 表3,验证吊舱解算能力")
|
||||
ap.add_argument("port", nargs="?", default="/dev/ttyAMA0",
|
||||
help="串口设备号(默认 /dev/ttyAMA0)")
|
||||
ap.add_argument("--baud", type=int, default=115200, help="波特率(默认 115200)")
|
||||
ap.add_argument("--seconds", type=float, default=60.0,
|
||||
help="运行秒数(默认 60;0 = 一直跑到 Ctrl+C)")
|
||||
ap.add_argument("--rate", type=float, default=20.0,
|
||||
help="表3 发送频率 Hz(规格 1~60,默认 20)")
|
||||
ap.add_argument("--lat", type=float, default=39.9042,
|
||||
help="伪造载机纬度(默认 39.9042)")
|
||||
ap.add_argument("--lon", type=float, default=116.4074,
|
||||
help="伪造载机经度(默认 116.4074)")
|
||||
ap.add_argument("--abs-alt", type=float, default=None,
|
||||
help="伪造绝对高度 m(默认取 --rel-alt,即假设起飞点海拔 0)")
|
||||
ap.add_argument("--rel-alt", type=float, default=50.0,
|
||||
help="伪造相对高度 m(默认 50,典型作业高度)")
|
||||
ap.add_argument("--roll", type=float, default=0.0, help="静态横滚 °")
|
||||
ap.add_argument("--pitch", type=float, default=0.0, help="静态俯仰 °")
|
||||
ap.add_argument("--yaw", type=float, default=0.0, help="起始方位 °")
|
||||
ap.add_argument("--yaw-rate", type=float, default=0.0,
|
||||
help="方位角速度 °/s(阶段3 动态姿态验证用)")
|
||||
ap.add_argument("--speed", type=float, default=0.0,
|
||||
help="伪造地速 m/s(>0 时位置沿 --yaw 方向匀速推进)")
|
||||
ap.add_argument("--freeze-time", action="store_true",
|
||||
help="表3 时戳冻结在启动时刻(对照实验:排除吊舱校验时间)")
|
||||
ap.add_argument("--detect", choices=sorted(_DETECT_PARAM_X),
|
||||
help="启动时下发 目标识别开(0x07)")
|
||||
ap.add_argument("--auto-lock", action="store_true",
|
||||
help="在目标识别后追加 自动锁定(0x0F)+ 识别转跟踪(0x10)")
|
||||
ap.add_argument("--motor-on", action="store_true", help="启动时先下发 开电机(0x27)")
|
||||
ap.add_argument("--unlock-on-exit", action="store_true",
|
||||
help="退出前发 0x0E 停止跟踪")
|
||||
ap.add_argument("--json", metavar="PATH", help="同时落一份原始采样 JSON")
|
||||
ap.add_argument("--quiet", action="store_true", help="不打印每秒进度,只在结束输出汇总")
|
||||
args = ap.parse_args(argv)
|
||||
|
||||
if not 1.0 <= args.rate <= 60.0:
|
||||
print("[sim_fc] --rate 应在规格 1~60Hz 范围内(当前 %.1f)" % args.rate)
|
||||
return EXIT_LINK_ERROR
|
||||
if args.auto_lock and not args.detect:
|
||||
print("[sim_fc] --auto-lock 需配合 --detect"
|
||||
"(0x0F 是常驻模式,但要先开 0x07 才有识别源)")
|
||||
return EXIT_LINK_ERROR
|
||||
if not args.quiet:
|
||||
print(f"[sim_fc] {args.port} @ {args.baud} — 喂表3 lat={args.lat} "
|
||||
f"lon={args.lon} rel_alt={args.rel_alt}m @{args.rate}Hz, "
|
||||
f"{args.seconds:.0f}s Ctrl+C 可提前结束")
|
||||
|
||||
abs_alt = args.abs_alt if args.abs_alt is not None else args.rel_alt
|
||||
feeder = Feeder(args.lat, args.lon, abs_alt, args.rel_alt,
|
||||
args.roll, args.pitch, args.yaw, args.yaw_rate,
|
||||
args.speed, args.freeze_time)
|
||||
collector = Collector(args.lat, args.lon)
|
||||
|
||||
try:
|
||||
pod = PodSerial(args.port, args.baud)
|
||||
except Exception as exc:
|
||||
print(f"[sim_fc] 打开串口失败: {exc}")
|
||||
return EXIT_LINK_ERROR
|
||||
|
||||
if args.motor_on:
|
||||
pod.send(encode_control(ControlCommand(code=ControlCode.MOTOR_ON)))
|
||||
if not args.quiet:
|
||||
print("[sim_fc] 已下发 开电机(0x27)")
|
||||
if args.detect:
|
||||
px = _DETECT_PARAM_X[args.detect]
|
||||
pod.send(encode_control(ControlCommand(
|
||||
code=ControlCode.TARGET_DETECT_ON, param_x=px)))
|
||||
if not args.quiet:
|
||||
print(f"[sim_fc] 已下发 目标识别开(0x07, X={px} "
|
||||
f"{'人/车' if px == 0 else '无人机'})")
|
||||
if args.auto_lock:
|
||||
pod.send(encode_control(
|
||||
ControlCommand(code=ControlCode.AUTO_LOCK_ON_FOUND)))
|
||||
pod.send(encode_control(
|
||||
ControlCommand(code=ControlCode.DETECT_TO_TRACK)))
|
||||
if not args.quiet:
|
||||
print("[sim_fc] 已追加 自动锁定(0x0F)+ 识别转跟踪(0x10)")
|
||||
|
||||
link_error: Optional[BaseException] = None
|
||||
t0 = time.monotonic()
|
||||
next_send = 0.0
|
||||
next_report = 1.0
|
||||
n_sent = 0
|
||||
|
||||
try:
|
||||
while True:
|
||||
now_s = time.monotonic() - t0
|
||||
if args.seconds > 0 and now_s >= args.seconds:
|
||||
break
|
||||
|
||||
if now_s >= next_send:
|
||||
try:
|
||||
pod.send(encode_attitude(feeder.build(now_s)))
|
||||
n_sent += 1
|
||||
except OSError as exc:
|
||||
link_error = exc
|
||||
break
|
||||
next_send += 1.0 / args.rate
|
||||
if next_send < now_s: # 落后太多(系统卡顿)则重置节拍
|
||||
next_send = now_s + 1.0 / args.rate
|
||||
|
||||
try:
|
||||
for frame in pod.poll():
|
||||
collector.update(frame, now_s)
|
||||
except OSError as exc:
|
||||
link_error = exc
|
||||
break
|
||||
|
||||
if not args.quiet and now_s >= next_report:
|
||||
print(f"[sim_fc] t={now_s:5.1f}s 发表3={n_sent} "
|
||||
f"遥测={collector.n_tele} 目标={collector.n_tgt} "
|
||||
f"锁定次数={collector.n_lock_events}")
|
||||
next_report += 1.0
|
||||
time.sleep(0.002)
|
||||
except KeyboardInterrupt:
|
||||
if not args.quiet:
|
||||
print("\n[sim_fc] 用户中断,仍输出汇总")
|
||||
|
||||
elapsed = time.monotonic() - t0
|
||||
if args.unlock_on_exit:
|
||||
try:
|
||||
pod.send(encode_control(ControlCommand(code=ControlCode.STOP_TRACK)))
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
pod.close()
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
summary = collector.summary(elapsed, n_sent, abs_alt, args.rel_alt)
|
||||
code, lines = verdict(summary, link_error)
|
||||
|
||||
print("\n[sim_fc] ===== 汇总 =====")
|
||||
print(f" 运行时长 {summary['elapsed_s']:.1f}s")
|
||||
print(f" 表3 发送 {summary['attitude_sent']} 帧 ({summary['attitude_hz']:.1f}Hz)")
|
||||
print(f" 表2-2 遥测 {summary['telemetry_frames']} 帧 ({summary['telemetry_hz']:.1f}Hz)")
|
||||
print(f" 锁定次数 {summary['lock_events']}"
|
||||
+ (f", 首次 {summary['first_lock_s']:.1f}s"
|
||||
if summary['first_lock_s'] is not None else " (未观察到锁定)"))
|
||||
print(f" 表4 目标 {summary['target_frames']} 帧 ({summary['target_hz']:.2f}Hz)")
|
||||
if summary["target_frames"]:
|
||||
print(f" 经纬度均值 {summary['lat_mean']:.7f}, {summary['lon_mean']:.7f}")
|
||||
print(f" 散布 σ lat {summary['lat_stdev_deg']:.2e}° "
|
||||
f"lon {summary['lon_stdev_deg']:.2e}°")
|
||||
print(f" 相对我方 东 {summary['east_mean_m']:+.1f}m "
|
||||
f"北 {summary['north_mean_m']:+.1f}m "
|
||||
f"(散布 {summary['spread_east_m']:.2f}m / {summary['spread_north_m']:.2f}m)")
|
||||
print(f" 绝对/相对高度 {summary['abs_alt_mean_m']:.1f} / "
|
||||
f"{summary['rel_alt_mean_m']:.1f} m (喂入 {abs_alt:.0f} / {args.rel_alt:.0f})")
|
||||
if summary["target_stale_s"] is not None:
|
||||
print(f" 最后更新距今 {summary['target_stale_s']}s")
|
||||
for ln in lines:
|
||||
print(" " + ln)
|
||||
|
||||
if args.json:
|
||||
payload = {"args": vars(args), "summary": summary,
|
||||
"samples": collector.records}
|
||||
Path(args.json).write_text(json.dumps(payload, ensure_ascii=False, indent=2),
|
||||
encoding="utf-8")
|
||||
print(f"[sim_fc] 原始采样已写入 {args.json}")
|
||||
return code
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user