From d255d22fc98e805d490230c991a559357f626e82 Mon Sep 17 00:00:00 2001 From: keita <16924591+keita_sakura@user.noreply.gitee.com> Date: Mon, 28 Sep 2026 15:22:14 +0800 Subject: [PATCH] =?UTF-8?q?=E6=BF=80=E5=85=89=E9=97=A8=E6=8E=A7=E6=94=B9?= =?UTF-8?q?=E4=B8=BA=E5=8F=AF=E9=85=8D=E7=BD=AE,=20=E6=9C=AC=E8=AE=BE?= =?UTF-8?q?=E5=A4=87(=E6=97=A0=E6=BF=80=E5=85=89)=E7=BD=AEfalse?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 链路: 新增link.require_laser_valid(默认true, 有激光机型的锁定位置位粘滞兜底); 本吊舱厂商确认不含激光测距 → pod_bridge.json 置false, 并加部署配置回归测试。 文档新增§10记录设备特性(无激光/锁定位置位丢失后不复位/脱靶量无无效标志位)。 全量351条通过(新增3条激光门控用例) --- docs/树莓派4B部署注意事项.md | 14 +++++++++ utils/config/pod_bridge.json | 3 +- utils/pod_bridge/bridge.py | 17 ++++++++--- utils/pod_bridge/config.py | 19 ++++++++++-- utils/tests/pod_bridge/bridge_test.py | 43 ++++++++++++++++++++++++--- 5 files changed, 85 insertions(+), 11 deletions(-) diff --git a/docs/树莓派4B部署注意事项.md b/docs/树莓派4B部署注意事项.md index e4fbc85..3794c94 100644 --- a/docs/树莓派4B部署注意事项.md +++ b/docs/树莓派4B部署注意事项.md @@ -164,3 +164,17 @@ WantedBy=multi-user.target - 热路径不打日志是设计使然(告警走 WarnThrottle),现场排查用 `journalctl -u pod-bridge -f`。 - 代码热路径按 10Hz 引导 + 20Hz 姿态下发,Pi 4B 单核占用可忽略,无需实时内核。 + +## 10. 本设备实测特性(2026-09-28,厂商确认) + +- **本吊舱不含激光测距模块**。后果: + - 表2-2 激光测距字段恒为 0(无效),不可作为判据; + - `pod_bridge.json` 的 `link.require_laser_valid` **必须为 false**,否则引导 + 永久刹车(该闸门是为"有激光且锁定位置位粘滞"的机型准备的); + - ⚠ 需向厂商确认:无激光时吊舱如何解算表4 的目标绝对/相对高度 + (是否假设目标在地面),这直接决定引导用的坐标精度。 +- **锁定位置位(表2-2 状态字节1 bit2)在目标丢失后不复位**——实测观察; + 表2-2 脱靶量是定长字段且协议**没有无效标志位**,丢失时表现为 0, + 无法区分"不输出"与"输出 0"。 + → 单靠 bit2 会在目标丢失后继续朝过期坐标飞,**必须有第二判据**(见 §8/§11)。 + diff --git a/utils/config/pod_bridge.json b/utils/config/pod_bridge.json index 8c32df9..6244698 100644 --- a/utils/config/pod_bridge.json +++ b/utils/config/pod_bridge.json @@ -8,7 +8,8 @@ }, "link": { "attitude_downlink_hz": 20.0, - "telemetry_timeout_ms": 500 + "telemetry_timeout_ms": 500, + "require_laser_valid": false }, "guidance": { "frequency_hz": 10.0, diff --git a/utils/pod_bridge/bridge.py b/utils/pod_bridge/bridge.py index 205eaf8..3c3fbe8 100644 --- a/utils/pod_bridge/bridge.py +++ b/utils/pod_bridge/bridge.py @@ -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: diff --git a/utils/pod_bridge/config.py b/utils/pod_bridge/config.py index 9c6a178..8126b39 100644 --- a/utils/pod_bridge/config.py +++ b/utils/pod_bridge/config.py @@ -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 diff --git a/utils/tests/pod_bridge/bridge_test.py b/utils/tests/pod_bridge/bridge_test.py index 3b4049c..9089406 100644 --- a/utils/tests/pod_bridge/bridge_test.py +++ b/utils/tests/pod_bridge/bridge_test.py @@ -10,7 +10,7 @@ import time from dataclasses import replace from pod_bridge.bridge import PodBridge -from pod_bridge.config import BridgeConfig +from pod_bridge.config import BridgeConfig, load_from_json from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo, Telemetry, decode_attitude, decode_control) from pod_bridge.px4_gateway import OwnState @@ -79,9 +79,9 @@ def _valid_state(now_ms, lat=0.0, lon=0.0, yaw_rad=0.0, gs=0.0): time_ms=now_ms, position_time_ms=now_ms) -def _locked_telemetry(): - """状态字节1 bit2=1(锁定)的遥测帧。""" - return Telemetry(status1=0b00000100) +def _locked_telemetry(laser=50.0): + """状态字节1 bit2=1(锁定)的遥测帧;默认带有效激光(默认闸门要求激光有效)。""" + return Telemetry(status1=0b00000100, laser_range_m=laser) def _bridge(px4, pod, cfg=None): @@ -297,6 +297,41 @@ def test_lock_lost_midflight_falls_back_to_brake(): assert out2.mode.name == "BRAKE_HOVER" +# --- 激光有效性门控(锁定位置位粘滞时的兜底)----------------------------------- +def test_laser_invalid_blocks_guidance_when_required(): + """默认 require_laser_valid=True:锁定位置位但激光无效 → 目标无效 → 刹车。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry(laser=None)) + b = _bridge(px4, pod) + + out = asyncio.run(b.step(1000)) + assert out.mode.name == "BRAKE_HOVER" + assert px4.velocity_calls == [] + + +def test_laser_invalid_allowed_when_gate_disabled(): + """本设备无激光(厂商确认)→ require_laser_valid=False 时照常引导。""" + px4 = FakePx4(_valid_state(1000)) + pod = FakePod() + pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry(laser=None)) + cfg = BridgeConfig() + cfg.link.require_laser_valid = False + b = _bridge(px4, pod, cfg) + + out = asyncio.run(b.step(1000)) + assert out.mode.name == "VELOCITY_HEADING" + + +def test_deployed_config_disables_laser_gate(): + """部署配置回归:本机无激光,pod_bridge.json 必须关掉激光闸门。""" + import os + path = os.path.join(os.path.dirname(os.path.dirname(os.path.dirname( + os.path.abspath(__file__)))), "config", "pod_bridge.json") + cfg = load_from_json(path) + assert cfg.link.require_laser_valid is False + + # --- 缺陷③:run() 异常护栏 ----------------------------------------------------- class FlakyPod(FakePod): """前 fail_times 次 poll 抛串口异常,之后恢复正常。"""