实现L1航向伺服(路线A1): 吊舱角度直入控制律 + 桥接胶水 + 干跑入口
- 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
This commit is contained in:
@@ -16,6 +16,9 @@ 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
|
||||
|
||||
|
||||
@@ -46,12 +49,34 @@ class LinkConfig:
|
||||
# 激光无效即不解算,以此兜底;无激光的吊舱置 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)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
@@ -228,6 +253,47 @@ def _load_guidance(root: dict) -> GuidanceConfig:
|
||||
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:
|
||||
@@ -246,4 +312,6 @@ def load_from_json(path: str) -> BridgeConfig:
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user