keita
5a944af4f5
- visual_track_control: ControlSnapshot 加可选 los_beta_deg/los_alpha_deg, 第④步角度直入优先否则回退像素路径(向后兼容); config 拆出 load_from_dict 供内嵌复用 - pod_bridge: BridgeConfig 内嵌 TrackingConfig + 新 HeadingServoConfig; bridge._build_snapshot 合成 β/α(姿态来自PX4/角度来自吊舱,信号双路适配无电机), step 按 heading_servo.enabled 切 L1/L2, DEGRADED_HOLD 补发刹车保 offboard 心跳; run() 循环频率按生效律取(修 L1 频率错配) - run_bridge.py: 闭环 CLI 入口, --dry-run(S2 只接吊舱不发offboard) 与实飞优雅停机 - 配置落 L1 首飞值(slow_approach/0.2mps、垂直压低、无电机仅用脱靶量) - 新增测试 29 条(vtc5+config9+bridgeL1 10+runner5); 全量 401 passed
318 lines
13 KiB
Python
318 lines
13 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 visual_track_control.config import TrackingConfig
|
||
from visual_track_control.config import load_from_dict as _load_tracking_dict
|
||
|
||
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 # 吊舱遥测超龄判定（链路级）
|
||
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
|
||
require_vision: bool = False # true = 需我方视觉作第二判据（S3 起，§2.3）
|
||
|
||
|
||
@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)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 校验原语（错误消息携带完整字段名）
|
||
# ---------------------------------------------------------------------------
|
||
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.require_vision = _get_bool(
|
||
s, "heading_servo", "require_vision", c.require_vision)
|
||
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_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)
|
||
return cfg
|