keita
477ad5f5de
- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环 - 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏 - 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单
235 lines
8.8 KiB
Python
235 lines
8.8 KiB
Python
"""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
|