songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增pod_bridge吊舱桥接中间件(树莓派4B部署)

Browse Source 
- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环
- 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏
- 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
This commit is contained in:
keita
2026-09-23 14:38:23 +08:00
parent 3a8e2c7f4b
commit 477ad5f5de
17 changed files with 2758 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
utils/config/pod_bridge.json
+29
View File
@@ -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
}
}
utils/pod_bridge/__init__.py
+108
View File
@@ -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",
]
utils/pod_bridge/bridge.py
+207
View File
@@ -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)
utils/pod_bridge/config.py
+234
View File
@@ -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
utils/pod_bridge/guidance.py
+204
View File
@@ -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
utils/pod_bridge/pod_serial.py
+68
View File
@@ -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)
utils/pod_bridge/protocol.py
+419
View File
@@ -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(
"<BBBhhBbhh2s",
SYNC_CONTROL[0], SYNC_CONTROL[1],
cmd.code & 0xFF,
_to_s16(cmd.param_x, 1), _to_s16(cmd.param_y, 1),
cmd.param3 & 0xFF,
max(-128, min(127, int(cmd.file_save_mode))),
_to_s16(cmd.param_x2, 1), _to_s16(cmd.param_y2, 1),
b"\x00\x00",
)
return body + bytes([checksum(body)])
def decode_control(data: bytes) -> ControlCommand:
_check(data, CONTROL_LEN, SYNC_CONTROL)
code, x, y, param3, save, x2, y2 = struct.unpack("<BhhBbhh", data[2:13])
return ControlCommand(code=code, param_x=x, param_y=y, param3=param3,
file_save_mode=save, param_x2=x2, param_y2=y2)
# --- 表2-2 周期遥测 ---------------------------------------------------------
@dataclass
class Telemetry:
"""吊舱 → 控制器 周期遥测帧（60Hz）。角度单位 °，角速度 °/s，测距 m。"""
status1: int = 0
status2: int = 0
status3: int = 0
off_target_x_deg: float = 0.0 # 目标脱靶量 X 轴偏移角
off_target_y_deg: float = 0.0 # 目标脱靶量 Y 轴偏移角
frame_roll_deg: float = 0.0 # 横滚框架角
frame_pitch_deg: float = 0.0 # 俯仰框架角
frame_yaw_deg: float = 0.0 # 方位框架角
att_roll_deg: float = 0.0 # 横滚姿态角
att_pitch_deg: float = 0.0 # 俯仰姿态角
roll_rate_dps: float = 0.0
pitch_rate_dps: float = 0.0
yaw_rate_dps: float = 0.0
laser_range_m: float | None = None # None 表示无效（原始 0）
self_check: int = 0
@property
def track_source(self) -> 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(
"<BBBBBBhhhhhhhhhhHB2s",
SYNC_TELEMETRY[0], SYNC_TELEMETRY[1],
t.status1 & 0xFF, t.status2 & 0xFF, 0, t.status3 & 0xFF,
_to_s16(t.off_target_x_deg, ANGLE_SCALE),
_to_s16(t.off_target_y_deg, ANGLE_SCALE),
_to_s16(t.frame_roll_deg, ANGLE_SCALE),
_to_s16(t.frame_pitch_deg, ANGLE_SCALE),
_to_s16(t.frame_yaw_deg, ANGLE_SCALE),
_to_s16(t.att_roll_deg, ANGLE_SCALE),
_to_s16(t.att_pitch_deg, ANGLE_SCALE),
_to_s16(t.roll_rate_dps, RATE_SCALE),
_to_s16(t.pitch_rate_dps, RATE_SCALE),
_to_s16(t.yaw_rate_dps, RATE_SCALE),
laser,
t.self_check & 0xFF,
b"\x00\x00",
)
return body + bytes([checksum(body)])
def decode_telemetry(data: bytes) -> 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(
"<BBBBhhhhhhhhhhHB2s", data[2:31])
return Telemetry(
status1=s1, status2=s2, status3=s3,
off_target_x_deg=ox * ANGLE_SCALE,
off_target_y_deg=oy * ANGLE_SCALE,
frame_roll_deg=froll * ANGLE_SCALE,
frame_pitch_deg=fpitch * ANGLE_SCALE,
frame_yaw_deg=fyaw * ANGLE_SCALE,
att_roll_deg=aroll * ANGLE_SCALE,
att_pitch_deg=apitch * ANGLE_SCALE,
roll_rate_dps=rrate * RATE_SCALE,
pitch_rate_dps=prate * RATE_SCALE,
yaw_rate_dps=yrate * RATE_SCALE,
laser_range_m=(laser * RANGE_SCALE) if laser != 0 else None,
self_check=selfcheck,
)
# --- 表3 载机姿态和经纬度 ---------------------------------------------------
@dataclass
class CarrierAttitude:
"""控制器 → 吊舱 载机姿态/位置/时间帧（供吊舱解算目标经纬度）。"""
roll_deg: float = 0.0
pitch_deg: float = 0.0
yaw_deg: float = 0.0
latitude_deg: float = 0.0
longitude_deg: float = 0.0
abs_alt_m: float = 0.0
rel_alt_m: float = 0.0
year: int = 2000 # 完整年份（编码时存 year-2000）
month: int = 1
day: int = 1
hour: int = 0
minute: int = 0
second: int = 0
centisecond: int = 0 # 百分秒，10ms 单位，0~99
airspeed_mps: float = 0.0
ground_speed_mps: float = 0.0
def encode_attitude(a: CarrierAttitude) -> bytes:
body = struct.pack(
"<BBhhhiihhBBBBBBBHH",
SYNC_ATTITUDE[0], SYNC_ATTITUDE[1],
_to_s16(a.roll_deg, ANGLE_SCALE),
_to_s16(a.pitch_deg, ANGLE_SCALE),
_to_s16(a.yaw_deg, ANGLE_SCALE),
_to_s32(a.latitude_deg, LATLON_SCALE),
_to_s32(a.longitude_deg, LATLON_SCALE),
_to_s16(a.abs_alt_m, ALT_SCALE),
_to_s16(a.rel_alt_m, ALT_SCALE),
(a.year - 2000) & 0xFF, a.month & 0xFF, a.day & 0xFF,
a.hour & 0xFF, a.minute & 0xFF, a.second & 0xFF, a.centisecond & 0xFF,
_to_u16(a.airspeed_mps, SPEED_SCALE),
_to_u16(a.ground_speed_mps, SPEED_SCALE),
)
return body + bytes([checksum(body)])
def decode_attitude(data: bytes) -> 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("<hhhiihhBBBBBBBHH", data[2:31])
return CarrierAttitude(
roll_deg=roll * ANGLE_SCALE,
pitch_deg=pitch * ANGLE_SCALE,
yaw_deg=yaw * ANGLE_SCALE,
latitude_deg=lat * LATLON_SCALE,
longitude_deg=lon * LATLON_SCALE,
abs_alt_m=absalt * ALT_SCALE,
rel_alt_m=relalt * ALT_SCALE,
year=2000 + yr, month=mo, day=day, hour=hour, minute=mi,
second=sec, centisecond=cs,
airspeed_mps=air * SPEED_SCALE,
ground_speed_mps=gs * SPEED_SCALE,
)
# --- 表4 目标经纬度 ---------------------------------------------------------
@dataclass
class TargetGeo:
"""吊舱 → 控制器 目标经纬度帧（吊舱解算结果）。"""
latitude_deg: float = 0.0
longitude_deg: float = 0.0
abs_alt_m: float = 0.0
rel_alt_m: float = 0.0
year: int = 2000
month: int = 1
day: int = 1
hour: int = 0
minute: int = 0
second: int = 0
centisecond: int = 0
def encode_target(g: TargetGeo) -> bytes:
body = struct.pack(
"<BBiihhBBBBBBB10s",
SYNC_TARGET[0], SYNC_TARGET[1],
_to_s32(g.latitude_deg, LATLON_SCALE),
_to_s32(g.longitude_deg, LATLON_SCALE),
_to_s16(g.abs_alt_m, ALT_SCALE),
_to_s16(g.rel_alt_m, ALT_SCALE),
(g.year - 2000) & 0xFF, g.month & 0xFF, g.day & 0xFF,
g.hour & 0xFF, g.minute & 0xFF, g.second & 0xFF, g.centisecond & 0xFF,
b"\x00" * 10,
)
return body + bytes([checksum(body)])
def decode_target(data: bytes) -> TargetGeo:
_check(data, TARGET_LEN, SYNC_TARGET)
(lat, lon, absalt, relalt, yr, mo, day, hour, mi, sec, cs,
_rsv) = struct.unpack("<iihhBBBBBBB10s", data[2:31])
return TargetGeo(
latitude_deg=lat * LATLON_SCALE,
longitude_deg=lon * LATLON_SCALE,
abs_alt_m=absalt * ALT_SCALE,
rel_alt_m=relalt * ALT_SCALE,
year=2000 + yr, month=mo, day=day, hour=hour, minute=mi,
second=sec, centisecond=cs,
)
def _check(data: bytes, length: int, sync: tuple) -> 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
utils/pod_bridge/px4_gateway.py
+169
View File
@@ -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()
utils/tests/pod_bridge/bridge_test.py
+358
View File
@@ -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 # 计数已复位
utils/tests/pod_bridge/config_test.py
+115
View File
@@ -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 == ""
utils/tests/pod_bridge/guidance_test.py
+152
View File
@@ -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 # 复位后首条仍从零渐变
utils/tests/pod_bridge/pod_serial_test.py
+89
View File
@@ -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)
utils/tests/pod_bridge/protocol_test.py
+222
View File
@@ -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
utils/tests/pod_bridge/px4_gateway_test.py
+214
View File
@@ -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