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