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