新增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())
|
||||
@@ -0,0 +1,269 @@
|
||||
"""sim_fc_test.py —— 模拟飞控喂表3 工具的纯软单测(注入 fake serial,无硬件)。
|
||||
|
||||
覆盖:
|
||||
* Feeder:静态姿态 / 时戳冻结 / yaw 匀速推进 / 地速位移;
|
||||
* offset_m: East/North 换算方向;
|
||||
* Collector:遥测与表4 计数、锁定跳变、贴地假设、回环帧识别;
|
||||
* stddev / ptpm 统计;
|
||||
* verdict 四种判读分支;
|
||||
* main() 端到端(假串口回回放表4)→ 退出码 0。
|
||||
"""
|
||||
|
||||
import sys
|
||||
import types
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.protocol import (CarrierAttitude, TargetGeo, Telemetry,
|
||||
encode_attitude, encode_target)
|
||||
from pod_bridge.sim_fc import (EXIT_LINK_ERROR, EXIT_NO_TARGET_FRAME, EXIT_OK,
|
||||
EXIT_ZERO_GEO, Collector, Feeder, main, offset_m,
|
||||
ptpm, stddev, verdict)
|
||||
|
||||
_LAT, _LON = 39.9042, 116.4074
|
||||
|
||||
|
||||
class _FakeSerialInstance:
|
||||
"""pyserial.Serial 的最小替身:可预设 RX 字节,记录 TX 写入。"""
|
||||
|
||||
def __init__(self, **kwargs):
|
||||
self.kwargs = kwargs
|
||||
self._buf = b""
|
||||
self.written = []
|
||||
self.closed = False
|
||||
|
||||
def seed_rx(self, data: bytes) -> None:
|
||||
self._buf += data
|
||||
|
||||
@property
|
||||
def in_waiting(self):
|
||||
return len(self._buf)
|
||||
|
||||
def read(self, n):
|
||||
data, self._buf = self._buf[:n], self._buf[n:]
|
||||
return data
|
||||
|
||||
def write(self, data):
|
||||
self.written.append(bytes(data))
|
||||
return len(data)
|
||||
|
||||
def close(self):
|
||||
self.closed = True
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def fake_serial(monkeypatch):
|
||||
"""注入 serial 模块并返回**共享实例**——PodSerial 内部 Serial(...) 必须拿到
|
||||
同一个对象,否则外部 seed_rx 预置的字节 main() 读不到。"""
|
||||
mod = types.ModuleType("serial")
|
||||
mod.EIGHTBITS = 8
|
||||
mod.PARITY_NONE = "N"
|
||||
mod.STOPBITS_ONE = 1
|
||||
shared = _FakeSerialInstance(port="FAKE")
|
||||
mod.Serial = lambda **kwargs: shared
|
||||
monkeypatch.setitem(sys.modules, "serial", mod)
|
||||
return shared
|
||||
|
||||
|
||||
def _locked_telemetry(locked: bool) -> Telemetry:
|
||||
status1 = (1 << 2) if locked else 0
|
||||
return Telemetry(status1=status1)
|
||||
|
||||
|
||||
# --- Feeder -----------------------------------------------------------------
|
||||
def test_feeder_static_defaults():
|
||||
f = Feeder(_LAT, _LON, abs_alt_m=50.0, rel_alt_m=50.0)
|
||||
frame = f.build(0.0)
|
||||
assert isinstance(frame, CarrierAttitude)
|
||||
assert (frame.latitude_deg, frame.longitude_deg) == (_LAT, _LON)
|
||||
assert (frame.roll_deg, frame.pitch_deg, frame.yaw_deg) == (0.0, 0.0, 0.0)
|
||||
assert (frame.abs_alt_m, frame.rel_alt_m) == (50.0, 50.0)
|
||||
assert 0 <= frame.centisecond <= 99
|
||||
|
||||
|
||||
def test_feeder_freeze_time_keeps_stamp():
|
||||
f = Feeder(_LAT, _LON, 50.0, 50.0, freeze_time=True)
|
||||
assert f.build(0.0).second == f.build(30.0).second
|
||||
|
||||
|
||||
def test_feeder_yaw_sweep_wraps():
|
||||
f = Feeder(_LAT, _LON, 50.0, 50.0, yaw_deg=350.0, yaw_rate_dps=20.0)
|
||||
assert abs(f.build(0.0).yaw_deg - 350.0) < 1e-9
|
||||
assert abs(f.build(1.0).yaw_deg - 10.0) < 1e-9 # 360 → 回绕到 10°
|
||||
|
||||
|
||||
def test_feeder_ground_speed_moves_position_north():
|
||||
f = Feeder(_LAT, _LON, 50.0, 50.0, yaw_deg=0.0, ground_speed_mps=10.0)
|
||||
later = f.build(10.0) # 向北 100m
|
||||
north_m = (later.latitude_deg - _LAT) * 111320.0
|
||||
assert 95.0 < north_m < 105.0
|
||||
assert abs((later.longitude_deg - _LON) * 111320.0) < 1.0
|
||||
|
||||
|
||||
# --- 几何 / 统计 ------------------------------------------------------------
|
||||
def test_offset_m_directions():
|
||||
east, north = offset_m(0.0, 0.0, 0.0, 0.0001)
|
||||
assert north == pytest.approx(0.0, abs=1e-6)
|
||||
assert east > 10.0
|
||||
east2, north2 = offset_m(0.0, 0.0, 0.0001, 0.0)
|
||||
assert east2 == pytest.approx(0.0, abs=1e-6)
|
||||
assert north2 > 10.0
|
||||
|
||||
|
||||
def test_stddev_and_ptpm():
|
||||
assert stddev([1.0]) == 0.0
|
||||
assert stddev([]) == 0.0
|
||||
assert stddev([2.0, 2.0, 2.0]) == 0.0
|
||||
assert stddev([1.0, 3.0]) == pytest.approx(1.0)
|
||||
assert ptpm([]) == 0.0
|
||||
assert ptpm([1.0, 5.0, 3.0]) == pytest.approx(4.0)
|
||||
|
||||
|
||||
# --- Collector --------------------------------------------------------------
|
||||
def test_collector_counts_and_lock_events():
|
||||
c = Collector(_LAT, _LON)
|
||||
for t, lk in ((0.1, False), (0.2, True), (0.3, True), (0.4, False)):
|
||||
c.update(_locked_telemetry(lk), t)
|
||||
assert c.n_tele == 4
|
||||
assert c.n_lock_events == 2 # False→True, True→False
|
||||
assert c.first_lock_s == pytest.approx(0.2)
|
||||
assert c.n_tgt == 0
|
||||
|
||||
|
||||
def test_collector_target_stats_and_first_frame():
|
||||
c = Collector(_LAT, _LON)
|
||||
assert c.has_valid_geo is False
|
||||
c.update(TargetGeo(latitude_deg=_LAT, longitude_deg=_LON), 1.0)
|
||||
c.update(TargetGeo(latitude_deg=_LAT + 0.0001, longitude_deg=_LON), 2.0)
|
||||
assert c.has_valid_geo is True
|
||||
assert c.first_tgt_s == pytest.approx(1.0)
|
||||
assert c.last_tgt_s == pytest.approx(2.0)
|
||||
north = c.records[-1]["north_m"]
|
||||
assert north == pytest.approx(11.132, abs=0.5)
|
||||
|
||||
|
||||
def test_collector_flags_loopback_and_unknown():
|
||||
c = Collector(_LAT, _LON)
|
||||
c.update(CarrierAttitude(), 0.1)
|
||||
c.update(object(), 0.2)
|
||||
assert c.n_loopback == 1
|
||||
assert c.n_other == 1
|
||||
|
||||
|
||||
def test_collector_summary_includes_geo_validity():
|
||||
c = Collector(_LAT, _LON)
|
||||
assert c.summary(1.0, 20, 50.0, 50.0)["has_valid_geo"] is False
|
||||
c.update(TargetGeo(latitude_deg=_LAT, longitude_deg=_LON, rel_alt_m=50.0), 0.5)
|
||||
summary = c.summary(1.0, 20, 50.0, 50.0)
|
||||
assert summary["has_valid_geo"] is True
|
||||
assert summary["target_hz"] == pytest.approx(1.0)
|
||||
assert summary["target_stale_s"] == pytest.approx(0.5)
|
||||
|
||||
|
||||
# --- verdict ----------------------------------------------------------------
|
||||
def _summary(**over):
|
||||
base = {
|
||||
"elapsed_s": 10.0, "target_frames": 0, "target_hz": 0.0,
|
||||
"first_target_s": None, "first_lock_s": None, "has_valid_geo": False,
|
||||
"loopback_frames": 0, "east_mean_m": 0.0, "north_mean_m": 0.0,
|
||||
"spread_east_m": 0.0, "spread_north_m": 0.0,
|
||||
"rel_alt_mean_m": 0.0, "ref_rel_alt_m": 50.0,
|
||||
}
|
||||
base.update(over)
|
||||
return base
|
||||
|
||||
|
||||
def test_verdict_no_target_frame():
|
||||
code, lines = verdict(_summary(), None)
|
||||
assert code == EXIT_NO_TARGET_FRAME
|
||||
assert any("Q1 失败" in ln for ln in lines)
|
||||
|
||||
|
||||
def test_verdict_zero_geo():
|
||||
code, lines = verdict(_summary(target_frames=100), None)
|
||||
assert code == EXIT_ZERO_GEO
|
||||
assert any("有帧无解算" in ln for ln in lines)
|
||||
|
||||
|
||||
def test_verdict_ok_and_ground_plane_warning():
|
||||
code, lines = verdict(_summary(target_frames=100, target_hz=5.5,
|
||||
has_valid_geo=True, first_target_s=3.4,
|
||||
first_lock_s=3.0, rel_alt_mean_m=50.0), None)
|
||||
assert code == EXIT_OK
|
||||
assert any("Q1 通过" in ln for ln in lines)
|
||||
assert any("目标在地面" in ln for ln in lines) # 表4 相对高度 ≈ 喂入值
|
||||
|
||||
|
||||
def test_verdict_link_error_takes_priority():
|
||||
code, lines = verdict(_summary(target_frames=100, has_valid_geo=True),
|
||||
OSError(5, "Input/output error"))
|
||||
assert code == EXIT_LINK_ERROR
|
||||
assert any("链路中断" in ln for ln in lines)
|
||||
|
||||
|
||||
def test_verdict_flags_loopback():
|
||||
code, lines = verdict(_summary(target_frames=1, has_valid_geo=True,
|
||||
loopback_frames=7), None)
|
||||
assert code == EXIT_OK
|
||||
assert any("回环" in ln for ln in lines)
|
||||
|
||||
|
||||
# --- main() 端到端(假串口回环) ---------------------------------------------
|
||||
def test_main_end_to_end_reports_target(fake_serial, tmp_path, capsys):
|
||||
inst = fake_serial
|
||||
target = TargetGeo(latitude_deg=_LAT + 0.0002, longitude_deg=_LON,
|
||||
abs_alt_m=50.0, rel_alt_m=50.0)
|
||||
inst.seed_rx(encode_target(target))
|
||||
|
||||
code = main(["FAKE", "--seconds", "0.05", "--rate", "60",
|
||||
"--rel-alt", "50", "--quiet", "--json", str(tmp_path / "r.json")])
|
||||
out = capsys.readouterr().out
|
||||
|
||||
assert code == EXIT_OK
|
||||
assert inst.closed is True
|
||||
assert len(inst.written) >= 1 # 至少发过一帧 表3
|
||||
assert inst.written[0].startswith(b"\xeb\x91") # 同步字 EB 91
|
||||
assert "Q1 通过" in out
|
||||
assert (tmp_path / "r.json").exists()
|
||||
|
||||
|
||||
def test_main_no_target_returns_error(fake_serial, capsys):
|
||||
fake_serial # RX 为空
|
||||
code = main(["FAKE", "--seconds", "0.05", "--rate", "60", "--quiet"])
|
||||
assert code == EXIT_NO_TARGET_FRAME
|
||||
|
||||
|
||||
def test_main_rejects_bad_rate(fake_serial, capsys):
|
||||
code = main(["FAKE", "--rate", "100", "--quiet"])
|
||||
out = capsys.readouterr().out
|
||||
assert code == EXIT_LINK_ERROR
|
||||
assert "1~60Hz" in out
|
||||
|
||||
|
||||
def test_main_auto_lock_requires_detect(fake_serial, capsys):
|
||||
code = main(["FAKE", "--auto-lock", "--quiet"])
|
||||
out = capsys.readouterr().out
|
||||
assert code == EXIT_LINK_ERROR
|
||||
assert "--auto-lock 需配合" in out
|
||||
|
||||
|
||||
def test_main_sends_detect_sequence(fake_serial, capsys):
|
||||
inst = fake_serial
|
||||
main(["FAKE", "--seconds", "0.02", "--rate", "60", "--quiet",
|
||||
"--detect", "drone", "--auto-lock", "--motor-on"])
|
||||
codes = [w[2] for w in inst.written]
|
||||
assert 0x27 in codes # 开电机
|
||||
assert 0x07 in codes # 目标识别开
|
||||
assert 0x0F in codes # 自动锁定
|
||||
assert 0x10 in codes # 识别转跟踪
|
||||
|
||||
|
||||
def test_feeder_frame_is_encodable():
|
||||
"""Feeder 产物必须能通过 protocol 编码/解码往返(防止字段对不上规格)。"""
|
||||
from pod_bridge.protocol import decode_attitude
|
||||
f = Feeder(_LAT, _LON, 50.0, 50.0, roll_deg=1.5, pitch_deg=-2.0, yaw_deg=30.0)
|
||||
frame = f.build(0.0)
|
||||
back = decode_attitude(encode_attitude(frame))
|
||||
assert back.latitude_deg == pytest.approx(frame.latitude_deg, abs=1e-7)
|
||||
assert back.rel_alt_m == pytest.approx(50.0, abs=0.2) # ALT_SCALE=0.2m
|
||||
assert back.yaw_deg == pytest.approx(30.0, abs=0.01)
|
||||
Reference in New Issue
Block a user