"""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())