keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
357 lines
15 KiB
Python
357 lines
15 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
|
||
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
|