"""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 visual_track_control.config import TrackingConfig from visual_track_control.config import load_from_dict as _load_tracking_dict from .guidance import GuidanceConfig from .ground_link import GroundInjectConfig @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 # 吊舱遥测超龄判定（链路级） require_laser_valid: bool = True # 目标有效是否要求激光测距有效 # （吊舱锁定位置位在目标丢失后可能不复位， # 激光无效即不解算，以此兜底；无激光的吊舱置 False） @dataclass class HeadingServoConfig: """L1 航向伺服（方案 §2/§4）：把吊舱表2-2 角度喂进 TrackingControlLaw。 信号合成（§2.1）：β = beta_sign·(beta_from_frame·frame_yaw + beta_from_off_target·off_target_x)， α 同理。无电机吊舱 frame_yaw 恒 0（"框架角无用"）→ 仅用脱靶量；装电机后两者叠加。 符号（±1）台架 S1 标定后填，配置化而非硬编码，防接反反向转。 """ enabled: bool = False # True = bridge 走 L1(TrackingControlLaw)，False = L2(GuidanceLaw) beta_sign: int = 1 # 水平合成符号（台架标定 ±1） alpha_sign: int = 1 # 垂直合成符号 beta_from_frame: bool = False # β 是否叠加框架角 frame_yaw_deg（装电机后开） beta_from_off_target: bool = True # β 是否叠加脱靶量 off_target_x_deg alpha_from_frame: bool = False # α 是否叠加框架角 frame_pitch_deg alpha_from_off_target: bool = True # α 是否叠加脱靶量 off_target_y_deg vertical_enabled: bool = True # 无人机 L1 开垂直（决策3）；false → α 强制 0 # --- 目标有效性第二判据：LOS 运动学一致性（2026-10-19 取代 require_vision）--- # 背景：吊舱已取代双目+YOLO 成为唯一目标源，"我方视觉"不再存在；而 bit2 # 丢失后不复位、脱靶量丢失表现为 0。故改用"机头转动 vs 脱靶量变化"的一致性 # 作为独立于 bit2 的活体判据（详见 los_consistency.py）。 los_consistency_enabled: bool = True los_window_ms: int = 3000 # 比较窗口（毫秒） los_min_yaw_change_deg: float = 20.0 # 窗口内机头至少转这么多才判定（激励门槛） los_max_residual_deg: float = 8.0 # |Δβ+Δψ| 上限，超过判不一致 @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) heading_servo: HeadingServoConfig = field(default_factory=HeadingServoConfig) tracking: TrackingConfig = field(default_factory=TrackingConfig) ground_inject: GroundInjectConfig = field(default_factory=GroundInjectConfig) # --------------------------------------------------------------------------- # 校验原语（错误消息携带完整字段名） # --------------------------------------------------------------------------- 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 _get_bool(section: dict, section_name: str, key: str, default: bool) -> bool: if key not in section: return bool(default) value = section[key] if not isinstance(value, bool): 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", "require_laser_valid"}, "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")) c.require_laser_valid = _get_bool( s, "link", "require_laser_valid", c.require_laser_valid) 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 _get_sign(section: dict, section_name: str, key: str, default: int) -> int: """符号位：仅允许 -1 / +1（配置化防接反，方案 §2.1/§6）。""" if key not in section: return int(default) value = section[key] if not _is_integer(value) or value not in (-1, 1): raise ValueError(f"配置非法(仅允许 -1 或 1): {section_name}.{key}") return int(value) def _load_heading_servo(root: dict) -> HeadingServoConfig: s = _require_section(root, "heading_servo") allowed = {f.name for f in dataclasses.fields(HeadingServoConfig)} _reject_unknown(s, allowed, "heading_servo") c = HeadingServoConfig() c.enabled = _get_bool(s, "heading_servo", "enabled", c.enabled) c.beta_sign = _get_sign(s, "heading_servo", "beta_sign", c.beta_sign) c.alpha_sign = _get_sign(s, "heading_servo", "alpha_sign", c.alpha_sign) c.beta_from_frame = _get_bool( s, "heading_servo", "beta_from_frame", c.beta_from_frame) c.beta_from_off_target = _get_bool( s, "heading_servo", "beta_from_off_target", c.beta_from_off_target) c.alpha_from_frame = _get_bool( s, "heading_servo", "alpha_from_frame", c.alpha_from_frame) c.alpha_from_off_target = _get_bool( s, "heading_servo", "alpha_from_off_target", c.alpha_from_off_target) c.vertical_enabled = _get_bool( s, "heading_servo", "vertical_enabled", c.vertical_enabled) c.los_consistency_enabled = _get_bool( s, "heading_servo", "los_consistency_enabled", c.los_consistency_enabled) c.los_window_ms = int(_positive( _get_integer(s, "heading_servo", "los_window_ms", c.los_window_ms), "heading_servo.los_window_ms")) c.los_min_yaw_change_deg = _in_range( _get_number(s, "heading_servo", "los_min_yaw_change_deg", c.los_min_yaw_change_deg), 0.0, 180.0, "heading_servo.los_min_yaw_change_deg") c.los_max_residual_deg = _in_range( _get_number(s, "heading_servo", "los_max_residual_deg", c.los_max_residual_deg), 0.0, 180.0, "heading_servo.los_max_residual_deg") return c def _load_tracking(root: dict) -> TrackingConfig: """L1 控制律配置：缺省 → TrackingConfig() 默认；提供 → 按 vtc 严格解析（各分组齐全）。""" s = _require_section(root, "tracking") if not s: return TrackingConfig() return _load_tracking_dict(s) def _load_ground_inject(root: dict) -> GroundInjectConfig: """地面坐标注入配置（预定流程「地面给坐标」，A2）。""" s = _require_section(root, "ground_inject") allowed = {f.name for f in dataclasses.fields(GroundInjectConfig)} _reject_unknown(s, allowed, "ground_inject") c = GroundInjectConfig() c.enabled = _get_bool(s, "ground_inject", "enabled", c.enabled) c.host = _get_string(s, "ground_inject", "host", c.host) c.port = int(_in_range( _get_integer(s, "ground_inject", "port", c.port), 1, 65535, "ground_inject.port")) c.timeout_ms = int(_positive( _get_integer(s, "ground_inject", "timeout_ms", c.timeout_ms), "ground_inject.timeout_ms")) return c 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) cfg.heading_servo = _load_heading_servo(root) cfg.tracking = _load_tracking(root) cfg.ground_inject = _load_ground_inject(root) return cfg