diff --git a/docs/树莓派4B部署注意事项.md b/docs/树莓派4B部署注意事项.md new file mode 100644 index 0000000..a10a3ec --- /dev/null +++ b/docs/树莓派4B部署注意事项.md @@ -0,0 +1,143 @@ +# pod_bridge 移交树莓派 4B 部署注意事项 + +> 目标平台:树莓派 4B + **Ubuntu 24.04(aarch64)**。 +> 范围:`utils/pod_bridge`(及其依赖的 `visual_track_control` 控制原语库)。 +> 核对日期:2026-09-23,对照规格《光电吊舱控制协议G版-PB.docx》; +> 依赖轮子已按 PyPI 2026-09-23 上游信息复核。 + +--- + +## 1. 系统与 Python 版本(硬性) + +- **Ubuntu 24.04(aarch64)**:Ubuntu 的 Pi 镜像只有 64 位,天然满足 MAVSDK + 对 aarch64 的要求(见 §3);不要自行改装任何 32 位系统。 +- **Python 3.12**(Ubuntu 24.04 自带)满足 PEP 604 标注(`float | None` / + `FrameParser | None`)要求的 ≥3.10,直接用,无需另装。 +- Ubuntu 24.04 系统 Python 受 PEP 668 保护,**禁止 `pip install` 进系统环境** + (报 externally-managed-environment),一律建 venv。注意 Ubuntu 默认 + **不带 venv 支持**,先 apt 补齐: + + ```bash + sudo apt update && sudo apt install -y python3-venv python3-pip + python3 -m venv ~/venv-pod + ~/venv-pod/bin/pip install -r requirements-pod.txt + ``` + +## 2. 依赖裁剪(树莓派上别全装) + +`requirements.txt` 是全仓库清单,**吊舱桥接只需要其中两个**,务必裁剪, +否则 torch/cv2 在 Pi 4B 上是几百 MB 且毫无意义: + +```text +# requirements-pod.txt(桥接最小集) +mavsdk +pyserial +pytest # 仅上机自测需要,部署运行可省 +``` + +- `pod_bridge` 本身纯 Python + 惰性导入:`pyserial` 只在 `PodSerial` 实例化时加载, + `mavsdk` 只在 `Px4Gateway.connect` / 指令发送时加载——单测全绿不需要任何硬件库。 +- **不要**在 Pi 上装 ultralytics/torch/opencv:那是 `stereo_yolo` 检测链的依赖, + Pi 4B 跑不动实时 YOLO,检测应留在机载算力更强的平台或吊舱内部。 +- 若确实要装 opencv,用 `opencv-python-headless`(无 GUI 依赖), + 并 `sudo apt install libopenblas-dev libatlas-base-dev`。 + +## 3. MAVSDK 在 ARM 上的坑 + +- pip 的 `mavsdk` 包内嵌 `mavsdk_server` 二进制;**aarch64 有官方 wheel** + (PyPI 核对:mavsdk 4.0.0 提供 `cp38-abi3 manylinux2014_aarch64`, + abi3 向前兼容 Python 3.12,要求 glibc≥2.17,Ubuntu 24.04 为 2.39 ✅)。 + 32 位 armv7l 支持不稳定——Ubuntu 24.04 Pi 镜像只有 64 位,无此问题。 +- 安装后别只验证 import,要让内嵌二进制真跑一次: + `find ~/venv-pod -name mavsdk_server` 找到路径后直接执行(无参数即起服务, + Ctrl+C 退出),能打印启动信息说明架构匹配;`Exec format error` 则装错了架构。 +- 连接串写法(`pod_bridge.json` 的 `px4.connection_string`): + - Pi 串口直连飞控 TELEM:`serial:///dev/ttyUSB0:57600`(波特率按飞控侧设置); + - 经数传/仿真:`udp://:14540`。 +- PX4 侧需确认 TELEM 口已配 MAVLink 且 offboard 心跳未被占用。 + +## 4. 串口硬件(吊舱侧,最容易翻车) + +- 协议为 **115200 8N1 无校验**(规格 §2.1)。先确认吊舱串口电平: + - **RS232 → 必须加电平转换或直接用 USB 转串口模块**(FTDI/CH340,推荐, + 免折腾 GPIO UART,设备号 `/dev/ttyUSB0`); + - TTL 3.3V → 可用 Pi GPIO14/15(`/dev/serial0`)。 +- 用 GPIO UART 时(**Ubuntu 没有 raspi-config**,手工配置): + - `/boot/firmware/config.txt` 确认有 `enable_uart=1`;若控制台占着串口, + 关掉:`sudo systemctl disable --now serial-getty@ttyAMA0.service`; + - Pi 4B 上 GPIO UART 默认是 mini-UART(ttyS0,波特率随主频漂移),115200 下 + 不稳——同一文件加 `dtoverlay=disable-bt` 把 PL011(ttyAMA0)换回来 + (设备树 overlay 由固件加载,与发行版无关,Ubuntu 同样生效); + - **Ubuntu 没有 Raspberry Pi OS 的 `/dev/serial0` 软链**,设备名直接写 + `/dev/ttyAMA0`(或 ttyS0); + - **Pi 是 3.3V 电平,绝不直连 5V/RS232 电平**,会烧 GPIO。 +- 接线三线:TX/RX **交叉**、**必须共地(GND)**,不共地是通讯乱码的第一大原因。 +- 用户权限:`sudo usermod -aG dialout $USER`,重登录生效,否则 open 串口 Permission denied。 + +## 5. 时钟同步(规格表3 依赖) + +- `bridge._build_attitude` 用**宿主机 UTC 墙钟**填表3 的年月日时分秒; + Pi 4B **无 RTC**,断电后时间是错的,吊舱会拿到错误时间戳。 +- 上机后确认 `timedatectl` 显示 `System clock synchronized: yes`; + 无网环境考虑给 Pi 加 RTC 模块,或由飞控 GPS 授时(需改代码,见 §8 已知问题)。 + +## 6. 配置文件(`utils/config/pod_bridge.json`)上机必改两处 + +| 字段 | 改成 | +|---|---| +| `pod_serial.port` | 实际设备号,如 `/dev/ttyUSB0`(留空会 fail-fast 报错,这是刻意的) | +| `px4.connection_string` | 实际飞控连接,如 `serial:///dev/ttyUSB1:57600` | + +- 引导参数(`guidance.*`)全部是**占位值,未经标定不得用于飞行决策**; + 首次实机只验证通讯收发,速度上限先调小。 +- 配置加载拒绝未知键——改 JSON 时注意拼写,多写的键会直接报错(防呆设计)。 + +## 7. 上机验证顺序(每步过了再下一步) + +1. **纯软件自测**(无需任何接线): + `cd utils && ~/venv-pod/bin/python -m pytest tests/pod_bridge -q` → 应 92 全绿。 +2. **串口回环**:USB 转串口模块 TX/RX 短接,跑一个 feed/echo 小脚本验证字节收发。 +3. **只接吊舱**:观察能否解析出 表2-2(60Hz EE 16)帧;用 `send_control(归中 0x2B)` + 验证下行。 +4. **只接飞控**:`Px4Gateway.connect` 后确认 `own_state().position_valid` 为 True。 +5. **全链路地面联试**:电机不装桨,观察 offboard 心跳与引导输出。 +6. 最后才考虑带动力测试。 + +## 8. 已知问题处置状态(2026-09-23 更新) + +代码审查(2026-09-22)发现 4 项,其中 3 项安全相关缺陷**已于 2026-09-23 修复**, +新增回归测试 8 条(pod_bridge 测试 84 → 92,全量 348 条全绿): + +1. ~~`Px4Gateway` 四路遥测共用 `OwnState.time_ms`~~ → **已修**:`OwnState` 拆出 + `position_time_ms`(仅 position 流刷新),引导的位置超龄判定改用它; +2. ~~表2-2 锁定状态(bit2)未参与引导门控~~ → **已修**:`PodBridge._pod_locked` + 要求遥测在 `link.telemetry_timeout_ms` 内且 bit2=锁定,表4 坐标才视为有效 + (该配置项由此从死配置变为生效配置); +3. ~~`PodBridge.run` 主循环无异常护栏~~ → **已修**:单周期异常按「第 1 次 + + 每满 100 次」告警并补发零速刹车保 offboard 心跳,任务不终止; +4. 表3 时间戳用宿主机墙钟而非 GPS 时间(见 §5)——**仍未修**,属可接受风险, + 如需精确授时按 §5 对策处理。 + +## 9. 常驻运行(systemd 示例) + +```ini +# /etc/systemd/system/pod-bridge.service +[Unit] +Description=Pod-PX4 Bridge +After=network.target + +[Service] +ExecStart=/home//venv-pod/bin/python -m pod_bridge_app +WorkingDirectory=/home//code_drone-test/utils +Restart=always +RestartSec=3 +StandardOutput=journal +StandardError=journal + +[Install] +WantedBy=multi-user.target +``` + +- 热路径不打日志是设计使然(告警走 WarnThrottle),现场排查用 + `journalctl -u pod-bridge -f`。 +- 代码热路径按 10Hz 引导 + 20Hz 姿态下发,Pi 4B 单核占用可忽略,无需实时内核。 diff --git a/requirements-pod.txt b/requirements-pod.txt new file mode 100644 index 0000000..ae99a8e --- /dev/null +++ b/requirements-pod.txt @@ -0,0 +1,6 @@ +# requirements-pod.txt —— 树莓派 4B(Ubuntu 24.04)吊舱桥接最小依赖 +# 只装这几个,切勿在 Pi 上装全量 requirements.txt(torch/cv2 无意义且数百 MB) +# 详见 docs/树莓派4B部署注意事项.md §2 +mavsdk # PX4 遥测订阅 + offboard 指令(aarch64 官方 wheel) +pyserial # 吊舱串口(115200 8N1) +pytest # 仅上机自测(tests/pod_bridge 92 条)需要,部署运行可省 diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..1c7d90a --- /dev/null +++ b/requirements.txt @@ -0,0 +1,21 @@ +# code_drone-test Python 依赖 +# 扫描自全仓库 .py 文件的实际 import(2026-09-22) + +# ---- 运行时核心 ---- +numpy # target_tracker 卡尔曼滤波 / stereo_yolo sbs 拆分 +opencv-python # camera.py / camera_test.py / video_test.py / demo_stereo_yolo.py (cv2) + +# ---- 检测推理(stereo_yolo,真实推理才加载,torch 为其依赖) ---- +ultralytics + +# ---- 飞控通信(MAVSDK offboard -> PX4) ---- +mavsdk # test.py / demo_track_control.py / pod_bridge.px4_gateway + +# ---- 吊舱串口(pod_bridge.pod_serial,惰性导入) ---- +pyserial + +# ---- 可视化(test.py 轨迹绘图) ---- +matplotlib + +# ---- 测试 ---- +pytest # utils/tests 全量单测(256 条) diff --git a/utils/config/pod_bridge.json b/utils/config/pod_bridge.json new file mode 100644 index 0000000..8c32df9 --- /dev/null +++ b/utils/config/pod_bridge.json @@ -0,0 +1,29 @@ +{ + "pod_serial": { + "port": "", + "baudrate": 115200 + }, + "px4": { + "connection_string": "udp://:14540" + }, + "link": { + "attitude_downlink_hz": 20.0, + "telemetry_timeout_ms": 500 + }, + "guidance": { + "frequency_hz": 10.0, + "target_stale_ms": 1000, + "position_stale_ms": 1000, + "stop_radius_m": 5.0, + "max_speed_mps": 5.0, + "speed_kp": 0.5, + "speed_ki": 0.0, + "speed_kd": 0.0, + "speed_integral_limit": 0.0, + "speed_derivative_filter_coef": 0.0, + "speed_slew_mps2": 2.0, + "max_yaw_rate_dps": 30.0, + "yaw_kp": 1.0, + "yaw_slew_dps2": 60.0 + } +} diff --git a/utils/pod_bridge/__init__.py b/utils/pod_bridge/__init__.py new file mode 100644 index 0000000..a0e2946 --- /dev/null +++ b/utils/pod_bridge/__init__.py @@ -0,0 +1,108 @@ +"""pod_bridge —— 光电吊舱 ↔ PX4 飞控 协议桥接（规格：docs/光电吊舱控制协议G版-PB.docx）。 + +吊舱自带 CPU 完成目标解算；本包只做双向搬运 + 格式转换： + PX4 遥测(姿态/经纬度/时间) → 表3(EB 91) 串口下发吊舱 + 吊舱 表4(EE 18) 目标经纬度 + 表2-2(EE 16) 遥测 → offboard 速度设定点驱动 PX4 朝目标飞行 + +当前含纯协议编解码层 protocol（四帧 encode/decode + FrameParser）、地理位置 +引导律 guidance（载机位置 + 目标经纬度 → offboard 速度/航向）、配置 config +（BridgeConfig + load_from_json）、飞控侧网关 px4_gateway（读 PX4 遥测 + +复用 MavsdkCommandGateway 发 offboard 指令）、吊舱侧串口 pod_serial（PodSerial ++ PodLink 接口）、闭环主循环 bridge（PodBridge）；均可 mock 单测，无硬件依赖。 +""" + +from .bridge import PodBridge +from .config import ( + BridgeConfig, + LinkConfig, + PodSerialConfig, + Px4Config, + load_from_json, +) +from .guidance import ( + GuidanceConfig, + GuidanceInput, + GuidanceLaw, + haversine_distance_m, + initial_bearing_deg, +) +from .pod_serial import PodLink, PodSerial +from .protocol import ( + ALT_SCALE, + ANGLE_SCALE, + ATTITUDE_LEN, + CONTROL_LEN, + CarrierAttitude, + ControlCode, + ControlCommand, + FrameParser, + LATLON_SCALE, + RANGE_SCALE, + RATE_SCALE, + SPEED_SCALE, + SYNC_ATTITUDE, + SYNC_CONTROL, + SYNC_TARGET, + SYNC_TELEMETRY, + TARGET_LEN, + TELEMETRY_LEN, + Telemetry, + TargetGeo, + checksum, + decode_attitude, + decode_control, + decode_target, + decode_telemetry, + encode_attitude, + encode_control, + encode_target, + encode_telemetry, +) +from .px4_gateway import OwnState, Px4Gateway + +__all__ = [ + "ALT_SCALE", + "ANGLE_SCALE", + "ATTITUDE_LEN", + "BridgeConfig", + "CONTROL_LEN", + "CarrierAttitude", + "ControlCode", + "ControlCommand", + "FrameParser", + "GuidanceConfig", + "GuidanceInput", + "GuidanceLaw", + "LATLON_SCALE", + "LinkConfig", + "OwnState", + "PodBridge", + "PodLink", + "PodSerial", + "PodSerialConfig", + "Px4Config", + "Px4Gateway", + "RANGE_SCALE", + "RATE_SCALE", + "SPEED_SCALE", + "SYNC_ATTITUDE", + "SYNC_CONTROL", + "SYNC_TARGET", + "SYNC_TELEMETRY", + "TARGET_LEN", + "TELEMETRY_LEN", + "Telemetry", + "TargetGeo", + "checksum", + "decode_attitude", + "decode_control", + "decode_target", + "decode_telemetry", + "encode_attitude", + "encode_control", + "encode_target", + "encode_telemetry", + "haversine_distance_m", + "initial_bearing_deg", + "load_from_json", +] diff --git a/utils/pod_bridge/bridge.py b/utils/pod_bridge/bridge.py new file mode 100644 index 0000000..205eaf8 --- /dev/null +++ b/utils/pod_bridge/bridge.py @@ -0,0 +1,207 @@ +"""bridge.py —— 吊舱 ↔ PX4 桥接主循环（asyncio 胶水层）。 + +闭环（每控制周期，guidance.frequency_hz）： + ① poll 吊舱串口 → 缓存最新 表4 目标经纬度 / 表2-2 遥测（带单调时间戳） + ② 按 link.attitude_downlink_hz 节流，读 PX4 遥测 → 打包 表3 载机姿态下发吊舱 + （吊舱据此解算目标经纬度，文档要求 1~60Hz） + ③ 目标门控：表2-2 状态字节1 bit2=锁定 且遥测在 link.telemetry_timeout_ms 内， + 表4 坐标才视为有效（规格：表4 周期发送，搜索状态下坐标无意义；激光无效时 + 吊舱不解算、表4 自然中断，由 guidance.target_stale_ms 兜底） + ④ 自身状态 + 最新目标 → GuidanceInput → GuidanceLaw.update → ControlOutput + ⑤ 按模式分发：VELOCITY_HEADING → offboard 速度；BRAKE_HOVER → 零速刹车 + （引导律对一切不安全情形输出 BRAKE_HOVER，故 offboard 始终有心跳，无失联悬停） + +run() 主循环带异常护栏：单周期异常（如串口拔线）按节流节奏告警并补发零速刹车 +保 offboard 心跳，任务不终止。 + +时钟一致性：Px4Gateway 与 PodBridge 必须共用同一单调毫秒时钟（OwnState.time_ms +与 step(now_ms) 同源），否则 position/target 超龄判定失效。connect() 工厂已统一。 +""" + +import asyncio +import logging +import math +import time +from datetime import datetime, timezone +from typing import Optional + +from visual_track_control.staleness_checker import is_stale +from visual_track_control.types import ControlMode + +from .config import BridgeConfig +from .guidance import GuidanceInput, GuidanceLaw +from .pod_serial import PodLink, PodSerial +from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry, + encode_attitude, encode_control) +from .px4_gateway import Px4Gateway + +logger = logging.getLogger(__name__) + + +def _monotonic_ms() -> int: + return int(time.monotonic() * 1000) + + +class PodBridge: + """px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。""" + + def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink, + now_ms_fn=_monotonic_ms) -> None: + self._cfg = cfg + self._px4 = px4 + self._pod = pod + self._now_ms = now_ms_fn + self._guidance = GuidanceLaw(cfg.guidance) + self._attitude_period_ms = max( + 1, int(round(1000.0 / cfg.link.attitude_downlink_hz))) + self._last_attitude_ms: Optional[int] = None + self._latest_target = None # (TargetGeo, time_ms) + self._latest_telemetry = None # (Telemetry, time_ms) + self._step_failures = 0 # run() 连续异常计数（告警节流用） + + # --- 生命周期 ----------------------------------------------------------- + async def start(self) -> bool: + """启动 PX4 遥测订阅与 offboard；offboard 失败返回 False（不应继续闭环）。""" + await self._px4.start_telemetry() + if not await self._px4.start_offboard(): + logger.error("[Bridge] offboard 启动失败，桥接未运行") + return False + logger.info("[Bridge] 启动：引导 %.1fHz，姿态下发 %.1fHz", + self._cfg.guidance.frequency_hz, + self._cfg.link.attitude_downlink_hz) + return True + + async def stop(self) -> None: + """安全停机：先零速刹车，再停 offboard / 遥测 / 串口。""" + try: + await self._px4.send_brake_hover(self._now_ms()) + finally: + await self._px4.stop_offboard() + await self._px4.stop_telemetry() + self._pod.close() + self._guidance.reset() + logger.info("[Bridge] 已停止") + + # --- 主循环 ------------------------------------------------------------- + async def run(self, stop_event: asyncio.Event) -> None: + """按引导频率循环 step()，直到 stop_event 置位。 + + 异常护栏：单周期抛错（典型：吊舱串口拔线 SerialException/OSError）不终止 + 任务，按「第 1 次 + 每满 100 次」节奏告警（对齐 WarnThrottle 语义），并补发 + 零速刹车保住 offboard 心跳——否则 PX4 数秒内触发 offboard 失联保护。 + CancelledError 继承 BaseException，不在此拦截。 + """ + period_s = 1.0 / self._cfg.guidance.frequency_hz + while not stop_event.is_set(): + try: + await self.step(self._now_ms()) + self._step_failures = 0 + except Exception as exc: + self._step_failures += 1 + if self._step_failures == 1 or self._step_failures % 100 == 0: + logger.warning("[Bridge] 控制周期异常(第 %d 次): %s", + self._step_failures, exc) + try: + await self._px4.send_brake_hover(self._now_ms()) + except Exception: + pass # 补发失败不掩盖原异常，下周期重试 + await asyncio.sleep(period_s) + + async def step(self, now_ms: int): + """单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。""" + self._ingest_pod_frames(now_ms) + + own = self._px4.own_state() + self._downlink_attitude(own, now_ms) + + out = self._guidance.update(self._build_guidance_input(own, now_ms), now_ms) + await self._dispatch(out, now_ms) + return out + + # --- 吊舱侧 ------------------------------------------------------------- + def _ingest_pod_frames(self, now_ms: int) -> None: + for frame in self._pod.poll(): + if isinstance(frame, TargetGeo): + self._latest_target = (frame, now_ms) + elif isinstance(frame, Telemetry): + self._latest_telemetry = (frame, now_ms) + + def _downlink_attitude(self, own, now_ms: int) -> None: + if (self._last_attitude_ms is not None + and now_ms - self._last_attitude_ms < self._attitude_period_ms): + return + self._pod.send(encode_attitude(self._build_attitude(own))) + self._last_attitude_ms = now_ms + + def _build_attitude(self, own) -> CarrierAttitude: + t = datetime.now(timezone.utc) + return CarrierAttitude( + roll_deg=math.degrees(own.roll_rad), + pitch_deg=math.degrees(own.pitch_rad), + yaw_deg=math.degrees(own.yaw_rad), + latitude_deg=own.latitude_deg, + longitude_deg=own.longitude_deg, + abs_alt_m=own.absolute_altitude_m, + rel_alt_m=own.relative_altitude_m, + year=t.year, month=t.month, day=t.day, + hour=t.hour, minute=t.minute, second=t.second, + centisecond=t.microsecond // 10000, + airspeed_mps=0.0, # 无空速管，置 0（文档允许） + ground_speed_mps=own.ground_speed_mps, + ) + + def send_control(self, cmd: ControlCommand) -> None: + """触发式下发 表2-1 控制指令（如开目标识别 / 框选跟踪）。""" + self._pod.send(encode_control(cmd)) + + # --- 飞控侧 ------------------------------------------------------------- + def _build_guidance_input(self, own, now_ms: int) -> GuidanceInput: + tgt = self._latest_target + locked = self._pod_locked(now_ms) + return GuidanceInput( + own_lat_deg=own.latitude_deg, + own_lon_deg=own.longitude_deg, + own_yaw_deg=math.degrees(own.yaw_rad), + own_position_valid=own.position_valid, + own_position_time_ms=own.position_time_ms, # position 流专用时间戳 + target_lat_deg=tgt[0].latitude_deg if tgt else 0.0, + target_lon_deg=tgt[0].longitude_deg if tgt else 0.0, + target_valid=tgt is not None and locked, # 未锁定=目标无效 + target_time_ms=tgt[1] if tgt else 0, + ) + + def _pod_locked(self, now_ms: int) -> bool: + """吊舱锁定门控：表2-2 遥测在 link.telemetry_timeout_ms 内且状态字节1 + bit2=锁定 才算锁定。从未收到遥测 / 遥测超龄 / 搜索状态一律视为未锁定 + （宁可刹车，不朝搜索状态下的无意义坐标飞）。""" + tel = self._latest_telemetry + if tel is None: + return False + frame, t_ms = tel + if is_stale(t_ms, now_ms, self._cfg.link.telemetry_timeout_ms): + return False + return frame.is_target_locked + + async def _dispatch(self, out, now_ms: int) -> None: + if out.mode is ControlMode.VELOCITY_HEADING and out.valid: + await self._px4.send_velocity_heading(out, now_ms) + elif out.mode is ControlMode.BRAKE_HOVER and out.valid: + await self._px4.send_brake_hover(now_ms) + + # --- 诊断访问 ----------------------------------------------------------- + @property + def latest_target(self): + return self._latest_target + + @property + def latest_telemetry(self): + return self._latest_telemetry + + # --- 实机工厂 ----------------------------------------------------------- + @classmethod + async def connect(cls, cfg: BridgeConfig) -> "PodBridge": + """按配置连接 PX4(MAVSDK) 与吊舱(串口)，共用单一单调时钟。""" + px4 = await Px4Gateway.connect(cfg.px4.connection_string, + now_ms_fn=_monotonic_ms) + pod = PodSerial(cfg.pod_serial.port, cfg.pod_serial.baudrate) + return cls(cfg, px4, pod, now_ms_fn=_monotonic_ms) diff --git a/utils/pod_bridge/config.py b/utils/pod_bridge/config.py new file mode 100644 index 0000000..9c6a178 --- /dev/null +++ b/utils/pod_bridge/config.py @@ -0,0 +1,234 @@ +"""config.py —— 吊舱桥接配置（BridgeConfig + load_from_json）。 + +集中管理：吊舱串口、PX4(MAVSDK) 连接串、链路频率/超时、引导律参数 +（复用 guidance.GuidanceConfig）。风格对齐 visual_track_control/config.py： +启动期一次性解析，错误携带完整字段名（如 "guidance.max_speed_mps"）抛 ValueError， +解析结果与 JSON 脱钩，热路径不热更新。 + +与 visual_track_control/config.py 的差异：本配置多处取值仍待定（串口设备号、 +PX4 连接串等），故采用「缺字段回退 dataclass 默认值」而非全字段强制；为保证 +安全，仍对出现的字段做类型/范围校验，并**拒绝各分组内的未知键**（防拼写错误 +导致的静默回退）。 +""" + +import dataclasses +import json +from dataclasses import dataclass, field +from typing import Any + +from .guidance import GuidanceConfig + + +@dataclass +class PodSerialConfig: + """吊舱侧串口（文档：115200、无校验、8N1）。port 待定，留空为占位。""" + + port: str = "" + baudrate: int = 115200 + + +@dataclass +class Px4Config: + """飞控侧 MAVSDK 连接串。TELEM 串口形如 serial:///dev/ttyS1:57600； + SITL/仿真可用 udp://:14540。""" + + connection_string: str = "udp://:14540" + + +@dataclass +class LinkConfig: + """链路频率与超时。""" + + attitude_downlink_hz: float = 20.0 # 表3 载机姿态下发频率（文档：1~60Hz） + telemetry_timeout_ms: int = 500 # 吊舱遥测/目标超龄判定（链路级） + + +@dataclass +class BridgeConfig: + pod_serial: PodSerialConfig = field(default_factory=PodSerialConfig) + px4: Px4Config = field(default_factory=Px4Config) + link: LinkConfig = field(default_factory=LinkConfig) + guidance: GuidanceConfig = field(default_factory=GuidanceConfig) + + +# --------------------------------------------------------------------------- +# 校验原语（错误消息携带完整字段名） +# --------------------------------------------------------------------------- +def _is_number(value: Any) -> bool: + return isinstance(value, (int, float)) and not isinstance(value, bool) + + +def _is_integer(value: Any) -> bool: + return isinstance(value, int) and not isinstance(value, bool) + + +def _require_section(root: dict, section: str) -> dict: + value = root.get(section) + if value is None: + return {} # 缺省分组：全部回退默认值 + if not isinstance(value, dict): + raise ValueError(f"配置缺失分组或类型错误: {section}") + return value + + +def _reject_unknown(section: dict, allowed: set, section_name: str) -> None: + unknown = set(section.keys()) - allowed + if unknown: + names = ", ".join(f"{section_name}.{k}" for k in sorted(unknown)) + raise ValueError(f"配置存在未知字段: {names}") + + +def _get_number(section: dict, section_name: str, key: str, default: float) -> float: + if key not in section: + return float(default) + value = section[key] + if not _is_number(value): + raise ValueError(f"配置字段类型错误(需数值): {section_name}.{key}") + return float(value) + + +def _get_integer(section: dict, section_name: str, key: str, default: int) -> int: + if key not in section: + return int(default) + value = section[key] + if not _is_integer(value): + raise ValueError(f"配置字段类型错误(需整数): {section_name}.{key}") + return int(value) + + +def _get_string(section: dict, section_name: str, key: str, default: str) -> str: + if key not in section: + return str(default) + value = section[key] + if not isinstance(value, str): + raise ValueError(f"配置字段类型错误(需字符串): {section_name}.{key}") + return value + + +def _positive(value: float, field_name: str) -> float: + if not (value > 0.0): + raise ValueError(f"配置非法(必须为正): {field_name}") + return value + + +def _non_negative(value: float, field_name: str) -> float: + if value < 0.0: + raise ValueError(f"配置非法(不得为负): {field_name}") + return value + + +def _in_range(value: float, lo: float, hi: float, field_name: str, + half_open: bool = False) -> float: + """范围校验：默认闭区间 [lo, hi]；half_open=True 为左闭右开 [lo, hi)。""" + ok = (lo <= value < hi) if half_open else (lo <= value <= hi) + if not ok: + raise ValueError(f"配置非法(超出范围): {field_name}") + return value + + +# --------------------------------------------------------------------------- +# 各分组加载 +# --------------------------------------------------------------------------- +def _load_pod_serial(root: dict) -> PodSerialConfig: + s = _require_section(root, "pod_serial") + _reject_unknown(s, {"port", "baudrate"}, "pod_serial") + c = PodSerialConfig() + c.port = _get_string(s, "pod_serial", "port", c.port) + c.baudrate = int(_positive( + _get_integer(s, "pod_serial", "baudrate", c.baudrate), "pod_serial.baudrate")) + return c + + +def _load_px4(root: dict) -> Px4Config: + s = _require_section(root, "px4") + _reject_unknown(s, {"connection_string"}, "px4") + c = Px4Config() + c.connection_string = _get_string( + s, "px4", "connection_string", c.connection_string) + if not c.connection_string: + raise ValueError("配置非法(不得为空): px4.connection_string") + return c + + +def _load_link(root: dict) -> LinkConfig: + s = _require_section(root, "link") + _reject_unknown(s, {"attitude_downlink_hz", "telemetry_timeout_ms"}, "link") + c = LinkConfig() + c.attitude_downlink_hz = _in_range( + _positive(_get_number(s, "link", "attitude_downlink_hz", + c.attitude_downlink_hz), "link.attitude_downlink_hz"), + 1.0, 60.0, "link.attitude_downlink_hz") + c.telemetry_timeout_ms = int(_positive( + _get_integer(s, "link", "telemetry_timeout_ms", c.telemetry_timeout_ms), + "link.telemetry_timeout_ms")) + return c + + +def _load_guidance(root: dict) -> GuidanceConfig: + s = _require_section(root, "guidance") + allowed = {f.name for f in dataclasses.fields(GuidanceConfig)} + _reject_unknown(s, allowed, "guidance") + d = GuidanceConfig() + g = GuidanceConfig() + + g.frequency_hz = _positive( + _get_number(s, "guidance", "frequency_hz", d.frequency_hz), + "guidance.frequency_hz") + g.target_stale_ms = int(_positive( + _get_integer(s, "guidance", "target_stale_ms", d.target_stale_ms), + "guidance.target_stale_ms")) + g.position_stale_ms = int(_positive( + _get_integer(s, "guidance", "position_stale_ms", d.position_stale_ms), + "guidance.position_stale_ms")) + g.stop_radius_m = _non_negative( + _get_number(s, "guidance", "stop_radius_m", d.stop_radius_m), + "guidance.stop_radius_m") + g.max_speed_mps = _positive( + _get_number(s, "guidance", "max_speed_mps", d.max_speed_mps), + "guidance.max_speed_mps") + g.speed_kp = _non_negative( + _get_number(s, "guidance", "speed_kp", d.speed_kp), "guidance.speed_kp") + g.speed_ki = _non_negative( + _get_number(s, "guidance", "speed_ki", d.speed_ki), "guidance.speed_ki") + g.speed_kd = _non_negative( + _get_number(s, "guidance", "speed_kd", d.speed_kd), "guidance.speed_kd") + g.speed_integral_limit = _non_negative( + _get_number(s, "guidance", "speed_integral_limit", d.speed_integral_limit), + "guidance.speed_integral_limit") + g.speed_derivative_filter_coef = _in_range( + _get_number(s, "guidance", "speed_derivative_filter_coef", + d.speed_derivative_filter_coef), + 0.0, 1.0, "guidance.speed_derivative_filter_coef", half_open=True) + g.speed_slew_mps2 = _positive( + _get_number(s, "guidance", "speed_slew_mps2", d.speed_slew_mps2), + "guidance.speed_slew_mps2") + g.max_yaw_rate_dps = _positive( + _get_number(s, "guidance", "max_yaw_rate_dps", d.max_yaw_rate_dps), + "guidance.max_yaw_rate_dps") + g.yaw_kp = _non_negative( + _get_number(s, "guidance", "yaw_kp", d.yaw_kp), "guidance.yaw_kp") + g.yaw_slew_dps2 = _positive( + _get_number(s, "guidance", "yaw_slew_dps2", d.yaw_slew_dps2), + "guidance.yaw_slew_dps2") + return g + + +def load_from_json(path: str) -> BridgeConfig: + """从 JSON 文件一次性加载桥接配置（启动期调用，调参重启生效）。""" + try: + with open(path, "r", encoding="utf-8") as fp: + root = json.load(fp) + except OSError: + raise ValueError(f"配置文件打开失败: {path}") from None + except json.JSONDecodeError as exc: + raise ValueError(f"配置文件解析失败: {path} ({exc})") from None + + if not isinstance(root, dict): + raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)") + + cfg = BridgeConfig() + cfg.pod_serial = _load_pod_serial(root) + cfg.px4 = _load_px4(root) + cfg.link = _load_link(root) + cfg.guidance = _load_guidance(root) + return cfg diff --git a/utils/pod_bridge/guidance.py b/utils/pod_bridge/guidance.py new file mode 100644 index 0000000..89224b0 --- /dev/null +++ b/utils/pod_bridge/guidance.py @@ -0,0 +1,204 @@ +"""guidance.py —— 地理位置引导律：载机位置 + 目标经纬度 → offboard 速度/航向。 + +吊舱（表4）给出目标经纬度，本律计算到目标的方位角/距离，生成"朝目标飞"的 +控制意图，输出复用 visual_track_control.types.ControlOutput，可直接喂现有 +MavsdkCommandGateway（速率式分支：vx_body_mps 前向 + yaw_rate_dps 偏航）。 + +与 visual_track_control.TrackingControlLaw 的区别：后者由像素视线角驱动（视觉 +伺服），本律由绝对地理坐标驱动（航位推算）。复用其纯算法构件（PID / 限幅 / +超龄判定），故 pod_bridge 依赖 visual_track_control 作为控制原语库。 + +控制策略（速率式，避开"绝对航向为正北 0° 被网关误判为无航向指令"的歧义）： + * 方位误差 err = wrap180(目标方位 − 当前航向) → 偏航角速度（比例 + 变化率限幅） + * 距离误差 = 地面距离 − 停止半径 → 前向基准速度（PID + 限幅） + * 对准门控：前向 = 基准速度 · max(0, cos(err))，未对准时先原地转向再前进 + * 垂直：高度保持（vz = 0）；高度逼近策略待后续 config 化 + * 到达停止半径 / 目标或载机位置不可用 / 超龄 → 零速刹车（BRAKE_HOVER） +""" + +import logging +import math +from dataclasses import dataclass + +from visual_track_control.pid_controller import PidController, PidParams +from visual_track_control.rate_limiter import SlewRateLimiter, clamp +from visual_track_control.staleness_checker import is_stale +from visual_track_control.types import ControlMode, ControlOutput + +logger = logging.getLogger(__name__) + +_K_DEG_TO_RAD = math.pi / 180.0 +_K_RAD_TO_DEG = 180.0 / math.pi +_K_EARTH_RADIUS_M = 6371008.8 # WGS84 平均半径 +_K_MAX_DT_S = 0.5 # 单步 dt 上限（秒）：异常节拍保护 + + +# --- 大地测量（纯函数）----------------------------------------------------- +def haversine_distance_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + """两点地面距离（米，haversine 大圆距离）。""" + phi1 = lat1 * _K_DEG_TO_RAD + phi2 = lat2 * _K_DEG_TO_RAD + dphi = (lat2 - lat1) * _K_DEG_TO_RAD + dlmb = (lon2 - lon1) * _K_DEG_TO_RAD + a = (math.sin(dphi / 2.0) ** 2 + + math.cos(phi1) * math.cos(phi2) * math.sin(dlmb / 2.0) ** 2) + return 2.0 * _K_EARTH_RADIUS_M * math.asin(min(1.0, math.sqrt(a))) + + +def initial_bearing_deg(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + """从点 1 指向点 2 的初始方位角（度，正北为 0，顺时针 [0, 360)）。""" + phi1 = lat1 * _K_DEG_TO_RAD + phi2 = lat2 * _K_DEG_TO_RAD + dlmb = (lon2 - lon1) * _K_DEG_TO_RAD + x = math.sin(dlmb) * math.cos(phi2) + y = (math.cos(phi1) * math.sin(phi2) + - math.sin(phi1) * math.cos(phi2) * math.cos(dlmb)) + return (math.atan2(x, y) * _K_RAD_TO_DEG) % 360.0 + + +def _wrap_deg_180(deg: float) -> float: + """角度回绕到 [-180, 180)。""" + d = math.fmod(deg + 180.0, 360.0) + if d < 0.0: + d += 360.0 + return d - 180.0 + + +# --- 配置 / 输入 ------------------------------------------------------------ +@dataclass +class GuidanceConfig: + """引导律参数（全部可调，后续 config.py 从 JSON 加载并校验）。""" + + frequency_hz: float = 10.0 + target_stale_ms: int = 1000 # 目标经纬度超龄门限 + position_stale_ms: int = 1000 # 载机位置超龄门限 + stop_radius_m: float = 5.0 # 到达此半径内悬停（不再前进） + max_speed_mps: float = 5.0 # 前向速度上限 + speed_kp: float = 0.5 # 距离比例增益（mps / m） + speed_ki: float = 0.0 + speed_kd: float = 0.0 + speed_integral_limit: float = 0.0 + speed_derivative_filter_coef: float = 0.0 + speed_slew_mps2: float = 2.0 # 前向加速度限幅 + max_yaw_rate_dps: float = 30.0 # 偏航角速度上限 + yaw_kp: float = 1.0 # 方位误差比例增益（dps / deg） + yaw_slew_dps2: float = 60.0 # 偏航角速度变化率限幅 + + +@dataclass +class GuidanceInput: + """单周期引导输入：载机状态（PX4 遥测）+ 目标（吊舱表4），各带取样时间戳。""" + + own_lat_deg: float = 0.0 + own_lon_deg: float = 0.0 + own_yaw_deg: float = 0.0 # 当前航向（NED，度，正北 0 顺时针） + own_position_valid: bool = False # GPS 定位/位置是否可用 + own_position_time_ms: int = 0 + + target_lat_deg: float = 0.0 + target_lon_deg: float = 0.0 + target_valid: bool = False # 表4 目标经纬度是否有效 + target_time_ms: int = 0 + + +# --- 引导律 ----------------------------------------------------------------- +class GuidanceLaw: + """地理位置引导主控律。update() 单周期返回 ControlOutput。 + + 诊断量（last_distance_m / last_bearing_deg / last_heading_err_deg）每周期 + 更新，供桥接层日志使用；刹车/降级时为最近一次有效计算值或 NaN。 + """ + + def __init__(self, cfg: GuidanceConfig) -> None: + self._cfg = cfg + self._speed_pid = PidController(PidParams( + kp=cfg.speed_kp, ki=cfg.speed_ki, kd=cfg.speed_kd, + integral_limit=cfg.speed_integral_limit, + derivative_filter_coef=cfg.speed_derivative_filter_coef, + output_limit=cfg.max_speed_mps, + )) + self._speed_limiter = SlewRateLimiter(cfg.speed_slew_mps2) + self._yaw_limiter = SlewRateLimiter(cfg.yaw_slew_dps2) + self._last_now_ms = 0 + self._last_mode = ControlMode.BRAKE_HOVER + self.last_distance_m = float("nan") + self.last_bearing_deg = float("nan") + self.last_heading_err_deg = float("nan") + + def update(self, inp: GuidanceInput, now_ms: int) -> ControlOutput: + dt_s = self._advance_dt(now_ms) + out = ControlOutput() + + # ---- 可用性 / 超龄判定 ---- + position_ok = (inp.own_position_valid + and not is_stale(inp.own_position_time_ms, now_ms, + self._cfg.position_stale_ms)) + target_ok = (inp.target_valid + and not is_stale(inp.target_time_ms, now_ms, + self._cfg.target_stale_ms)) + if not position_ok or not target_ok: + return self._brake(out, "载机位置或目标不可用/超龄，零速刹车") + + distance = haversine_distance_m(inp.own_lat_deg, inp.own_lon_deg, + inp.target_lat_deg, inp.target_lon_deg) + bearing = initial_bearing_deg(inp.own_lat_deg, inp.own_lon_deg, + inp.target_lat_deg, inp.target_lon_deg) + heading_err = _wrap_deg_180(bearing - inp.own_yaw_deg) + self.last_distance_m = distance + self.last_bearing_deg = bearing + self.last_heading_err_deg = heading_err + + # ---- 到达停止半径 → 悬停 ---- + if distance <= self._cfg.stop_radius_m: + return self._brake(out, "已到达停止半径，悬停") + + # ---- 偏航角速度（比例 + 变化率限幅 + 上限）---- + target_rate = clamp(self._cfg.yaw_kp * heading_err, + -self._cfg.max_yaw_rate_dps, self._cfg.max_yaw_rate_dps) + out.yaw_rate_dps = self._yaw_limiter.limit(target_rate, dt_s) + + # ---- 前向速度（距离 PID + 对准门控 + 加速度限幅 + 上限）---- + base_speed = clamp(self._speed_pid.update(distance - self._cfg.stop_radius_m, + dt_s), + 0.0, self._cfg.max_speed_mps) + alignment = max(0.0, math.cos(heading_err * _K_DEG_TO_RAD)) + out.vx_body_mps = self._speed_limiter.limit(base_speed * alignment, dt_s) + + out.vz_mps = 0.0 # 高度保持 + out.mode = ControlMode.VELOCITY_HEADING + out.valid = True + return self._finish_and_log(out, "朝目标引导") + + def reset(self) -> None: + """停止闭环时清全部内部状态。""" + self._speed_pid.reset() + self._speed_limiter.reset() + self._yaw_limiter.reset() + self._last_now_ms = 0 + self._last_mode = ControlMode.BRAKE_HOVER + + def _advance_dt(self, now_ms: int) -> float: + """节拍推算：异常节拍/时钟回拨用配置频率兜底，单步上限 0.5s。""" + dt_s = 1.0 / self._cfg.frequency_hz + if self._last_now_ms > 0 and now_ms > self._last_now_ms: + dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S) + self._last_now_ms = now_ms + return dt_s + + def _brake(self, out: ControlOutput, reason: str) -> ControlOutput: + """零速刹车：进入时清积分与限幅器历史，恢复引导后指令从零渐变。""" + if self._last_mode is not ControlMode.BRAKE_HOVER: + self._speed_pid.reset() + self._speed_limiter.reset() + self._yaw_limiter.reset() + out.mode = ControlMode.BRAKE_HOVER + out.valid = True # 速度分量保持默认全零 + return self._finish_and_log(out, reason) + + def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput: + """仅模式变化时打印状态切换日志（热路径零日志）。""" + if out.mode is not self._last_mode: + logger.info("[Guidance] 状态切换: %s -> %s (%s)", + self._last_mode.name, out.mode.name, reason) + self._last_mode = out.mode + return out diff --git a/utils/pod_bridge/pod_serial.py b/utils/pod_bridge/pod_serial.py new file mode 100644 index 0000000..3026035 --- /dev/null +++ b/utils/pod_bridge/pod_serial.py @@ -0,0 +1,68 @@ +"""pod_serial.py —— 吊舱串口链路（pyserial 8N1 + FrameParser 分帧）。 + +文档：波特率 115200、无校验、8N1。本模块只做字节搬运与分帧，不理解帧语义： + * poll() —— 非阻塞读取已到达字节（timeout=0 + in_waiting），喂 FrameParser， + 返回本次解析出的帧列表（粘包/半帧由 FrameParser 处理）； + * send() —— 写入一帧已编码字节； + * close() —— 关闭串口。 + +非阻塞轮询设计便于与 asyncio 桥接主循环单线程协作（无需读线程）。PodLink 为 +duck-typed 接口，桥接层据此工作，测试可注入内存 FakePod，无需硬件。 +""" + +import logging +from typing import List, Protocol, runtime_checkable + +from .protocol import FrameParser + +logger = logging.getLogger(__name__) + + +@runtime_checkable +class PodLink(Protocol): + """吊舱链路接口（PodSerial 实机实现 / 测试 FakePod 均满足）。""" + + def poll(self) -> List[object]: + """返回本次解析出的帧列表（Telemetry / TargetGeo 等）。""" + ... + + def send(self, data: bytes) -> None: + """写入一帧已编码字节。""" + ... + + def close(self) -> None: + ... + + +class PodSerial: + """实机串口实现。pyserial 惰性导入：未装 pyserial 也能 import 本模块并用 mock。""" + + def __init__(self, port: str, baudrate: int = 115200, + parser: FrameParser | None = None) -> None: + if not port: + raise ValueError("吊舱串口未配置: pod_serial.port 为空") + import serial # 惰性导入 + self._ser = serial.Serial( + port=port, baudrate=baudrate, + bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, + stopbits=serial.STOPBITS_ONE, + timeout=0, write_timeout=1.0, + ) + self._parser = parser or FrameParser() + logger.info("[PodSerial] 打开 %s @%d 8N1", port, baudrate) + + def poll(self) -> List[object]: + waiting = self._ser.in_waiting + if waiting <= 0: + return [] + return self._parser.feed(self._ser.read(waiting)) + + def send(self, data: bytes) -> None: + self._ser.write(data) + + def close(self) -> None: + try: + self._ser.close() + logger.info("[PodSerial] 串口已关闭") + except Exception as exc: + logger.warning("[PodSerial] 关闭异常: %s", exc) diff --git a/utils/pod_bridge/protocol.py b/utils/pod_bridge/protocol.py new file mode 100644 index 0000000..abd4602 --- /dev/null +++ b/utils/pod_bridge/protocol.py @@ -0,0 +1,419 @@ +"""protocol.py —— 光电吊舱控制协议 G 版编解码（纯函数 + 数据类，无硬件依赖）。 + +规格：docs/光电吊舱控制协议G版-PB.docx。 + +四种定长帧，统一规则： + * 多字节小端（低字节在前，高字节在后）； + * 末尾校验和 = 从第 0 字节累加取低 8 位（sum(body) & 0xFF）； + * 文档样例 eb 90 2b 00..00 a6 已锁定校验算法。 + + 表2-1 控制指令 同步字 EB 90 16B 控制器 → 吊舱 触发式 + 表2-2 周期遥测 同步字 EE 16 32B 吊舱 → 控制器 60Hz + 表3 载机姿态 同步字 EB 91 32B 控制器 → 吊舱 1~60Hz + 表4 目标经纬度 同步字 EE 18 32B 吊舱 → 控制器 1~60Hz + +控制器（本桥接）发送 表2-1/表3、接收 表2-2/表4；为便于回环测试与后续吊舱 +模拟器，四种帧均提供对称的 encode/decode。 +""" + +from dataclasses import dataclass, field +from enum import IntEnum +import struct + +# --- 同步字与帧长 ----------------------------------------------------------- +SYNC_CONTROL = (0xEB, 0x90) # 表2-1 +SYNC_TELEMETRY = (0xEE, 0x16) # 表2-2 +SYNC_ATTITUDE = (0xEB, 0x91) # 表3 +SYNC_TARGET = (0xEE, 0x18) # 表4 + +CONTROL_LEN = 16 +TELEMETRY_LEN = 32 +ATTITUDE_LEN = 32 +TARGET_LEN = 32 + +# --- 量纲（每 LSB 物理量）--------------------------------------------------- +ANGLE_SCALE = 0.01 # 角度 / 偏移角，° +RATE_SCALE = 0.01 # 角速度，°/s +RANGE_SCALE = 0.1 # 激光测距，m +ALT_SCALE = 0.2 # 高度，m +LATLON_SCALE = 1e-7 # 经纬度，° +SPEED_SCALE = 0.5 # 空速 / 地速，m/s + +_S16_MIN, _S16_MAX = -32768, 32767 +_U16_MAX = 65535 +_S32_MIN, _S32_MAX = -(2 ** 31), 2 ** 31 - 1 + + +def checksum(body: bytes) -> int: + """从第 0 字节累加取低 8 位（body 不含校验和自身）。""" + return sum(body) & 0xFF + + +def _to_s16(value: float, scale: float) -> int: + return max(_S16_MIN, min(_S16_MAX, int(round(value / scale)))) + + +def _to_u16(value: float, scale: float) -> int: + return max(0, min(_U16_MAX, int(round(value / scale)))) + + +def _to_s32(value: float, scale: float) -> int: + return max(_S32_MIN, min(_S32_MAX, int(round(value / scale)))) + + +class ControlCode(IntEnum): + """表2-1 字节 2「控制信息」取值。""" + + NONE = 0x00 # 空指令 + VISIBLE_1 = 0x01 # 可见光1 + VISIBLE_2 = 0x02 # 可见光2 + IR_1 = 0x03 # 红外1 + IR_2 = 0x04 # 红外2 + TARGET_DETECT_ON = 0x07 # 目标识别开（参数X：0=人/车，1=无人机） + TARGET_DETECT_OFF = 0x08 # 目标识别关（默认） + VIDEO_RECORD_ON = 0x09 # 视频录制开 + VIDEO_RECORD_OFF = 0x0A # 视频录制关（默认） + BOX_TRACK = 0x0C # 框选跟踪（左上 X/Y + 右下 X2/Y2） + POINT_TRACK = 0x0D # 指点跟踪（X/Y） + STOP_TRACK = 0x0E # 停止跟踪 + AUTO_LOCK_ON_FOUND = 0x0F # 发现目标自动锁定 + DETECT_TO_TRACK = 0x10 # 识别转跟踪 + IR_WHITE_HOT = 0x11 # 红外白热 + IR_BLACK_HOT = 0x12 # 红外黑热 + ANTI_OCCLUSION_OFF = 0x17 # 关抗遮挡 + ANTI_OCCLUSION_ON = 0x18 # 开抗遮挡 + GIMBAL_SEARCH = 0x24 # 云台搜索（X=方位转速0.1°/s，Y=俯仰转速0.1°/s） + OPTICAL_ZOOM = 0x25 # 光学变焦（X=倍数0.1倍） + SET_FRAME_ANGLE = 0x26 # 指定框架角度（X=方位0.01°，Y=俯仰0.01°） + MOTOR_ON = 0x27 # 开电机 + MOTOR_OFF = 0x28 # 关电机 + FOLLOW_OFF = 0x29 # 关跟随 + FOLLOW_ON = 0x2A # 跟随 + CENTER = 0x2B # 归中 + SUPPRESS_GYRO_DRIFT = 0x2C # 抑制陀螺仪漂移 + ELECTRIC_LOCK = 0x30 # 电锁模式 + ELECTRIC_UNLOCK = 0x31 # 解除电锁 + PIP_ON = 0x40 # 开画中画 + PIP_OFF = 0x41 # 关画中画 + OSD_ON = 0x51 # OSD 显示开 + OSD_OFF = 0x52 # OSD 显示关 + SET_ZOOM = 0x5A # 指定变焦倍数（X=倍数0.1倍） + TAKE_PHOTO = 0x5B # 拍照指令 + EIS_ON = 0x5C # 开电子稳像 + EIS_OFF = 0x5D # 关电子稳像 + SERVO_STAB_ON = 0x60 # 开伺服稳像 + SERVO_STAB_OFF = 0x61 # 关伺服稳像 + VERSION_QUERY = 0x62 # 版本查询 + SET_BITRATE = 0x64 # 设置码率（X=kbps） + + +# --- 表2-1 控制指令 --------------------------------------------------------- +@dataclass +class ControlCommand: + """控制器 → 吊舱 控制帧。param* 语义随 code 变化，见 ControlCode 注释。""" + + code: int = ControlCode.NONE + param_x: int = 0 # S16 + param_y: int = 0 # S16 + param3: int = 0 # U8（画中画小画面等） + file_save_mode: int = 0 # S8（0=时间戳，1=前缀+序号） + param_x2: int = 0 # S16（框选跟踪右下角横向） + param_y2: int = 0 # S16（框选跟踪右下角纵向） + + +def encode_control(cmd: ControlCommand) -> bytes: + body = struct.pack( + " ControlCommand: + _check(data, CONTROL_LEN, SYNC_CONTROL) + code, x, y, param3, save, x2, y2 = struct.unpack(" int: + """Bit7-6 跟踪视频源：0可见1 1可见2 2红外1 3红外2。""" + return (self.status1 >> 6) & 0x3 + + @property + def is_target_locked(self) -> bool: + """Bit2 目标跟踪状态：True=锁定，False=搜索。""" + return bool((self.status1 >> 2) & 0x1) + + @property + def video_storing(self) -> bool: + return bool((self.status2 >> 5) & 0x1) + + @property + def motor_on(self) -> bool: + return bool((self.status2 >> 3) & 0x1) + + @property + def follow_on(self) -> bool: + return bool((self.status2 >> 2) & 0x1) + + @property + def electric_lock_on(self) -> bool: + return bool((self.status2 >> 1) & 0x1) + + @property + def big_picture(self) -> int: + return (self.status3 >> 6) & 0x3 + + @property + def small_picture(self) -> int: + return (self.status3 >> 4) & 0x3 + + +def encode_telemetry(t: Telemetry) -> bytes: + laser = 0 if t.laser_range_m is None else _to_u16(t.laser_range_m, RANGE_SCALE) + body = struct.pack( + " Telemetry: + _check(data, TELEMETRY_LEN, SYNC_TELEMETRY) + (s1, s2, _rsv, s3, ox, oy, froll, fpitch, fyaw, aroll, apitch, + rrate, prate, yrate, laser, selfcheck, _rsv2) = struct.unpack( + " bytes: + body = struct.pack( + " CarrierAttitude: + _check(data, ATTITUDE_LEN, SYNC_ATTITUDE) + (roll, pitch, yaw, lat, lon, absalt, relalt, yr, mo, day, hour, mi, sec, + cs, air, gs) = struct.unpack(" bytes: + body = struct.pack( + " TargetGeo: + _check(data, TARGET_LEN, SYNC_TARGET) + (lat, lon, absalt, relalt, yr, mo, day, hour, mi, sec, cs, + _rsv) = struct.unpack(" None: + if len(data) != length: + raise ValueError(f"帧长错误: 期望 {length} 字节, 实际 {len(data)}") + if data[0] != sync[0] or data[1] != sync[1]: + raise ValueError( + f"同步字错误: 期望 {sync[0]:02X} {sync[1]:02X}, " + f"实际 {data[0]:02X} {data[1]:02X}") + if checksum(data[:length - 1]) != data[length - 1]: + raise ValueError("校验和错误") + + +# --- 字节流分帧器 ----------------------------------------------------------- +_FRAME_SPECS = { + SYNC_CONTROL: (CONTROL_LEN, decode_control), + SYNC_TELEMETRY: (TELEMETRY_LEN, decode_telemetry), + SYNC_ATTITUDE: (ATTITUDE_LEN, decode_attitude), + SYNC_TARGET: (TARGET_LEN, decode_target), +} + + +class FrameParser: + """串口字节流分帧：按同步字滑窗取定长帧，校验通过则解码输出。 + + 默认只接收吊舱 → 控制器的两种帧（遥测 EE16 / 目标 EE18）；可通过 kinds + 指定要识别的同步字集合（如吊舱模拟器侧识别 EB90/EB91）。粘包、半帧、坏校验 + 均安全处理：坏校验丢弃该同步字首字节后重扫，半帧留待下次 feed。 + """ + + def __init__(self, kinds: tuple = (SYNC_TELEMETRY, SYNC_TARGET)) -> None: + for k in kinds: + if k not in _FRAME_SPECS: + raise ValueError(f"未知帧类型: {k}") + self._specs = {k: _FRAME_SPECS[k] for k in kinds} + self._buf = bytearray() + + def feed(self, data: bytes): + """喂入任意长度字节，返回本次可解析出的帧列表（按到达顺序）。""" + self._buf.extend(data) + frames = [] + while True: + idx, sync = self._find_sync() + if idx is None: + # 无同步字：仅保留最后 1 字节（可能是下一帧同步字首字节） + if len(self._buf) > 1: + del self._buf[:-1] + break + if idx > 0: + del self._buf[:idx] + length, decoder = self._specs[sync] + if len(self._buf) < length: + break # 半帧，等待更多数据 + frame = bytes(self._buf[:length]) + if checksum(frame[:length - 1]) == frame[length - 1]: + frames.append(decoder(frame)) + del self._buf[:length] + else: + del self._buf[:1] # 坏校验：跳过该同步字首字节重扫 + return frames + + def _find_sync(self): + earliest_idx, earliest_sync = None, None + for sync in self._specs: + i = self._buf.find(bytes(sync)) + if i >= 0 and (earliest_idx is None or i < earliest_idx): + earliest_idx, earliest_sync = i, sync + return earliest_idx, earliest_sync diff --git a/utils/pod_bridge/px4_gateway.py b/utils/pod_bridge/px4_gateway.py new file mode 100644 index 0000000..8e20726 --- /dev/null +++ b/utils/pod_bridge/px4_gateway.py @@ -0,0 +1,169 @@ +"""px4_gateway.py —— PX4(MAVSDK) 侧网关：读飞控遥测 + 发 offboard 速度指令。 + +职责（桥接的飞控侧）： + * 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_body），缓存为 + OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)； + * 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点 + （不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。 + +设计：遥测拆为「纯缓存更新方法 update_*（无 IO，可单测）」+「薄异步订阅胶水 +start_telemetry/_consume」，便于无硬件 mock 测试。指令发送整体委托给被组合的 +MavsdkCommandGateway。 +""" + +import asyncio +import logging +import math +import time +from dataclasses import dataclass, replace +from typing import Any, Callable, Optional + +from visual_track_control.mavsdk_gateway import MavsdkCommandGateway + +logger = logging.getLogger(__name__) + +_GPS_FIX_3D = 3 # mavsdk GpsFixType.THREE_D_FIX 起视为定位有效 + + +def _enum_int(value: Any) -> int: + """MAVSDK 枚举（Enum/IntEnum）或裸整数统一转 int。""" + if hasattr(value, "value"): + try: + return int(value.value) + except (TypeError, ValueError): + return 0 + try: + return int(value) + except (TypeError, ValueError): + return 0 + + +@dataclass +class OwnState: + """PX4 遥测缓存快照（角度弧度 NED；位置度；高度/速度米制）。""" + + latitude_deg: float = 0.0 + longitude_deg: float = 0.0 + absolute_altitude_m: float = 0.0 + relative_altitude_m: float = 0.0 + roll_rad: float = 0.0 + pitch_rad: float = 0.0 + yaw_rad: float = 0.0 + ground_speed_mps: float = 0.0 + fix_type: int = 0 + num_satellites: int = 0 + has_position: bool = False + has_attitude: bool = False + position_valid: bool = False # has_position 且 GPS 3D 定位 + time_ms: int = 0 # 任意遥测流最近一次更新（毫秒），仅供诊断 + position_time_ms: int = 0 # position 流最近一次更新（毫秒），位置超龄判定专用 + + +class Px4Gateway: + """drone：mavsdk System（duck-typed，便于注入 mock）。""" + + def __init__(self, drone: Any, + now_ms_fn: Optional[Callable[[], int]] = None) -> None: + self._drone = drone + self._cmd = MavsdkCommandGateway(drone) + self._now_ms = now_ms_fn or (lambda: int(time.monotonic() * 1000)) + self._state = OwnState() + self._tasks: list = [] + + # --- 连接（实机）------------------------------------------------------- + @classmethod + async def connect(cls, connection_string: str, + now_ms_fn: Optional[Callable[[], int]] = None) -> "Px4Gateway": + """创建 MAVSDK System、连接并等待联通后返回网关。""" + from mavsdk import System + drone = System() + logger.info("[Px4Gateway] 连接飞控 %s ...", connection_string) + await drone.connect(system_address=connection_string) + async for state in drone.core.connection_state(): + if state.is_connected: + logger.info("[Px4Gateway] 飞控已连接") + break + return cls(drone, now_ms_fn=now_ms_fn) + + # --- 遥测缓存更新（纯方法，可单测）------------------------------------ + def update_position(self, p: Any) -> None: + self._state.latitude_deg = float(p.latitude_deg) + self._state.longitude_deg = float(p.longitude_deg) + self._state.absolute_altitude_m = float(p.absolute_altitude_m) + self._state.relative_altitude_m = float(p.relative_altitude_m) + self._state.has_position = True + self._recompute_valid() + now = self._now_ms() + self._state.time_ms = now + self._state.position_time_ms = now # 仅此流刷新，姿态高频流不得代刷 + + def update_attitude(self, e: Any) -> None: + self._state.roll_rad = float(e.roll_rad) + self._state.pitch_rad = float(e.pitch_rad) + self._state.yaw_rad = float(e.yaw_rad) + self._state.has_attitude = True + self._state.time_ms = self._now_ms() + + def update_gps(self, g: Any) -> None: + self._state.fix_type = _enum_int(getattr(g, "fix_type", 0)) + self._state.num_satellites = _enum_int(getattr(g, "num_satellites", 0)) + self._recompute_valid() + self._state.time_ms = self._now_ms() + + def update_velocity_body(self, v: Any) -> None: + self._state.ground_speed_mps = math.hypot( + float(v.forward_m_s), float(v.right_m_s)) + self._state.time_ms = self._now_ms() + + def own_state(self) -> OwnState: + """返回当前缓存的快照副本（调用方修改不影响内部状态）。""" + return replace(self._state) + + def _recompute_valid(self) -> None: + self._state.position_valid = (self._state.has_position + and self._state.fix_type >= _GPS_FIX_3D) + + # --- 遥测订阅（异步胶水）---------------------------------------------- + async def start_telemetry(self) -> None: + """订阅四路遥测流，后台任务持续刷新缓存。""" + t = self._drone.telemetry + self._tasks = [ + asyncio.ensure_future(self._consume(t.position(), self.update_position)), + asyncio.ensure_future( + self._consume(t.attitude_euler(), self.update_attitude)), + asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)), + asyncio.ensure_future( + self._consume(t.velocity_body(), self.update_velocity_body)), + ] + logger.info("[Px4Gateway] 遥测订阅已启动") + + async def _consume(self, stream: Any, handler: Callable[[Any], None]) -> None: + try: + async for msg in stream: + handler(msg) + except asyncio.CancelledError: + raise + except Exception as exc: + logger.warning("[Px4Gateway] 遥测流中断: %s", exc) + + async def stop_telemetry(self) -> None: + for task in self._tasks: + task.cancel() + if self._tasks: + await asyncio.gather(*self._tasks, return_exceptions=True) + self._tasks = [] + logger.info("[Px4Gateway] 遥测订阅已停止") + + # --- offboard 指令（委托 MavsdkCommandGateway）------------------------- + async def start_offboard(self) -> bool: + return await self._cmd.start() + + async def send_velocity_heading(self, output: Any, + time_boot_ms: Optional[int] = None) -> bool: + return await self._cmd.send_velocity_heading(output, time_boot_ms) + + async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool: + return await self._cmd.send_brake_hover(time_boot_ms) + + async def stop_offboard(self) -> None: + await self._cmd.stop() diff --git a/utils/tests/pod_bridge/bridge_test.py b/utils/tests/pod_bridge/bridge_test.py new file mode 100644 index 0000000..3b4049c --- /dev/null +++ b/utils/tests/pod_bridge/bridge_test.py @@ -0,0 +1,358 @@ +"""bridge_test.py —— PodBridge 闭环主循环单元测试（FakePx4 + FakePod，无硬件）。 + +覆盖：单周期 step（有/无目标、位置无效、位置时间戳超龄）、姿态下发节流与帧内容、 +吊舱帧入站缓存、锁定门控（表2-2 bit2 + 遥测超龄）、触发式控制指令编码、 +start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢复。 +""" + +import asyncio +import time +from dataclasses import replace + +from pod_bridge.bridge import PodBridge +from pod_bridge.config import BridgeConfig +from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo, + Telemetry, decode_attitude, decode_control) +from pod_bridge.px4_gateway import OwnState + + +class FakePx4: + def __init__(self, state=None): + self._state = state or OwnState() + self.telemetry_started = False + self.telemetry_stopped = False + self.offboard_started = False + self.offboard_stopped = False + self.offboard_start_ok = True + self.velocity_calls = [] + self.brake_calls = [] + + def own_state(self): + return replace(self._state) + + async def start_telemetry(self): + self.telemetry_started = True + + async def stop_telemetry(self): + self.telemetry_stopped = True + + async def start_offboard(self): + self.offboard_started = True + return self.offboard_start_ok + + async def stop_offboard(self): + self.offboard_stopped = True + + async def send_velocity_heading(self, out, time_boot_ms=None): + self.velocity_calls.append((out, time_boot_ms)) + return True + + async def send_brake_hover(self, time_boot_ms=None): + self.brake_calls.append(time_boot_ms) + return True + + +class FakePod: + def __init__(self): + self.inbox = [] + self.sent = [] + self.closed = False + + def queue(self, *frames): + self.inbox.extend(frames) + + def poll(self): + out, self.inbox = self.inbox, [] + return out + + def send(self, data): + self.sent.append(data) + + def close(self): + self.closed = True + + +def _valid_state(now_ms, lat=0.0, lon=0.0, yaw_rad=0.0, gs=0.0): + return OwnState(latitude_deg=lat, longitude_deg=lon, yaw_rad=yaw_rad, + ground_speed_mps=gs, fix_type=3, num_satellites=12, + has_position=True, has_attitude=True, position_valid=True, + time_ms=now_ms, position_time_ms=now_ms) + + +def _locked_telemetry(): + """状态字节1 bit2=1(锁定)的遥测帧。""" + return Telemetry(status1=0b00000100) + + +def _bridge(px4, pod, cfg=None): + return PodBridge(cfg or BridgeConfig(), px4, pod, now_ms_fn=lambda: 0) + + +# --- step：有目标 → 速度指令 ------------------------------------------------ +def test_step_with_target_commands_velocity(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(TargetGeo(latitude_deg=0.001, longitude_deg=0.0), # 正北 ~111m + _locked_telemetry()) + b = _bridge(px4, pod) + + out = asyncio.run(b.step(1000)) + assert out.mode.name == "VELOCITY_HEADING" + assert len(px4.velocity_calls) == 1 + assert px4.brake_calls == [] + + +def test_step_without_target_brakes(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod) + + out = asyncio.run(b.step(1000)) + assert out.mode.name == "BRAKE_HOVER" + assert len(px4.brake_calls) == 1 + assert px4.velocity_calls == [] + + +def test_step_invalid_position_brakes_even_with_target(): + state = replace(_valid_state(1000), position_valid=False, fix_type=0) + px4 = FakePx4(state) + pod = FakePod() + pod.queue(TargetGeo(latitude_deg=0.001)) + b = _bridge(px4, pod) + + out = asyncio.run(b.step(1000)) + assert out.mode.name == "BRAKE_HOVER" + + +# --- 姿态下发：首拍即发 + 节流 + 帧内容 ------------------------------------ +def test_attitude_downlinked_on_first_step_with_own_state(): + px4 = FakePx4(_valid_state(1000, lat=31.5, lon=121.5, yaw_rad=1.57, gs=4.0)) + pod = FakePod() + b = _bridge(px4, pod) + + asyncio.run(b.step(1000)) + assert len(pod.sent) == 1 + att = decode_attitude(pod.sent[0]) + assert abs(att.latitude_deg - 31.5) < 1e-6 + assert abs(att.longitude_deg - 121.5) < 1e-6 + assert abs(att.yaw_deg - 89.95) < 0.02 # 1.57 rad ≈ 89.95° + assert att.ground_speed_mps == 4.0 + assert att.year >= 2026 and 1 <= att.month <= 12 + + +def test_attitude_downlink_throttled(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + cfg = BridgeConfig() + cfg.link.attitude_downlink_hz = 20.0 # 周期 50ms + b = _bridge(px4, pod, cfg) + + async def scenario(): + await b.step(1000) # 发（首拍） + await b.step(1010) # 10ms < 50ms，跳过 + await b.step(1040) # 40ms < 50ms，跳过 + await b.step(1060) # 60ms ≥ 50ms，发 + asyncio.run(scenario()) + assert len(pod.sent) == 2 + + +# --- 吊舱帧入站缓存 -------------------------------------------------------- +def test_target_frame_updates_latest_and_flips_to_velocity(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod) + + asyncio.run(b.step(1000)) # 无目标 → 刹车 + assert b.latest_target is None + pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry()) + asyncio.run(b.step(1100)) # 收到目标且锁定 → 速度 + assert b.latest_target is not None + assert len(px4.velocity_calls) == 1 + + +def test_telemetry_frame_cached(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(Telemetry(laser_range_m=88.8)) + b = _bridge(px4, pod) + asyncio.run(b.step(1000)) + assert b.latest_telemetry is not None + assert b.latest_telemetry[0].laser_range_m == 88.8 + + +# --- 触发式控制指令 -------------------------------------------------------- +def test_send_control_encodes_frame(): + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod) + b.send_control(ControlCommand(code=ControlCode.CENTER)) + assert len(pod.sent) == 1 + assert decode_control(pod.sent[0]).code == ControlCode.CENTER + + +# --- 生命周期 -------------------------------------------------------------- +def test_start_wires_telemetry_and_offboard(): + px4 = FakePx4(_valid_state(0)) + b = _bridge(px4, FakePod()) + assert asyncio.run(b.start()) is True + assert px4.telemetry_started and px4.offboard_started + + +def test_start_returns_false_when_offboard_fails(): + px4 = FakePx4(_valid_state(0)) + px4.offboard_start_ok = False + b = _bridge(px4, FakePod()) + assert asyncio.run(b.start()) is False + + +def test_stop_brakes_and_tears_down(): + px4 = FakePx4(_valid_state(0)) + pod = FakePod() + b = _bridge(px4, pod) + + async def scenario(): + await b.start() + await b.stop() + asyncio.run(scenario()) + assert len(px4.brake_calls) >= 1 # 停机前刹车 + assert px4.offboard_stopped and px4.telemetry_stopped + assert pod.closed + + + +# --- 缺陷①回归:position 时间戳超龄 ------------------------------------------- +def test_step_brakes_when_position_timestamp_stale(): + """time_ms 被姿态流刷得很新,但 position_time_ms 超龄 → 必须刹车。""" + state = replace(_valid_state(1000), time_ms=100000, position_time_ms=1000) + px4 = FakePx4(state) + pod = FakePod() + pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry()) + b = _bridge(px4, pod) + + out = asyncio.run(b.step(100000)) # position 已 99s 未更新 + assert out.mode.name == "BRAKE_HOVER" + assert px4.velocity_calls == [] + + +# --- 缺陷②:锁定门控 ----------------------------------------------------------- +def test_target_without_lock_bit_brakes(): + """吊舱在搜索状态(bit2=0),即使收到表4 坐标也必须刹车。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(TargetGeo(latitude_deg=0.001), Telemetry(status1=0)) # 未锁定 + b = _bridge(px4, pod) + + out = asyncio.run(b.step(1000)) + assert out.mode.name == "BRAKE_HOVER" + assert px4.velocity_calls == [] + + +def test_target_without_any_telemetry_brakes(): + """只收到表4、从未收到表2-2 → 锁定状态未知,按未锁定处理。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(TargetGeo(latitude_deg=0.001)) + b = _bridge(px4, pod) + + out = asyncio.run(b.step(1000)) + assert out.mode.name == "BRAKE_HOVER" + + +def test_target_with_stale_telemetry_brakes(): + """锁定遥测超龄(> link.telemetry_timeout_ms)→ 目标视为无效。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + cfg = BridgeConfig() + cfg.link.telemetry_timeout_ms = 500 + b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0) + + async def scenario(): + pod.queue(_locked_telemetry(), TargetGeo(latitude_deg=0.001)) + await b.step(1000) # 遥测新鲜,但此刻若无目标则刹车 + pod.queue(TargetGeo(latitude_deg=0.001)) + out1 = await b.step(1200) # 遥测 200ms 前,仍新鲜 → 速度 + out2 = await b.step(1600) # 遥测 600ms 前,超龄 → 刹车 + return out1, out2 + + out1, out2 = asyncio.run(scenario()) + assert out1.mode.name == "VELOCITY_HEADING" + assert out2.mode.name == "BRAKE_HOVER" + + +def test_lock_lost_midflight_falls_back_to_brake(): + """先锁定引导,随后遥测变为搜索状态 → 立即回落刹车。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + b = _bridge(px4, pod) + + async def scenario(): + pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry()) + out1 = await b.step(1000) + pod.queue(Telemetry(status1=0)) # 丢失锁定 + out2 = await b.step(1100) + return out1, out2 + + out1, out2 = asyncio.run(scenario()) + assert out1.mode.name == "VELOCITY_HEADING" + assert out2.mode.name == "BRAKE_HOVER" + + +# --- 缺陷③:run() 异常护栏 ----------------------------------------------------- +class FlakyPod(FakePod): + """前 fail_times 次 poll 抛串口异常,之后恢复正常。""" + + def __init__(self, fail_times): + super().__init__() + self.fail_times = fail_times + + def poll(self): + if self.fail_times > 0: + self.fail_times -= 1 + raise OSError("serial device disconnected") + return super().poll() + + +def _run_bridge(px4, pod, hz=200.0): + cfg = BridgeConfig() + cfg.guidance.frequency_hz = hz # 加快节拍,缩短测试时间 + return PodBridge(cfg, px4, pod) # 用真实单调时钟 + + +def test_run_survives_poll_exceptions_and_keeps_heartbeat(): + """串口持续异常:任务不死、每周期补发刹车保心跳、stop_event 正常退出。""" + now = int(time.monotonic() * 1000) + px4 = FakePx4(_valid_state(now)) + pod = FlakyPod(fail_times=10**9) # 永远失败 + b = _run_bridge(px4, pod) + + async def scenario(): + stop = asyncio.Event() + task = asyncio.ensure_future(b.run(stop)) + await asyncio.sleep(0.2) + stop.set() + await asyncio.wait_for(task, timeout=2.0) + + asyncio.run(scenario()) + assert len(px4.brake_calls) > 0 # 异常期间持续补发刹车 + assert pod.fail_times > 0 # 循环确实在反复尝试(未提前退出) + + +def test_run_recovers_after_transient_failure(): + """串口短暂异常后恢复:异常期刹车,恢复后照常引导。""" + now = int(time.monotonic() * 1000) + px4 = FakePx4(_valid_state(now)) + pod = FlakyPod(fail_times=3) + pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry()) + b = _run_bridge(px4, pod) + + async def scenario(): + stop = asyncio.Event() + task = asyncio.ensure_future(b.run(stop)) + await asyncio.sleep(0.3) + stop.set() + await asyncio.wait_for(task, timeout=2.0) + + asyncio.run(scenario()) + assert len(px4.brake_calls) >= 3 # 3 次异常各补一发刹车 + assert len(px4.velocity_calls) >= 1 # 恢复后进入正常引导 + assert b._step_failures == 0 # 计数已复位 diff --git a/utils/tests/pod_bridge/config_test.py b/utils/tests/pod_bridge/config_test.py new file mode 100644 index 0000000..13c2e25 --- /dev/null +++ b/utils/tests/pod_bridge/config_test.py @@ -0,0 +1,115 @@ +"""config_test.py —— 桥接配置加载与校验单元测试。""" + +import json +from pathlib import Path + +import pytest + +from pod_bridge.config import (BridgeConfig, GuidanceConfig, PodSerialConfig, + load_from_json) + +_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json" + + +def _write(tmp_path, obj) -> str: + p = tmp_path / "cfg.json" + p.write_text(json.dumps(obj), encoding="utf-8") + return str(p) + + +def test_shipped_default_config_loads(): + cfg = load_from_json(str(_DEFAULT_CONFIG)) + assert cfg.pod_serial.baudrate == 115200 + assert cfg.px4.connection_string == "udp://:14540" + assert cfg.link.attitude_downlink_hz == pytest.approx(20.0) + assert cfg.guidance.max_speed_mps == pytest.approx(5.0) + assert cfg.guidance.stop_radius_m == pytest.approx(5.0) + + +def test_empty_json_uses_all_defaults(tmp_path): + cfg = load_from_json(_write(tmp_path, {})) + default = BridgeConfig() + assert cfg.pod_serial == default.pod_serial + assert cfg.px4 == default.px4 + assert cfg.link == default.link + assert cfg.guidance == default.guidance + + +def test_partial_override_keeps_other_defaults(tmp_path): + cfg = load_from_json(_write(tmp_path, { + "pod_serial": {"port": "COM3"}, + "guidance": {"max_speed_mps": 8.0}, + })) + assert cfg.pod_serial.port == "COM3" + assert cfg.pod_serial.baudrate == 115200 # 默认回退 + assert cfg.guidance.max_speed_mps == 8.0 + assert cfg.guidance.stop_radius_m == GuidanceConfig().stop_radius_m # 默认回退 + + +def test_full_override_roundtrip(tmp_path): + obj = { + "pod_serial": {"port": "/dev/ttyUSB0", "baudrate": 115200}, + "px4": {"connection_string": "serial:///dev/ttyS1:57600"}, + "link": {"attitude_downlink_hz": 30.0, "telemetry_timeout_ms": 800}, + "guidance": {"frequency_hz": 20.0, "stop_radius_m": 3.0, + "max_speed_mps": 7.5, "speed_kp": 0.8, "yaw_kp": 1.5}, + } + cfg = load_from_json(_write(tmp_path, obj)) + assert cfg.px4.connection_string == "serial:///dev/ttyS1:57600" + assert cfg.link.attitude_downlink_hz == 30.0 + assert cfg.link.telemetry_timeout_ms == 800 + assert cfg.guidance.frequency_hz == 20.0 + assert cfg.guidance.yaw_kp == 1.5 + + +# --- 校验错误 --------------------------------------------------------------- +def test_missing_file_raises(): + with pytest.raises(ValueError, match="打开失败"): + load_from_json(str(_DEFAULT_CONFIG.parent / "no_such_file.json")) + + +def test_non_object_root_raises(tmp_path): + p = tmp_path / "bad.json" + p.write_text("[1, 2, 3]", encoding="utf-8") + with pytest.raises(ValueError, match="根节点必须为对象"): + load_from_json(str(p)) + + +def test_malformed_json_raises(tmp_path): + p = tmp_path / "bad.json" + p.write_text("{not json", encoding="utf-8") + with pytest.raises(ValueError, match="解析失败"): + load_from_json(str(p)) + + +def test_wrong_type_raises_with_field_name(tmp_path): + with pytest.raises(ValueError, match="guidance.max_speed_mps"): + load_from_json(_write(tmp_path, {"guidance": {"max_speed_mps": "fast"}})) + + +def test_unknown_key_rejected(tmp_path): + with pytest.raises(ValueError, match="未知字段"): + load_from_json(_write(tmp_path, {"guidance": {"max_speed": 5.0}})) + + +def test_empty_connection_string_raises(tmp_path): + with pytest.raises(ValueError, match="px4.connection_string"): + load_from_json(_write(tmp_path, {"px4": {"connection_string": ""}})) + + +@pytest.mark.parametrize("obj,field", [ + ({"link": {"attitude_downlink_hz": 100.0}}, "link.attitude_downlink_hz"), # >60 + ({"link": {"attitude_downlink_hz": 0.5}}, "link.attitude_downlink_hz"), # <1 + ({"guidance": {"max_speed_mps": -1.0}}, "guidance.max_speed_mps"), # 非正 + ({"guidance": {"stop_radius_m": -2.0}}, "guidance.stop_radius_m"), # 负 + ({"guidance": {"speed_derivative_filter_coef": 1.0}}, # 上界开 + "guidance.speed_derivative_filter_coef"), + ({"pod_serial": {"baudrate": 0}}, "pod_serial.baudrate"), # 非正 +]) +def test_out_of_range_raises(tmp_path, obj, field): + with pytest.raises(ValueError, match=field.replace(".", r"\.")): + load_from_json(_write(tmp_path, obj)) + + +def test_pod_serial_default_port_is_placeholder(): + assert PodSerialConfig().port == "" diff --git a/utils/tests/pod_bridge/guidance_test.py b/utils/tests/pod_bridge/guidance_test.py new file mode 100644 index 0000000..ae276cc --- /dev/null +++ b/utils/tests/pod_bridge/guidance_test.py @@ -0,0 +1,152 @@ +"""guidance_test.py —— 地理位置引导律单元测试。 + +覆盖：大地测量（方位/距离）、对准/未对准/背向目标的速率式输出、到达悬停、 +目标/位置超龄与不可用刹车、速度上限与加速度限幅渐变。 +""" + +import math + +import pytest + +from pod_bridge.guidance import (GuidanceConfig, GuidanceInput, GuidanceLaw, + haversine_distance_m, initial_bearing_deg) +from visual_track_control.types import ControlMode + +_DEG_LAT_M = 111194.93 # 1° 纬度 ≈ 此米数（R=6371008.8） + + +def _inp(own_lat=0.0, own_lon=0.0, own_yaw=0.0, tgt_lat=0.0, tgt_lon=0.0, + pos_valid=True, tgt_valid=True, pos_ms=1000, tgt_ms=1000): + return GuidanceInput( + own_lat_deg=own_lat, own_lon_deg=own_lon, own_yaw_deg=own_yaw, + own_position_valid=pos_valid, own_position_time_ms=pos_ms, + target_lat_deg=tgt_lat, target_lon_deg=tgt_lon, + target_valid=tgt_valid, target_time_ms=tgt_ms, + ) + + +# --- 大地测量 --------------------------------------------------------------- +@pytest.mark.parametrize("tgt,expected", [ + ((0.001, 0.0), 0.0), # 正北 + ((0.0, 0.001), 90.0), # 正东（赤道） + ((-0.001, 0.0), 180.0), # 正南 + ((0.0, -0.001), 270.0), # 正西 +]) +def test_initial_bearing_cardinals(tgt, expected): + b = initial_bearing_deg(0.0, 0.0, tgt[0], tgt[1]) + # 0 与 360 等价，用回绕差判定 + diff = abs((b - expected + 180.0) % 360.0 - 180.0) + assert diff < 0.5 + + +def test_haversine_distance_one_millidegree(): + d = haversine_distance_m(0.0, 0.0, 0.001, 0.0) + assert d == pytest.approx(_DEG_LAT_M / 1000.0, abs=1.0) + + +def test_haversine_zero_distance(): + assert haversine_distance_m(31.2, 121.4, 31.2, 121.4) == pytest.approx(0.0, abs=1e-6) + + +# --- 对准目标：前进、不偏航 ------------------------------------------------- +def test_aligned_target_moves_forward_no_yaw(): + law = GuidanceLaw(GuidanceConfig()) + out = law.update(_inp(tgt_lat=0.001, own_yaw=0.0), now_ms=1000) # 正北 111m，机头朝北 + assert out.mode is ControlMode.VELOCITY_HEADING + assert out.valid is True + assert out.vx_body_mps > 0.0 + assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6) + assert out.vz_mps == 0.0 + assert law.last_bearing_deg == pytest.approx(0.0, abs=0.5) + + +def test_target_to_east_yaws_right_no_forward(): + law = GuidanceLaw(GuidanceConfig()) + # 目标正东（方位 90），机头朝北（0）→ 误差 +90：右转为正、未对准时前向门控为 0 + out = law.update(_inp(tgt_lon=0.001, own_yaw=0.0), now_ms=1000) + assert out.yaw_rate_dps > 0.0 + assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6) + + +def test_facing_target_moves_forward(): + law = GuidanceLaw(GuidanceConfig()) + # 目标正东、机头也朝东（yaw=90）→ 已对准，前进不偏航 + out = law.update(_inp(tgt_lon=0.001, own_yaw=90.0), now_ms=1000) + assert out.vx_body_mps > 0.0 + assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6) + + +def test_target_behind_no_forward_and_turns(): + law = GuidanceLaw(GuidanceConfig()) + out = law.update(_inp(tgt_lat=-0.001, own_yaw=0.0), now_ms=1000) # 正南，机头朝北 + assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6) # cos(180°)<0 门控为 0 + assert abs(out.yaw_rate_dps) > 0.0 + + +# --- 到达 / 不可用 / 超龄 → 刹车 ------------------------------------------- +def test_arrived_within_stop_radius_brakes(): + law = GuidanceLaw(GuidanceConfig(stop_radius_m=5.0)) + near = 2.0 / _DEG_LAT_M # ≈2m 纬度偏移 + out = law.update(_inp(tgt_lat=near), now_ms=1000) + assert out.mode is ControlMode.BRAKE_HOVER + assert out.vx_body_mps == 0.0 + + +def test_stale_target_brakes(): + law = GuidanceLaw(GuidanceConfig(target_stale_ms=1000)) + out = law.update(_inp(tgt_lat=0.001, tgt_ms=1000), now_ms=5000) + assert out.mode is ControlMode.BRAKE_HOVER + + +def test_invalid_position_brakes(): + law = GuidanceLaw(GuidanceConfig()) + out = law.update(_inp(tgt_lat=0.001, pos_valid=False), now_ms=1000) + assert out.mode is ControlMode.BRAKE_HOVER + + +def test_invalid_target_brakes(): + law = GuidanceLaw(GuidanceConfig()) + out = law.update(_inp(tgt_lat=0.001, tgt_valid=False), now_ms=1000) + assert out.mode is ControlMode.BRAKE_HOVER + + +# --- 速度上限与加速度限幅渐变 ---------------------------------------------- +def test_speed_ramps_up_and_clamps_to_max(): + cfg = GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0, + speed_slew_mps2=2.0, stop_radius_m=5.0) + law = GuidanceLaw(cfg) + forward = 0.0 + now = 1000 + for _ in range(60): # 6s，足够加速度限幅爬到上限 + out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=now, tgt_ms=now), + now_ms=now) + forward = out.vx_body_mps + assert forward <= cfg.max_speed_mps + 1e-9 + now += 100 + assert forward == pytest.approx(cfg.max_speed_mps, abs=0.1) + + +def test_first_command_is_slew_limited_not_full_speed(): + law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0, + speed_slew_mps2=2.0)) + out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000) + # 首拍从零渐变：dt=0.1s，加速度上限 2 → 前向 ≤ 0.2，远小于 5 + assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9 + + +def test_brake_then_recover_ramps_from_zero(): + law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, speed_slew_mps2=2.0)) + law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000) # 前进 + law.update(_inp(tgt_lat=0.05, tgt_valid=False), now_ms=1100) # 刹车，清状态 + out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1200) # 恢复 + assert out.mode is ControlMode.VELOCITY_HEADING + assert out.vx_body_mps <= 0.2 + 1e-9 # 从零重新渐变 + + +def test_reset_clears_state(): + law = GuidanceLaw(GuidanceConfig()) + law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000) + law.reset() + out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=2000, tgt_ms=2000), + now_ms=2000) + assert out.vx_body_mps <= 0.2 + 1e-9 # 复位后首条仍从零渐变 diff --git a/utils/tests/pod_bridge/pod_serial_test.py b/utils/tests/pod_bridge/pod_serial_test.py new file mode 100644 index 0000000..591d981 --- /dev/null +++ b/utils/tests/pod_bridge/pod_serial_test.py @@ -0,0 +1,89 @@ +"""pod_serial_test.py —— PodSerial 单元测试（注入 fake serial 模块，无硬件）。""" + +import sys +import types + +import pytest + +from pod_bridge.pod_serial import PodLink, PodSerial +from pod_bridge.protocol import TargetGeo, encode_target + + +class _FakeSerialInstance: + def __init__(self, **kwargs): + self.kwargs = kwargs + self._buf = b"" + self.written = [] + self.closed = False + + @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(data) + + def close(self): + self.closed = True + + +@pytest.fixture +def fake_serial(monkeypatch): + mod = types.ModuleType("serial") + mod.EIGHTBITS = 8 + mod.PARITY_NONE = "N" + mod.STOPBITS_ONE = 1 + mod.Serial = _FakeSerialInstance + monkeypatch.setitem(sys.modules, "serial", mod) + return mod + + +def test_empty_port_raises(): + with pytest.raises(ValueError, match="port 为空"): + PodSerial("") + + +def test_serial_opened_with_8n1(fake_serial): + ps = PodSerial("COM3", baudrate=115200) + kw = ps._ser.kwargs + assert kw["port"] == "COM3" + assert kw["baudrate"] == 115200 + assert kw["bytesize"] == 8 + assert kw["parity"] == "N" + assert kw["stopbits"] == 1 + assert kw["timeout"] == 0 # 非阻塞轮询 + + +def test_poll_parses_buffered_frames(fake_serial): + ps = PodSerial("COM3") + ps._ser._buf = encode_target(TargetGeo(latitude_deg=31.0, longitude_deg=121.0)) + frames = ps.poll() + assert len(frames) == 1 + assert isinstance(frames[0], TargetGeo) + assert frames[0].latitude_deg == pytest.approx(31.0, abs=1e-7) + + +def test_poll_empty_returns_empty_list(fake_serial): + ps = PodSerial("COM3") + assert ps.poll() == [] + + +def test_send_writes_bytes(fake_serial): + ps = PodSerial("COM3") + ps.send(b"\xeb\x90\x01") + assert ps._ser.written == [b"\xeb\x90\x01"] + + +def test_close_closes_serial(fake_serial): + ps = PodSerial("COM3") + ps.close() + assert ps._ser.closed is True + + +def test_satisfies_podlink_protocol(fake_serial): + ps = PodSerial("COM3") + assert isinstance(ps, PodLink) diff --git a/utils/tests/pod_bridge/protocol_test.py b/utils/tests/pod_bridge/protocol_test.py new file mode 100644 index 0000000..10bccc0 --- /dev/null +++ b/utils/tests/pod_bridge/protocol_test.py @@ -0,0 +1,222 @@ +"""protocol_test.py —— 光电吊舱协议编解码单元测试（规格：docs/光电吊舱控制协议G版-PB.docx）。 + +覆盖：文档样例校验和、四帧 encode/decode 回环、量纲换算、遥测位域属性、 +分帧器（粘包/半帧/噪声前缀/坏校验恢复）、异常帧拒绝。 +""" + +import pytest + +from pod_bridge.protocol import ( + CarrierAttitude, + ControlCode, + ControlCommand, + FrameParser, + Telemetry, + TargetGeo, + checksum, + decode_attitude, + decode_control, + decode_target, + decode_telemetry, + encode_attitude, + encode_control, + encode_target, + encode_telemetry, +) + + +# --- 校验和：文档第 589 行样例 --------------------------------------------- +def test_checksum_matches_doc_example(): + # 文档样例 eb 90 2b 00..00（15 字节 body）累加取低 8 位应为 0xa6 + body = bytes.fromhex("eb902b000000000000000000000000") + assert len(body) == 15 + assert checksum(body) == 0xA6 + + +def test_encode_control_center_matches_doc_bytes(): + frame = encode_control(ControlCommand(code=ControlCode.CENTER)) + assert frame == bytes.fromhex("eb902b000000000000000000000000a6") + assert len(frame) == 16 + + +# --- 表2-1 控制指令回环 ----------------------------------------------------- +def test_control_roundtrip_box_track(): + cmd = ControlCommand(code=ControlCode.BOX_TRACK, param_x=-120, param_y=340, + param3=2, file_save_mode=1, param_x2=500, param_y2=-600) + decoded = decode_control(encode_control(cmd)) + assert decoded == cmd + + +def test_control_signed_params_preserved(): + cmd = ControlCommand(code=ControlCode.POINT_TRACK, param_x=-32768, param_y=32767) + decoded = decode_control(encode_control(cmd)) + assert decoded.param_x == -32768 + assert decoded.param_y == 32767 + + +# --- 表3 载机姿态回环（含量纲换算）---------------------------------------- +def test_attitude_roundtrip(): + att = CarrierAttitude( + roll_deg=1.23, pitch_deg=-4.56, yaw_deg=179.99, + latitude_deg=31.2345678, longitude_deg=121.4737012, + abs_alt_m=123.4, rel_alt_m=56.8, + year=2026, month=9, day=22, hour=14, minute=30, second=15, centisecond=42, + airspeed_mps=15.5, ground_speed_mps=12.0, + ) + d = decode_attitude(encode_attitude(att)) + assert d.roll_deg == pytest.approx(1.23, abs=0.01) + assert d.pitch_deg == pytest.approx(-4.56, abs=0.01) + assert d.yaw_deg == pytest.approx(179.99, abs=0.01) + assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7) + assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7) + assert d.abs_alt_m == pytest.approx(123.4, abs=0.2) + assert d.rel_alt_m == pytest.approx(56.8, abs=0.2) + assert (d.year, d.month, d.day) == (2026, 9, 22) + assert (d.hour, d.minute, d.second, d.centisecond) == (14, 30, 15, 42) + assert d.airspeed_mps == pytest.approx(15.5, abs=0.5) + assert d.ground_speed_mps == pytest.approx(12.0, abs=0.5) + + +def test_attitude_sync_and_length(): + frame = encode_attitude(CarrierAttitude()) + assert len(frame) == 32 + assert frame[0] == 0xEB and frame[1] == 0x91 + + +# --- 表4 目标经纬度回环 ----------------------------------------------------- +def test_target_roundtrip(): + tgt = TargetGeo(latitude_deg=31.2345678, longitude_deg=121.4737012, + abs_alt_m=100.2, rel_alt_m=33.4, + year=2026, month=9, day=22, hour=8, minute=5, second=30, + centisecond=99) + d = decode_target(encode_target(tgt)) + assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7) + assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7) + assert d.abs_alt_m == pytest.approx(100.2, abs=0.2) + assert d.rel_alt_m == pytest.approx(33.4, abs=0.2) + assert d.year == 2026 and d.centisecond == 99 + + +def test_target_negative_longitude(): + tgt = TargetGeo(latitude_deg=-33.8688, longitude_deg=-70.6693) + d = decode_target(encode_target(tgt)) + assert d.longitude_deg == pytest.approx(-70.6693, abs=1e-7) + assert d.latitude_deg == pytest.approx(-33.8688, abs=1e-7) + + +# --- 表2-2 遥测：物理量 + 位域 --------------------------------------------- +def test_telemetry_roundtrip_physical_values(): + t = Telemetry(status1=0b10000100, status2=0b00101010, status3=0b01000000, + off_target_x_deg=1.23, off_target_y_deg=-4.56, + frame_roll_deg=10.0, frame_pitch_deg=-20.0, frame_yaw_deg=90.5, + att_roll_deg=2.0, att_pitch_deg=-3.0, + roll_rate_dps=1.5, pitch_rate_dps=-2.5, yaw_rate_dps=30.0, + laser_range_m=123.4, self_check=0) + d = decode_telemetry(encode_telemetry(t)) + assert d.off_target_x_deg == pytest.approx(1.23, abs=0.01) + assert d.off_target_y_deg == pytest.approx(-4.56, abs=0.01) + assert d.frame_yaw_deg == pytest.approx(90.5, abs=0.01) + assert d.yaw_rate_dps == pytest.approx(30.0, abs=0.01) + assert d.laser_range_m == pytest.approx(123.4, abs=0.1) + + +def test_telemetry_laser_zero_is_none(): + d = decode_telemetry(encode_telemetry(Telemetry(laser_range_m=None))) + assert d.laser_range_m is None + + +def test_telemetry_bitfield_properties(): + t = Telemetry(status1=0b11000100, status2=0b00101110, status3=0b01100000) + d = decode_telemetry(encode_telemetry(t)) + assert d.track_source == 0b11 # 红外2 + assert d.is_target_locked is True # status1 Bit2 = 1 + assert d.motor_on is True # status2 Bit3 = 1 + assert d.follow_on is True # status2 Bit2 = 1 + assert d.big_picture == 0b01 + assert d.small_picture == 0b10 + + +# --- 异常帧拒绝 ------------------------------------------------------------ +def test_decode_rejects_bad_sync(): + frame = bytearray(encode_control(ControlCommand(code=ControlCode.CENTER))) + frame[1] = 0x91 # 破坏同步字 + with pytest.raises(ValueError, match="同步字"): + decode_control(bytes(frame)) + + +def test_decode_rejects_bad_checksum(): + frame = bytearray(encode_target(TargetGeo(latitude_deg=10.0))) + frame[31] ^= 0xFF + with pytest.raises(ValueError, match="校验和"): + decode_target(bytes(frame)) + + +def test_decode_rejects_wrong_length(): + with pytest.raises(ValueError, match="帧长"): + decode_telemetry(b"\xee\x16" + b"\x00" * 10) + + +# --- 分帧器 ---------------------------------------------------------------- +def test_parser_single_frame(): + parser = FrameParser() + frames = parser.feed(encode_target(TargetGeo(latitude_deg=31.0, longitude_deg=121.0))) + assert len(frames) == 1 + assert isinstance(frames[0], TargetGeo) + assert frames[0].latitude_deg == pytest.approx(31.0, abs=1e-7) + + +def test_parser_byte_by_byte_half_frame(): + parser = FrameParser() + raw = encode_telemetry(Telemetry(laser_range_m=50.0)) + collected = [] + for i in range(len(raw)): + collected.extend(parser.feed(raw[i:i + 1])) + assert len(collected) == 1 + assert collected[0].laser_range_m == pytest.approx(50.0, abs=0.1) + + +def test_parser_concatenated_and_mixed_frames(): + parser = FrameParser() + blob = (encode_telemetry(Telemetry(laser_range_m=10.0)) + + encode_target(TargetGeo(latitude_deg=1.0)) + + encode_telemetry(Telemetry(laser_range_m=20.0))) + frames = parser.feed(blob) + assert [type(f) for f in frames] == [Telemetry, TargetGeo, Telemetry] + assert frames[0].laser_range_m == pytest.approx(10.0, abs=0.1) + assert frames[2].laser_range_m == pytest.approx(20.0, abs=0.1) + + +def test_parser_skips_noise_prefix(): + parser = FrameParser() + blob = b"\x00\xff\x12\x34" + encode_target(TargetGeo(latitude_deg=5.0)) + frames = parser.feed(blob) + assert len(frames) == 1 + assert frames[0].latitude_deg == pytest.approx(5.0, abs=1e-7) + + +def test_parser_recovers_from_bad_checksum(): + parser = FrameParser() + bad = bytearray(encode_telemetry(Telemetry(laser_range_m=1.0))) + bad[31] ^= 0xFF # 坏校验 + good = encode_target(TargetGeo(latitude_deg=7.0)) + frames = parser.feed(bytes(bad) + good) + # 坏帧被丢弃，紧随的好帧仍能解析 + assert len(frames) == 1 + assert isinstance(frames[0], TargetGeo) + + +def test_parser_split_across_feeds(): + parser = FrameParser() + raw = encode_target(TargetGeo(latitude_deg=42.0)) + assert parser.feed(raw[:5]) == [] # 半帧，未输出 + frames = parser.feed(raw[5:]) + assert len(frames) == 1 + assert frames[0].latitude_deg == pytest.approx(42.0, abs=1e-7) + + +def test_parser_accepts_configured_kinds(): + parser = FrameParser(kinds=((0xEB, 0x90),)) # 只识别控制帧（模拟器侧） + frames = parser.feed(encode_control(ControlCommand(code=ControlCode.CENTER))) + assert len(frames) == 1 + assert isinstance(frames[0], ControlCommand) + assert frames[0].code == ControlCode.CENTER diff --git a/utils/tests/pod_bridge/px4_gateway_test.py b/utils/tests/pod_bridge/px4_gateway_test.py new file mode 100644 index 0000000..8e93717 --- /dev/null +++ b/utils/tests/pod_bridge/px4_gateway_test.py @@ -0,0 +1,214 @@ +"""px4_gateway_test.py —— Px4Gateway 单元测试（mock 遥测流 + mock offboard）。 + +覆盖：遥测缓存更新（position/attitude/gps/velocity）、position_valid 判据、 +快照副本隔离、时间戳注入、枚举转换、offboard 指令委托、异步订阅启停。 +""" + +import asyncio +from enum import Enum +from types import SimpleNamespace + +import pytest + +from pod_bridge.px4_gateway import OwnState, Px4Gateway, _enum_int + + +class FakeOffboard: + def __init__(self): + self.body = [] + self.ned = [] + self.start_count = 0 + self.stop_count = 0 + + async def set_velocity_body(self, sp): + self.body.append(sp) + + async def set_velocity_ned(self, sp): + self.ned.append(sp) + + async def start(self): + self.start_count += 1 + + async def stop(self): + self.stop_count += 1 + + +class FakeTelemetry: + """各遥测流：先吐预设消息，再挂起（由 stop_telemetry 取消）。""" + + def __init__(self, position=None, attitude=None, gps=None, velocity=None): + self._position = position or [] + self._attitude = attitude or [] + self._gps = gps or [] + self._velocity = velocity or [] + + async def _stream(self, msgs): + for m in msgs: + yield m + await asyncio.sleep(3600) + + def position(self): + return self._stream(self._position) + + def attitude_euler(self): + return self._stream(self._attitude) + + def gps_info(self): + return self._stream(self._gps) + + def velocity_body(self): + return self._stream(self._velocity) + + +class FakeDrone: + def __init__(self, telemetry=None): + self.offboard = FakeOffboard() + self.telemetry = telemetry or FakeTelemetry() + + +def _pos(lat=31.0, lon=121.0, aalt=100.0, ralt=50.0): + return SimpleNamespace(latitude_deg=lat, longitude_deg=lon, + absolute_altitude_m=aalt, relative_altitude_m=ralt) + + +def _att(roll=0.1, pitch=0.2, yaw=1.57): + return SimpleNamespace(roll_rad=roll, pitch_rad=pitch, yaw_rad=yaw) + + +# --- 枚举转换 --------------------------------------------------------------- +def test_enum_int_handles_enum_and_raw(): + class Fix(Enum): + NO_FIX = 0 + THREE_D = 3 + + assert _enum_int(Fix.THREE_D) == 3 + assert _enum_int(3) == 3 + assert _enum_int(None) == 0 + + +# --- 缓存更新 --------------------------------------------------------------- +def test_update_position_sets_fields_but_not_valid_without_gps(): + gw = Px4Gateway(FakeDrone()) + gw.update_position(_pos()) + s = gw.own_state() + assert s.latitude_deg == 31.0 and s.longitude_deg == 121.0 + assert s.absolute_altitude_m == 100.0 and s.relative_altitude_m == 50.0 + assert s.has_position is True + assert s.position_valid is False # 尚无 GPS 定位 + + +def test_position_valid_requires_3d_fix(): + gw = Px4Gateway(FakeDrone()) + gw.update_position(_pos()) + gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=12)) + s = gw.own_state() + assert s.position_valid is True + assert s.num_satellites == 12 + # 定位降级 → 立即失效 + gw.update_gps(SimpleNamespace(fix_type=2, num_satellites=6)) + assert gw.own_state().position_valid is False + + +def test_update_attitude_sets_radians(): + gw = Px4Gateway(FakeDrone()) + gw.update_attitude(_att()) + s = gw.own_state() + assert s.has_attitude is True + assert s.yaw_rad == pytest.approx(1.57) + + +def test_update_velocity_body_ground_speed_is_horizontal_magnitude(): + gw = Px4Gateway(FakeDrone()) + gw.update_velocity_body(SimpleNamespace(forward_m_s=3.0, right_m_s=4.0, + down_m_s=-9.9)) + assert gw.own_state().ground_speed_mps == pytest.approx(5.0) # 不含 down + + +def test_timestamp_uses_injected_clock(): + clock = [1000] + gw = Px4Gateway(FakeDrone(), now_ms_fn=lambda: clock[0]) + gw.update_position(_pos()) + assert gw.own_state().time_ms == 1000 + assert gw.own_state().position_time_ms == 1000 + clock[0] = 1500 + gw.update_attitude(_att()) + assert gw.own_state().time_ms == 1500 + assert gw.own_state().position_time_ms == 1000 # 姿态流不得代刷位置时间戳 + + +def test_own_state_returns_isolated_copy(): + gw = Px4Gateway(FakeDrone()) + gw.update_position(_pos(lat=10.0)) + snap = gw.own_state() + snap.latitude_deg = 999.0 + assert gw.own_state().latitude_deg == 10.0 # 内部状态未被外部副本修改 + + +# --- offboard 指令委托 ------------------------------------------------------ +def test_command_delegation_reaches_offboard(): + drone = FakeDrone() + gw = Px4Gateway(drone) + + async def scenario(): + assert await gw.start_offboard() + out = SimpleNamespace(yaw_rate_dps=5.0, vx_body_mps=1.5, vz_mps=0.0, + vx_mps=0.0, vy_mps=0.0, yaw_deg=0.0) + assert await gw.send_velocity_heading(out, 1) + assert await gw.send_brake_hover(2) + await gw.stop_offboard() + + asyncio.run(scenario()) + assert drone.offboard.start_count == 1 + assert len(drone.offboard.body) >= 2 + assert drone.offboard.stop_count == 1 + + +# --- 异步订阅启停 ----------------------------------------------------------- +def test_start_telemetry_populates_cache_then_stop(): + telemetry = FakeTelemetry( + position=[_pos(lat=22.0, lon=114.0)], + attitude=[_att(yaw=0.5)], + gps=[SimpleNamespace(fix_type=3, num_satellites=10)], + velocity=[SimpleNamespace(forward_m_s=2.0, right_m_s=0.0, down_m_s=0.0)], + ) + gw = Px4Gateway(FakeDrone(telemetry)) + + async def scenario(): + await gw.start_telemetry() + await asyncio.sleep(0.05) # 让后台任务消费预设消息 + s = gw.own_state() + assert s.latitude_deg == 22.0 and s.longitude_deg == 114.0 + assert s.yaw_rad == pytest.approx(0.5) + assert s.position_valid is True + assert s.ground_speed_mps == pytest.approx(2.0) + await gw.stop_telemetry() + assert gw._tasks == [] + + asyncio.run(scenario()) + + +def test_default_own_state_is_invalid(): + s = OwnState() + assert s.has_position is False and s.position_valid is False + assert s.time_ms == 0 and s.position_time_ms == 0 + + + +def test_position_timestamp_not_refreshed_by_other_streams(): + """缺陷①回归:position 流断更后,attitude/gps/velocity 高频流继续刷新 + time_ms 时,position_time_ms 必须保持原值(位置超龄判定才有效)。""" + clock = [1000] + gw = Px4Gateway(FakeDrone(), now_ms_fn=lambda: clock[0]) + gw.update_position(_pos()) + assert gw.own_state().position_time_ms == 1000 + clock[0] = 2000 + gw.update_attitude(_att()) + gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=9)) + gw.update_velocity_body(SimpleNamespace(forward_m_s=1.0, right_m_s=0.0, + down_m_s=0.0)) + s = gw.own_state() + assert s.time_ms == 2000 # 诊断时间戳照常刷新 + assert s.position_time_ms == 1000 # 位置时间戳停留 → 超龄判定可触发 + clock[0] = 2500 + gw.update_position(_pos(lat=32.0)) # position 流恢复 → 时间戳跟进 + assert gw.own_state().position_time_ms == 2500