激光门控改为可配置, 本设备(无激光)置false
链路: 新增link.require_laser_valid(默认true, 有激光机型的锁定位置位粘滞兜底); 本吊舱厂商确认不含激光测距 → pod_bridge.json 置false, 并加部署配置回归测试。 文档新增§10记录设备特性(无激光/锁定位置位丢失后不复位/脱靶量无无效标志位)。 全量351条通过(新增3条激光门控用例)
This commit is contained in:
@@ -171,16 +171,25 @@ class PodBridge:
|
||||
)
|
||||
|
||||
def _pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱锁定门控:表2-2 遥测在 link.telemetry_timeout_ms 内且状态字节1
|
||||
bit2=锁定 才算锁定。从未收到遥测 / 遥测超龄 / 搜索状态一律视为未锁定
|
||||
(宁可刹车,不朝搜索状态下的无意义坐标飞)。"""
|
||||
"""吊舱目标门控:表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1
|
||||
bit2=锁定、且(若 link.require_laser_valid)激光测距有效,才算目标有效。
|
||||
|
||||
实测依据:部分吊舱目标丢失后**锁定位置位不复位**(脱靶量也没有无效标志位,
|
||||
恒为 0),仅凭 bit2 会朝过期坐标继续飞;激光无效时吊舱不解算目标经纬度,
|
||||
故此兜底。无激光或激光恒无效的吊舱把 require_laser_valid 置 False。
|
||||
|
||||
从未收到遥测 / 遥测超龄 / 搜索状态 / 激光无效 一律视为目标无效(宁可刹车)。"""
|
||||
tel = self._latest_telemetry
|
||||
if tel is None:
|
||||
return False
|
||||
frame, t_ms = tel
|
||||
if is_stale(t_ms, now_ms, self._cfg.link.telemetry_timeout_ms):
|
||||
return False
|
||||
return frame.is_target_locked
|
||||
if not frame.is_target_locked:
|
||||
return False
|
||||
if self._cfg.link.require_laser_valid and frame.laser_range_m is None:
|
||||
return False
|
||||
return True
|
||||
|
||||
async def _dispatch(self, out, now_ms: int) -> None:
|
||||
if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
|
||||
|
||||
@@ -40,7 +40,10 @@ class LinkConfig:
|
||||
"""链路频率与超时。"""
|
||||
|
||||
attitude_downlink_hz: float = 20.0 # 表3 载机姿态下发频率(文档:1~60Hz)
|
||||
telemetry_timeout_ms: int = 500 # 吊舱遥测/目标超龄判定(链路级)
|
||||
telemetry_timeout_ms: int = 500 # 吊舱遥测超龄判定(链路级)
|
||||
require_laser_valid: bool = True # 目标有效是否要求激光测距有效
|
||||
# (吊舱锁定位置位在目标丢失后可能不复位,
|
||||
# 激光无效即不解算,以此兜底;无激光的吊舱置 False)
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -105,6 +108,15 @@ def _get_string(section: dict, section_name: str, key: str, default: str) -> str
|
||||
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}")
|
||||
@@ -152,7 +164,8 @@ def _load_px4(root: dict) -> Px4Config:
|
||||
|
||||
def _load_link(root: dict) -> LinkConfig:
|
||||
s = _require_section(root, "link")
|
||||
_reject_unknown(s, {"attitude_downlink_hz", "telemetry_timeout_ms"}, "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",
|
||||
@@ -161,6 +174,8 @@ def _load_link(root: dict) -> LinkConfig:
|
||||
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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user