新增pod_bridge吊舱桥接中间件(树莓派4B部署)

- pod_bridge: 协议G版编解码/地理位置引导律/PX4网关/串口链路/闭环主循环
- 含三项安全修复: position_time_ms拆分、表2-2锁定门控、run()异常护栏
- 测试92条全绿; 附树莓派4B(Ubuntu 24.04)部署注意事项与最小依赖清单

utils/tests/pod_bridge/bridge_test.py

@@ -0,0 +1,358 @@
+"""bridge_test.py —— PodBridge 闭环主循环单元测试（FakePx4 + FakePod，无硬件）。
+
+覆盖：单周期 step（有/无目标、位置无效、位置时间戳超龄）、姿态下发节流与帧内容、
+吊舱帧入站缓存、锁定门控（表2-2 bit2 + 遥测超龄）、触发式控制指令编码、
+start/stop 生命周期与停机刹车、run() 主循环异常护栏与故障恢复。
+"""
+
+import asyncio
+import time
+from dataclasses import replace
+
+from pod_bridge.bridge import PodBridge
+from pod_bridge.config import BridgeConfig
+from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
+                                 Telemetry, decode_attitude, decode_control)
+from pod_bridge.px4_gateway import OwnState
+
+
+class FakePx4:
+    def __init__(self, state=None):
+        self._state = state or OwnState()
+        self.telemetry_started = False
+        self.telemetry_stopped = False
+        self.offboard_started = False
+        self.offboard_stopped = False
+        self.offboard_start_ok = True
+        self.velocity_calls = []
+        self.brake_calls = []
+
+    def own_state(self):
+        return replace(self._state)
+
+    async def start_telemetry(self):
+        self.telemetry_started = True
+
+    async def stop_telemetry(self):
+        self.telemetry_stopped = True
+
+    async def start_offboard(self):
+        self.offboard_started = True
+        return self.offboard_start_ok
+
+    async def stop_offboard(self):
+        self.offboard_stopped = True
+
+    async def send_velocity_heading(self, out, time_boot_ms=None):
+        self.velocity_calls.append((out, time_boot_ms))
+        return True
+
+    async def send_brake_hover(self, time_boot_ms=None):
+        self.brake_calls.append(time_boot_ms)
+        return True
+
+
+class FakePod:
+    def __init__(self):
+        self.inbox = []
+        self.sent = []
+        self.closed = False
+
+    def queue(self, *frames):
+        self.inbox.extend(frames)
+
+    def poll(self):
+        out, self.inbox = self.inbox, []
+        return out
+
+    def send(self, data):
+        self.sent.append(data)
+
+    def close(self):
+        self.closed = True
+
+
+def _valid_state(now_ms, lat=0.0, lon=0.0, yaw_rad=0.0, gs=0.0):
+    return OwnState(latitude_deg=lat, longitude_deg=lon, yaw_rad=yaw_rad,
+                    ground_speed_mps=gs, fix_type=3, num_satellites=12,
+                    has_position=True, has_attitude=True, position_valid=True,
+                    time_ms=now_ms, position_time_ms=now_ms)
+
+
+def _locked_telemetry():
+    """状态字节1 bit2=1(锁定)的遥测帧。"""
+    return Telemetry(status1=0b00000100)
+
+
+def _bridge(px4, pod, cfg=None):
+    return PodBridge(cfg or BridgeConfig(), px4, pod, now_ms_fn=lambda: 0)
+
+
+# --- step：有目标 → 速度指令 ------------------------------------------------
+def test_step_with_target_commands_velocity():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001, longitude_deg=0.0),  # 正北 ~111m
+              _locked_telemetry())
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "VELOCITY_HEADING"
+    assert len(px4.velocity_calls) == 1
+    assert px4.brake_calls == []
+
+
+def test_step_without_target_brakes():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+    assert len(px4.brake_calls) == 1
+    assert px4.velocity_calls == []
+
+
+def test_step_invalid_position_brakes_even_with_target():
+    state = replace(_valid_state(1000), position_valid=False, fix_type=0)
+    px4 = FakePx4(state)
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001))
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+
+
+# --- 姿态下发：首拍即发 + 节流 + 帧内容 ------------------------------------
+def test_attitude_downlinked_on_first_step_with_own_state():
+    px4 = FakePx4(_valid_state(1000, lat=31.5, lon=121.5, yaw_rad=1.57, gs=4.0))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    asyncio.run(b.step(1000))
+    assert len(pod.sent) == 1
+    att = decode_attitude(pod.sent[0])
+    assert abs(att.latitude_deg - 31.5) < 1e-6
+    assert abs(att.longitude_deg - 121.5) < 1e-6
+    assert abs(att.yaw_deg - 89.95) < 0.02      # 1.57 rad ≈ 89.95°
+    assert att.ground_speed_mps == 4.0
+    assert att.year >= 2026 and 1 <= att.month <= 12
+
+
+def test_attitude_downlink_throttled():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    cfg = BridgeConfig()
+    cfg.link.attitude_downlink_hz = 20.0  # 周期 50ms
+    b = _bridge(px4, pod, cfg)
+
+    async def scenario():
+        await b.step(1000)  # 发（首拍）
+        await b.step(1010)  # 10ms < 50ms，跳过
+        await b.step(1040)  # 40ms < 50ms，跳过
+        await b.step(1060)  # 60ms ≥ 50ms，发
+    asyncio.run(scenario())
+    assert len(pod.sent) == 2
+
+
+# --- 吊舱帧入站缓存 --------------------------------------------------------
+def test_target_frame_updates_latest_and_flips_to_velocity():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    asyncio.run(b.step(1000))                 # 无目标 → 刹车
+    assert b.latest_target is None
+    pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+    asyncio.run(b.step(1100))                 # 收到目标且锁定 → 速度
+    assert b.latest_target is not None
+    assert len(px4.velocity_calls) == 1
+
+
+def test_telemetry_frame_cached():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(Telemetry(laser_range_m=88.8))
+    b = _bridge(px4, pod)
+    asyncio.run(b.step(1000))
+    assert b.latest_telemetry is not None
+    assert b.latest_telemetry[0].laser_range_m == 88.8
+
+
+# --- 触发式控制指令 --------------------------------------------------------
+def test_send_control_encodes_frame():
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+    b.send_control(ControlCommand(code=ControlCode.CENTER))
+    assert len(pod.sent) == 1
+    assert decode_control(pod.sent[0]).code == ControlCode.CENTER
+
+
+# --- 生命周期 --------------------------------------------------------------
+def test_start_wires_telemetry_and_offboard():
+    px4 = FakePx4(_valid_state(0))
+    b = _bridge(px4, FakePod())
+    assert asyncio.run(b.start()) is True
+    assert px4.telemetry_started and px4.offboard_started
+
+
+def test_start_returns_false_when_offboard_fails():
+    px4 = FakePx4(_valid_state(0))
+    px4.offboard_start_ok = False
+    b = _bridge(px4, FakePod())
+    assert asyncio.run(b.start()) is False
+
+
+def test_stop_brakes_and_tears_down():
+    px4 = FakePx4(_valid_state(0))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    async def scenario():
+        await b.start()
+        await b.stop()
+    asyncio.run(scenario())
+    assert len(px4.brake_calls) >= 1     # 停机前刹车
+    assert px4.offboard_stopped and px4.telemetry_stopped
+    assert pod.closed
+
+
+
+# --- 缺陷①回归:position 时间戳超龄 -------------------------------------------
+def test_step_brakes_when_position_timestamp_stale():
+    """time_ms 被姿态流刷得很新,但 position_time_ms 超龄 → 必须刹车。"""
+    state = replace(_valid_state(1000), time_ms=100000, position_time_ms=1000)
+    px4 = FakePx4(state)
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(100000))  # position 已 99s 未更新
+    assert out.mode.name == "BRAKE_HOVER"
+    assert px4.velocity_calls == []
+
+
+# --- 缺陷②:锁定门控 -----------------------------------------------------------
+def test_target_without_lock_bit_brakes():
+    """吊舱在搜索状态(bit2=0),即使收到表4 坐标也必须刹车。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001), Telemetry(status1=0))  # 未锁定
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+    assert px4.velocity_calls == []
+
+
+def test_target_without_any_telemetry_brakes():
+    """只收到表4、从未收到表2-2 → 锁定状态未知,按未锁定处理。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    pod.queue(TargetGeo(latitude_deg=0.001))
+    b = _bridge(px4, pod)
+
+    out = asyncio.run(b.step(1000))
+    assert out.mode.name == "BRAKE_HOVER"
+
+
+def test_target_with_stale_telemetry_brakes():
+    """锁定遥测超龄(> link.telemetry_timeout_ms)→ 目标视为无效。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    cfg = BridgeConfig()
+    cfg.link.telemetry_timeout_ms = 500
+    b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0)
+
+    async def scenario():
+        pod.queue(_locked_telemetry(), TargetGeo(latitude_deg=0.001))
+        await b.step(1000)                    # 遥测新鲜,但此刻若无目标则刹车
+        pod.queue(TargetGeo(latitude_deg=0.001))
+        out1 = await b.step(1200)             # 遥测 200ms 前,仍新鲜 → 速度
+        out2 = await b.step(1600)             # 遥测 600ms 前,超龄 → 刹车
+        return out1, out2
+
+    out1, out2 = asyncio.run(scenario())
+    assert out1.mode.name == "VELOCITY_HEADING"
+    assert out2.mode.name == "BRAKE_HOVER"
+
+
+def test_lock_lost_midflight_falls_back_to_brake():
+    """先锁定引导,随后遥测变为搜索状态 → 立即回落刹车。"""
+    px4 = FakePx4(_valid_state(1000))
+    pod = FakePod()
+    b = _bridge(px4, pod)
+
+    async def scenario():
+        pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+        out1 = await b.step(1000)
+        pod.queue(Telemetry(status1=0))       # 丢失锁定
+        out2 = await b.step(1100)
+        return out1, out2
+
+    out1, out2 = asyncio.run(scenario())
+    assert out1.mode.name == "VELOCITY_HEADING"
+    assert out2.mode.name == "BRAKE_HOVER"
+
+
+# --- 缺陷③:run() 异常护栏 -----------------------------------------------------
+class FlakyPod(FakePod):
+    """前 fail_times 次 poll 抛串口异常,之后恢复正常。"""
+
+    def __init__(self, fail_times):
+        super().__init__()
+        self.fail_times = fail_times
+
+    def poll(self):
+        if self.fail_times > 0:
+            self.fail_times -= 1
+            raise OSError("serial device disconnected")
+        return super().poll()
+
+
+def _run_bridge(px4, pod, hz=200.0):
+    cfg = BridgeConfig()
+    cfg.guidance.frequency_hz = hz  # 加快节拍,缩短测试时间
+    return PodBridge(cfg, px4, pod)  # 用真实单调时钟
+
+
+def test_run_survives_poll_exceptions_and_keeps_heartbeat():
+    """串口持续异常:任务不死、每周期补发刹车保心跳、stop_event 正常退出。"""
+    now = int(time.monotonic() * 1000)
+    px4 = FakePx4(_valid_state(now))
+    pod = FlakyPod(fail_times=10**9)  # 永远失败
+    b = _run_bridge(px4, pod)
+
+    async def scenario():
+        stop = asyncio.Event()
+        task = asyncio.ensure_future(b.run(stop))
+        await asyncio.sleep(0.2)
+        stop.set()
+        await asyncio.wait_for(task, timeout=2.0)
+
+    asyncio.run(scenario())
+    assert len(px4.brake_calls) > 0       # 异常期间持续补发刹车
+    assert pod.fail_times > 0             # 循环确实在反复尝试(未提前退出)
+
+
+def test_run_recovers_after_transient_failure():
+    """串口短暂异常后恢复:异常期刹车,恢复后照常引导。"""
+    now = int(time.monotonic() * 1000)
+    px4 = FakePx4(_valid_state(now))
+    pod = FlakyPod(fail_times=3)
+    pod.queue(TargetGeo(latitude_deg=0.001), _locked_telemetry())
+    b = _run_bridge(px4, pod)
+
+    async def scenario():
+        stop = asyncio.Event()
+        task = asyncio.ensure_future(b.run(stop))
+        await asyncio.sleep(0.3)
+        stop.set()
+        await asyncio.wait_for(task, timeout=2.0)
+
+    asyncio.run(scenario())
+    assert len(px4.brake_calls) >= 3      # 3 次异常各补一发刹车
+    assert len(px4.velocity_calls) >= 1   # 恢复后进入正常引导
+    assert b._step_failures == 0          # 计数已复位

utils/tests/pod_bridge/config_test.py

@@ -0,0 +1,115 @@
+"""config_test.py —— 桥接配置加载与校验单元测试。"""
+
+import json
+from pathlib import Path
+
+import pytest
+
+from pod_bridge.config import (BridgeConfig, GuidanceConfig, PodSerialConfig,
+                               load_from_json)
+
+_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json"
+
+
+def _write(tmp_path, obj) -> str:
+    p = tmp_path / "cfg.json"
+    p.write_text(json.dumps(obj), encoding="utf-8")
+    return str(p)
+
+
+def test_shipped_default_config_loads():
+    cfg = load_from_json(str(_DEFAULT_CONFIG))
+    assert cfg.pod_serial.baudrate == 115200
+    assert cfg.px4.connection_string == "udp://:14540"
+    assert cfg.link.attitude_downlink_hz == pytest.approx(20.0)
+    assert cfg.guidance.max_speed_mps == pytest.approx(5.0)
+    assert cfg.guidance.stop_radius_m == pytest.approx(5.0)
+
+
+def test_empty_json_uses_all_defaults(tmp_path):
+    cfg = load_from_json(_write(tmp_path, {}))
+    default = BridgeConfig()
+    assert cfg.pod_serial == default.pod_serial
+    assert cfg.px4 == default.px4
+    assert cfg.link == default.link
+    assert cfg.guidance == default.guidance
+
+
+def test_partial_override_keeps_other_defaults(tmp_path):
+    cfg = load_from_json(_write(tmp_path, {
+        "pod_serial": {"port": "COM3"},
+        "guidance": {"max_speed_mps": 8.0},
+    }))
+    assert cfg.pod_serial.port == "COM3"
+    assert cfg.pod_serial.baudrate == 115200          # 默认回退
+    assert cfg.guidance.max_speed_mps == 8.0
+    assert cfg.guidance.stop_radius_m == GuidanceConfig().stop_radius_m  # 默认回退
+
+
+def test_full_override_roundtrip(tmp_path):
+    obj = {
+        "pod_serial": {"port": "/dev/ttyUSB0", "baudrate": 115200},
+        "px4": {"connection_string": "serial:///dev/ttyS1:57600"},
+        "link": {"attitude_downlink_hz": 30.0, "telemetry_timeout_ms": 800},
+        "guidance": {"frequency_hz": 20.0, "stop_radius_m": 3.0,
+                     "max_speed_mps": 7.5, "speed_kp": 0.8, "yaw_kp": 1.5},
+    }
+    cfg = load_from_json(_write(tmp_path, obj))
+    assert cfg.px4.connection_string == "serial:///dev/ttyS1:57600"
+    assert cfg.link.attitude_downlink_hz == 30.0
+    assert cfg.link.telemetry_timeout_ms == 800
+    assert cfg.guidance.frequency_hz == 20.0
+    assert cfg.guidance.yaw_kp == 1.5
+
+
+# --- 校验错误 ---------------------------------------------------------------
+def test_missing_file_raises():
+    with pytest.raises(ValueError, match="打开失败"):
+        load_from_json(str(_DEFAULT_CONFIG.parent / "no_such_file.json"))
+
+
+def test_non_object_root_raises(tmp_path):
+    p = tmp_path / "bad.json"
+    p.write_text("[1, 2, 3]", encoding="utf-8")
+    with pytest.raises(ValueError, match="根节点必须为对象"):
+        load_from_json(str(p))
+
+
+def test_malformed_json_raises(tmp_path):
+    p = tmp_path / "bad.json"
+    p.write_text("{not json", encoding="utf-8")
+    with pytest.raises(ValueError, match="解析失败"):
+        load_from_json(str(p))
+
+
+def test_wrong_type_raises_with_field_name(tmp_path):
+    with pytest.raises(ValueError, match="guidance.max_speed_mps"):
+        load_from_json(_write(tmp_path, {"guidance": {"max_speed_mps": "fast"}}))
+
+
+def test_unknown_key_rejected(tmp_path):
+    with pytest.raises(ValueError, match="未知字段"):
+        load_from_json(_write(tmp_path, {"guidance": {"max_speed": 5.0}}))
+
+
+def test_empty_connection_string_raises(tmp_path):
+    with pytest.raises(ValueError, match="px4.connection_string"):
+        load_from_json(_write(tmp_path, {"px4": {"connection_string": ""}}))
+
+
+@pytest.mark.parametrize("obj,field", [
+    ({"link": {"attitude_downlink_hz": 100.0}}, "link.attitude_downlink_hz"),  # >60
+    ({"link": {"attitude_downlink_hz": 0.5}}, "link.attitude_downlink_hz"),    # <1
+    ({"guidance": {"max_speed_mps": -1.0}}, "guidance.max_speed_mps"),         # 非正
+    ({"guidance": {"stop_radius_m": -2.0}}, "guidance.stop_radius_m"),         # 负
+    ({"guidance": {"speed_derivative_filter_coef": 1.0}},                      # 上界开
+     "guidance.speed_derivative_filter_coef"),
+    ({"pod_serial": {"baudrate": 0}}, "pod_serial.baudrate"),                  # 非正
+])
+def test_out_of_range_raises(tmp_path, obj, field):
+    with pytest.raises(ValueError, match=field.replace(".", r"\.")):
+        load_from_json(_write(tmp_path, obj))
+
+
+def test_pod_serial_default_port_is_placeholder():
+    assert PodSerialConfig().port == ""

utils/tests/pod_bridge/guidance_test.py

@@ -0,0 +1,152 @@
+"""guidance_test.py —— 地理位置引导律单元测试。
+
+覆盖：大地测量（方位/距离）、对准/未对准/背向目标的速率式输出、到达悬停、
+目标/位置超龄与不可用刹车、速度上限与加速度限幅渐变。
+"""
+
+import math
+
+import pytest
+
+from pod_bridge.guidance import (GuidanceConfig, GuidanceInput, GuidanceLaw,
+                                 haversine_distance_m, initial_bearing_deg)
+from visual_track_control.types import ControlMode
+
+_DEG_LAT_M = 111194.93  # 1° 纬度 ≈ 此米数（R=6371008.8）
+
+
+def _inp(own_lat=0.0, own_lon=0.0, own_yaw=0.0, tgt_lat=0.0, tgt_lon=0.0,
+         pos_valid=True, tgt_valid=True, pos_ms=1000, tgt_ms=1000):
+    return GuidanceInput(
+        own_lat_deg=own_lat, own_lon_deg=own_lon, own_yaw_deg=own_yaw,
+        own_position_valid=pos_valid, own_position_time_ms=pos_ms,
+        target_lat_deg=tgt_lat, target_lon_deg=tgt_lon,
+        target_valid=tgt_valid, target_time_ms=tgt_ms,
+    )
+
+
+# --- 大地测量 ---------------------------------------------------------------
+@pytest.mark.parametrize("tgt,expected", [
+    ((0.001, 0.0), 0.0),      # 正北
+    ((0.0, 0.001), 90.0),     # 正东（赤道）
+    ((-0.001, 0.0), 180.0),   # 正南
+    ((0.0, -0.001), 270.0),   # 正西
+])
+def test_initial_bearing_cardinals(tgt, expected):
+    b = initial_bearing_deg(0.0, 0.0, tgt[0], tgt[1])
+    # 0 与 360 等价，用回绕差判定
+    diff = abs((b - expected + 180.0) % 360.0 - 180.0)
+    assert diff < 0.5
+
+
+def test_haversine_distance_one_millidegree():
+    d = haversine_distance_m(0.0, 0.0, 0.001, 0.0)
+    assert d == pytest.approx(_DEG_LAT_M / 1000.0, abs=1.0)
+
+
+def test_haversine_zero_distance():
+    assert haversine_distance_m(31.2, 121.4, 31.2, 121.4) == pytest.approx(0.0, abs=1e-6)
+
+
+# --- 对准目标：前进、不偏航 -------------------------------------------------
+def test_aligned_target_moves_forward_no_yaw():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, own_yaw=0.0), now_ms=1000)  # 正北 111m，机头朝北
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.valid is True
+    assert out.vx_body_mps > 0.0
+    assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6)
+    assert out.vz_mps == 0.0
+    assert law.last_bearing_deg == pytest.approx(0.0, abs=0.5)
+
+
+def test_target_to_east_yaws_right_no_forward():
+    law = GuidanceLaw(GuidanceConfig())
+    # 目标正东（方位 90），机头朝北（0）→ 误差 +90：右转为正、未对准时前向门控为 0
+    out = law.update(_inp(tgt_lon=0.001, own_yaw=0.0), now_ms=1000)
+    assert out.yaw_rate_dps > 0.0
+    assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6)
+
+
+def test_facing_target_moves_forward():
+    law = GuidanceLaw(GuidanceConfig())
+    # 目标正东、机头也朝东（yaw=90）→ 已对准，前进不偏航
+    out = law.update(_inp(tgt_lon=0.001, own_yaw=90.0), now_ms=1000)
+    assert out.vx_body_mps > 0.0
+    assert out.yaw_rate_dps == pytest.approx(0.0, abs=1e-6)
+
+
+def test_target_behind_no_forward_and_turns():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=-0.001, own_yaw=0.0), now_ms=1000)  # 正南，机头朝北
+    assert out.vx_body_mps == pytest.approx(0.0, abs=1e-6)  # cos(180°)<0 门控为 0
+    assert abs(out.yaw_rate_dps) > 0.0
+
+
+# --- 到达 / 不可用 / 超龄 → 刹车 -------------------------------------------
+def test_arrived_within_stop_radius_brakes():
+    law = GuidanceLaw(GuidanceConfig(stop_radius_m=5.0))
+    near = 2.0 / _DEG_LAT_M  # ≈2m 纬度偏移
+    out = law.update(_inp(tgt_lat=near), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+    assert out.vx_body_mps == 0.0
+
+
+def test_stale_target_brakes():
+    law = GuidanceLaw(GuidanceConfig(target_stale_ms=1000))
+    out = law.update(_inp(tgt_lat=0.001, tgt_ms=1000), now_ms=5000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+def test_invalid_position_brakes():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, pos_valid=False), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+def test_invalid_target_brakes():
+    law = GuidanceLaw(GuidanceConfig())
+    out = law.update(_inp(tgt_lat=0.001, tgt_valid=False), now_ms=1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+
+
+# --- 速度上限与加速度限幅渐变 ----------------------------------------------
+def test_speed_ramps_up_and_clamps_to_max():
+    cfg = GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0,
+                         speed_slew_mps2=2.0, stop_radius_m=5.0)
+    law = GuidanceLaw(cfg)
+    forward = 0.0
+    now = 1000
+    for _ in range(60):  # 6s，足够加速度限幅爬到上限
+        out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=now, tgt_ms=now),
+                         now_ms=now)
+        forward = out.vx_body_mps
+        assert forward <= cfg.max_speed_mps + 1e-9
+        now += 100
+    assert forward == pytest.approx(cfg.max_speed_mps, abs=0.1)
+
+
+def test_first_command_is_slew_limited_not_full_speed():
+    law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, max_speed_mps=5.0,
+                                     speed_slew_mps2=2.0))
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)
+    # 首拍从零渐变：dt=0.1s，加速度上限 2 → 前向 ≤ 0.2，远小于 5
+    assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9
+
+
+def test_brake_then_recover_ramps_from_zero():
+    law = GuidanceLaw(GuidanceConfig(frequency_hz=10.0, speed_slew_mps2=2.0))
+    law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)   # 前进
+    law.update(_inp(tgt_lat=0.05, tgt_valid=False), now_ms=1100)  # 刹车，清状态
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1200)  # 恢复
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.vx_body_mps <= 0.2 + 1e-9  # 从零重新渐变
+
+
+def test_reset_clears_state():
+    law = GuidanceLaw(GuidanceConfig())
+    law.update(_inp(tgt_lat=0.05, own_yaw=0.0), now_ms=1000)
+    law.reset()
+    out = law.update(_inp(tgt_lat=0.05, own_yaw=0.0, pos_ms=2000, tgt_ms=2000),
+                     now_ms=2000)
+    assert out.vx_body_mps <= 0.2 + 1e-9  # 复位后首条仍从零渐变

utils/tests/pod_bridge/pod_serial_test.py

@@ -0,0 +1,89 @@
+"""pod_serial_test.py —— PodSerial 单元测试（注入 fake serial 模块，无硬件）。"""
+
+import sys
+import types
+
+import pytest
+
+from pod_bridge.pod_serial import PodLink, PodSerial
+from pod_bridge.protocol import TargetGeo, encode_target
+
+
+class _FakeSerialInstance:
+    def __init__(self, **kwargs):
+        self.kwargs = kwargs
+        self._buf = b""
+        self.written = []
+        self.closed = False
+
+    @property
+    def in_waiting(self):
+        return len(self._buf)
+
+    def read(self, n):
+        data, self._buf = self._buf[:n], self._buf[n:]
+        return data
+
+    def write(self, data):
+        self.written.append(data)
+
+    def close(self):
+        self.closed = True
+
+
+@pytest.fixture
+def fake_serial(monkeypatch):
+    mod = types.ModuleType("serial")
+    mod.EIGHTBITS = 8
+    mod.PARITY_NONE = "N"
+    mod.STOPBITS_ONE = 1
+    mod.Serial = _FakeSerialInstance
+    monkeypatch.setitem(sys.modules, "serial", mod)
+    return mod
+
+
+def test_empty_port_raises():
+    with pytest.raises(ValueError, match="port 为空"):
+        PodSerial("")
+
+
+def test_serial_opened_with_8n1(fake_serial):
+    ps = PodSerial("COM3", baudrate=115200)
+    kw = ps._ser.kwargs
+    assert kw["port"] == "COM3"
+    assert kw["baudrate"] == 115200
+    assert kw["bytesize"] == 8
+    assert kw["parity"] == "N"
+    assert kw["stopbits"] == 1
+    assert kw["timeout"] == 0  # 非阻塞轮询
+
+
+def test_poll_parses_buffered_frames(fake_serial):
+    ps = PodSerial("COM3")
+    ps._ser._buf = encode_target(TargetGeo(latitude_deg=31.0, longitude_deg=121.0))
+    frames = ps.poll()
+    assert len(frames) == 1
+    assert isinstance(frames[0], TargetGeo)
+    assert frames[0].latitude_deg == pytest.approx(31.0, abs=1e-7)
+
+
+def test_poll_empty_returns_empty_list(fake_serial):
+    ps = PodSerial("COM3")
+    assert ps.poll() == []
+
+
+def test_send_writes_bytes(fake_serial):
+    ps = PodSerial("COM3")
+    ps.send(b"\xeb\x90\x01")
+    assert ps._ser.written == [b"\xeb\x90\x01"]
+
+
+def test_close_closes_serial(fake_serial):
+    ps = PodSerial("COM3")
+    ps.close()
+    assert ps._ser.closed is True
+
+
+def test_satisfies_podlink_protocol(fake_serial):
+    ps = PodSerial("COM3")
+    assert isinstance(ps, PodLink)

utils/tests/pod_bridge/protocol_test.py

@@ -0,0 +1,222 @@
+"""protocol_test.py —— 光电吊舱协议编解码单元测试（规格：docs/光电吊舱控制协议G版-PB.docx）。
+
+覆盖：文档样例校验和、四帧 encode/decode 回环、量纲换算、遥测位域属性、
+分帧器（粘包/半帧/噪声前缀/坏校验恢复）、异常帧拒绝。
+"""
+
+import pytest
+
+from pod_bridge.protocol import (
+    CarrierAttitude,
+    ControlCode,
+    ControlCommand,
+    FrameParser,
+    Telemetry,
+    TargetGeo,
+    checksum,
+    decode_attitude,
+    decode_control,
+    decode_target,
+    decode_telemetry,
+    encode_attitude,
+    encode_control,
+    encode_target,
+    encode_telemetry,
+)
+
+
+# --- 校验和：文档第 589 行样例 ---------------------------------------------
+def test_checksum_matches_doc_example():
+    # 文档样例 eb 90 2b 00..00（15 字节 body）累加取低 8 位应为 0xa6
+    body = bytes.fromhex("eb902b000000000000000000000000")
+    assert len(body) == 15
+    assert checksum(body) == 0xA6
+
+
+def test_encode_control_center_matches_doc_bytes():
+    frame = encode_control(ControlCommand(code=ControlCode.CENTER))
+    assert frame == bytes.fromhex("eb902b000000000000000000000000a6")
+    assert len(frame) == 16
+
+
+# --- 表2-1 控制指令回环 -----------------------------------------------------
+def test_control_roundtrip_box_track():
+    cmd = ControlCommand(code=ControlCode.BOX_TRACK, param_x=-120, param_y=340,
+                         param3=2, file_save_mode=1, param_x2=500, param_y2=-600)
+    decoded = decode_control(encode_control(cmd))
+    assert decoded == cmd
+
+
+def test_control_signed_params_preserved():
+    cmd = ControlCommand(code=ControlCode.POINT_TRACK, param_x=-32768, param_y=32767)
+    decoded = decode_control(encode_control(cmd))
+    assert decoded.param_x == -32768
+    assert decoded.param_y == 32767
+
+
+# --- 表3 载机姿态回环（含量纲换算）----------------------------------------
+def test_attitude_roundtrip():
+    att = CarrierAttitude(
+        roll_deg=1.23, pitch_deg=-4.56, yaw_deg=179.99,
+        latitude_deg=31.2345678, longitude_deg=121.4737012,
+        abs_alt_m=123.4, rel_alt_m=56.8,
+        year=2026, month=9, day=22, hour=14, minute=30, second=15, centisecond=42,
+        airspeed_mps=15.5, ground_speed_mps=12.0,
+    )
+    d = decode_attitude(encode_attitude(att))
+    assert d.roll_deg == pytest.approx(1.23, abs=0.01)
+    assert d.pitch_deg == pytest.approx(-4.56, abs=0.01)
+    assert d.yaw_deg == pytest.approx(179.99, abs=0.01)
+    assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7)
+    assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7)
+    assert d.abs_alt_m == pytest.approx(123.4, abs=0.2)
+    assert d.rel_alt_m == pytest.approx(56.8, abs=0.2)
+    assert (d.year, d.month, d.day) == (2026, 9, 22)
+    assert (d.hour, d.minute, d.second, d.centisecond) == (14, 30, 15, 42)
+    assert d.airspeed_mps == pytest.approx(15.5, abs=0.5)
+    assert d.ground_speed_mps == pytest.approx(12.0, abs=0.5)
+
+
+def test_attitude_sync_and_length():
+    frame = encode_attitude(CarrierAttitude())
+    assert len(frame) == 32
+    assert frame[0] == 0xEB and frame[1] == 0x91
+
+
+# --- 表4 目标经纬度回环 -----------------------------------------------------
+def test_target_roundtrip():
+    tgt = TargetGeo(latitude_deg=31.2345678, longitude_deg=121.4737012,
+                    abs_alt_m=100.2, rel_alt_m=33.4,
+                    year=2026, month=9, day=22, hour=8, minute=5, second=30,
+                    centisecond=99)
+    d = decode_target(encode_target(tgt))
+    assert d.latitude_deg == pytest.approx(31.2345678, abs=1e-7)
+    assert d.longitude_deg == pytest.approx(121.4737012, abs=1e-7)
+    assert d.abs_alt_m == pytest.approx(100.2, abs=0.2)
+    assert d.rel_alt_m == pytest.approx(33.4, abs=0.2)
+    assert d.year == 2026 and d.centisecond == 99
+
+
+def test_target_negative_longitude():
+    tgt = TargetGeo(latitude_deg=-33.8688, longitude_deg=-70.6693)
+    d = decode_target(encode_target(tgt))
+    assert d.longitude_deg == pytest.approx(-70.6693, abs=1e-7)
+    assert d.latitude_deg == pytest.approx(-33.8688, abs=1e-7)
+
+
+# --- 表2-2 遥测：物理量 + 位域 ---------------------------------------------
+def test_telemetry_roundtrip_physical_values():
+    t = Telemetry(status1=0b10000100, status2=0b00101010, status3=0b01000000,
+                  off_target_x_deg=1.23, off_target_y_deg=-4.56,
+                  frame_roll_deg=10.0, frame_pitch_deg=-20.0, frame_yaw_deg=90.5,
+                  att_roll_deg=2.0, att_pitch_deg=-3.0,
+                  roll_rate_dps=1.5, pitch_rate_dps=-2.5, yaw_rate_dps=30.0,
+                  laser_range_m=123.4, self_check=0)
+    d = decode_telemetry(encode_telemetry(t))
+    assert d.off_target_x_deg == pytest.approx(1.23, abs=0.01)
+    assert d.off_target_y_deg == pytest.approx(-4.56, abs=0.01)
+    assert d.frame_yaw_deg == pytest.approx(90.5, abs=0.01)
+    assert d.yaw_rate_dps == pytest.approx(30.0, abs=0.01)
+    assert d.laser_range_m == pytest.approx(123.4, abs=0.1)
+
+
+def test_telemetry_laser_zero_is_none():
+    d = decode_telemetry(encode_telemetry(Telemetry(laser_range_m=None)))
+    assert d.laser_range_m is None
+
+
+def test_telemetry_bitfield_properties():
+    t = Telemetry(status1=0b11000100, status2=0b00101110, status3=0b01100000)
+    d = decode_telemetry(encode_telemetry(t))
+    assert d.track_source == 0b11          # 红外2
+    assert d.is_target_locked is True       # status1 Bit2 = 1
+    assert d.motor_on is True               # status2 Bit3 = 1
+    assert d.follow_on is True              # status2 Bit2 = 1
+    assert d.big_picture == 0b01
+    assert d.small_picture == 0b10
+
+
+# --- 异常帧拒绝 ------------------------------------------------------------
+def test_decode_rejects_bad_sync():
+    frame = bytearray(encode_control(ControlCommand(code=ControlCode.CENTER)))
+    frame[1] = 0x91  # 破坏同步字
+    with pytest.raises(ValueError, match="同步字"):
+        decode_control(bytes(frame))
+
+
+def test_decode_rejects_bad_checksum():
+    frame = bytearray(encode_target(TargetGeo(latitude_deg=10.0)))
+    frame[31] ^= 0xFF
+    with pytest.raises(ValueError, match="校验和"):
+        decode_target(bytes(frame))
+
+
+def test_decode_rejects_wrong_length():
+    with pytest.raises(ValueError, match="帧长"):
+        decode_telemetry(b"\xee\x16" + b"\x00" * 10)
+
+
+# --- 分帧器 ----------------------------------------------------------------
+def test_parser_single_frame():
+    parser = FrameParser()
+    frames = parser.feed(encode_target(TargetGeo(latitude_deg=31.0, longitude_deg=121.0)))
+    assert len(frames) == 1
+    assert isinstance(frames[0], TargetGeo)
+    assert frames[0].latitude_deg == pytest.approx(31.0, abs=1e-7)
+
+
+def test_parser_byte_by_byte_half_frame():
+    parser = FrameParser()
+    raw = encode_telemetry(Telemetry(laser_range_m=50.0))
+    collected = []
+    for i in range(len(raw)):
+        collected.extend(parser.feed(raw[i:i + 1]))
+    assert len(collected) == 1
+    assert collected[0].laser_range_m == pytest.approx(50.0, abs=0.1)
+
+
+def test_parser_concatenated_and_mixed_frames():
+    parser = FrameParser()
+    blob = (encode_telemetry(Telemetry(laser_range_m=10.0))
+            + encode_target(TargetGeo(latitude_deg=1.0))
+            + encode_telemetry(Telemetry(laser_range_m=20.0)))
+    frames = parser.feed(blob)
+    assert [type(f) for f in frames] == [Telemetry, TargetGeo, Telemetry]
+    assert frames[0].laser_range_m == pytest.approx(10.0, abs=0.1)
+    assert frames[2].laser_range_m == pytest.approx(20.0, abs=0.1)
+
+
+def test_parser_skips_noise_prefix():
+    parser = FrameParser()
+    blob = b"\x00\xff\x12\x34" + encode_target(TargetGeo(latitude_deg=5.0))
+    frames = parser.feed(blob)
+    assert len(frames) == 1
+    assert frames[0].latitude_deg == pytest.approx(5.0, abs=1e-7)
+
+
+def test_parser_recovers_from_bad_checksum():
+    parser = FrameParser()
+    bad = bytearray(encode_telemetry(Telemetry(laser_range_m=1.0)))
+    bad[31] ^= 0xFF  # 坏校验
+    good = encode_target(TargetGeo(latitude_deg=7.0))
+    frames = parser.feed(bytes(bad) + good)
+    # 坏帧被丢弃，紧随的好帧仍能解析
+    assert len(frames) == 1
+    assert isinstance(frames[0], TargetGeo)
+
+
+def test_parser_split_across_feeds():
+    parser = FrameParser()
+    raw = encode_target(TargetGeo(latitude_deg=42.0))
+    assert parser.feed(raw[:5]) == []      # 半帧，未输出
+    frames = parser.feed(raw[5:])
+    assert len(frames) == 1
+    assert frames[0].latitude_deg == pytest.approx(42.0, abs=1e-7)
+
+
+def test_parser_accepts_configured_kinds():
+    parser = FrameParser(kinds=((0xEB, 0x90),))  # 只识别控制帧（模拟器侧）
+    frames = parser.feed(encode_control(ControlCommand(code=ControlCode.CENTER)))
+    assert len(frames) == 1
+    assert isinstance(frames[0], ControlCommand)
+    assert frames[0].code == ControlCode.CENTER

utils/tests/pod_bridge/px4_gateway_test.py

@@ -0,0 +1,214 @@
+"""px4_gateway_test.py —— Px4Gateway 单元测试（mock 遥测流 + mock offboard）。
+
+覆盖：遥测缓存更新（position/attitude/gps/velocity）、position_valid 判据、
+快照副本隔离、时间戳注入、枚举转换、offboard 指令委托、异步订阅启停。
+"""
+
+import asyncio
+from enum import Enum
+from types import SimpleNamespace
+
+import pytest
+
+from pod_bridge.px4_gateway import OwnState, Px4Gateway, _enum_int
+
+
+class FakeOffboard:
+    def __init__(self):
+        self.body = []
+        self.ned = []
+        self.start_count = 0
+        self.stop_count = 0
+
+    async def set_velocity_body(self, sp):
+        self.body.append(sp)
+
+    async def set_velocity_ned(self, sp):
+        self.ned.append(sp)
+
+    async def start(self):
+        self.start_count += 1
+
+    async def stop(self):
+        self.stop_count += 1
+
+
+class FakeTelemetry:
+    """各遥测流：先吐预设消息，再挂起（由 stop_telemetry 取消）。"""
+
+    def __init__(self, position=None, attitude=None, gps=None, velocity=None):
+        self._position = position or []
+        self._attitude = attitude or []
+        self._gps = gps or []
+        self._velocity = velocity or []
+
+    async def _stream(self, msgs):
+        for m in msgs:
+            yield m
+        await asyncio.sleep(3600)
+
+    def position(self):
+        return self._stream(self._position)
+
+    def attitude_euler(self):
+        return self._stream(self._attitude)
+
+    def gps_info(self):
+        return self._stream(self._gps)
+
+    def velocity_body(self):
+        return self._stream(self._velocity)
+
+
+class FakeDrone:
+    def __init__(self, telemetry=None):
+        self.offboard = FakeOffboard()
+        self.telemetry = telemetry or FakeTelemetry()
+
+
+def _pos(lat=31.0, lon=121.0, aalt=100.0, ralt=50.0):
+    return SimpleNamespace(latitude_deg=lat, longitude_deg=lon,
+                           absolute_altitude_m=aalt, relative_altitude_m=ralt)
+
+
+def _att(roll=0.1, pitch=0.2, yaw=1.57):
+    return SimpleNamespace(roll_rad=roll, pitch_rad=pitch, yaw_rad=yaw)
+
+
+# --- 枚举转换 ---------------------------------------------------------------
+def test_enum_int_handles_enum_and_raw():
+    class Fix(Enum):
+        NO_FIX = 0
+        THREE_D = 3
+
+    assert _enum_int(Fix.THREE_D) == 3
+    assert _enum_int(3) == 3
+    assert _enum_int(None) == 0
+
+
+# --- 缓存更新 ---------------------------------------------------------------
+def test_update_position_sets_fields_but_not_valid_without_gps():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_position(_pos())
+    s = gw.own_state()
+    assert s.latitude_deg == 31.0 and s.longitude_deg == 121.0
+    assert s.absolute_altitude_m == 100.0 and s.relative_altitude_m == 50.0
+    assert s.has_position is True
+    assert s.position_valid is False  # 尚无 GPS 定位
+
+
+def test_position_valid_requires_3d_fix():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_position(_pos())
+    gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=12))
+    s = gw.own_state()
+    assert s.position_valid is True
+    assert s.num_satellites == 12
+    # 定位降级 → 立即失效
+    gw.update_gps(SimpleNamespace(fix_type=2, num_satellites=6))
+    assert gw.own_state().position_valid is False
+
+
+def test_update_attitude_sets_radians():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_attitude(_att())
+    s = gw.own_state()
+    assert s.has_attitude is True
+    assert s.yaw_rad == pytest.approx(1.57)
+
+
+def test_update_velocity_body_ground_speed_is_horizontal_magnitude():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_velocity_body(SimpleNamespace(forward_m_s=3.0, right_m_s=4.0,
+                                            down_m_s=-9.9))
+    assert gw.own_state().ground_speed_mps == pytest.approx(5.0)  # 不含 down
+
+
+def test_timestamp_uses_injected_clock():
+    clock = [1000]
+    gw = Px4Gateway(FakeDrone(), now_ms_fn=lambda: clock[0])
+    gw.update_position(_pos())
+    assert gw.own_state().time_ms == 1000
+    assert gw.own_state().position_time_ms == 1000
+    clock[0] = 1500
+    gw.update_attitude(_att())
+    assert gw.own_state().time_ms == 1500
+    assert gw.own_state().position_time_ms == 1000  # 姿态流不得代刷位置时间戳
+
+
+def test_own_state_returns_isolated_copy():
+    gw = Px4Gateway(FakeDrone())
+    gw.update_position(_pos(lat=10.0))
+    snap = gw.own_state()
+    snap.latitude_deg = 999.0
+    assert gw.own_state().latitude_deg == 10.0  # 内部状态未被外部副本修改
+
+
+# --- offboard 指令委托 ------------------------------------------------------
+def test_command_delegation_reaches_offboard():
+    drone = FakeDrone()
+    gw = Px4Gateway(drone)
+
+    async def scenario():
+        assert await gw.start_offboard()
+        out = SimpleNamespace(yaw_rate_dps=5.0, vx_body_mps=1.5, vz_mps=0.0,
+                              vx_mps=0.0, vy_mps=0.0, yaw_deg=0.0)
+        assert await gw.send_velocity_heading(out, 1)
+        assert await gw.send_brake_hover(2)
+        await gw.stop_offboard()
+
+    asyncio.run(scenario())
+    assert drone.offboard.start_count == 1
+    assert len(drone.offboard.body) >= 2
+    assert drone.offboard.stop_count == 1
+
+
+# --- 异步订阅启停 -----------------------------------------------------------
+def test_start_telemetry_populates_cache_then_stop():
+    telemetry = FakeTelemetry(
+        position=[_pos(lat=22.0, lon=114.0)],
+        attitude=[_att(yaw=0.5)],
+        gps=[SimpleNamespace(fix_type=3, num_satellites=10)],
+        velocity=[SimpleNamespace(forward_m_s=2.0, right_m_s=0.0, down_m_s=0.0)],
+    )
+    gw = Px4Gateway(FakeDrone(telemetry))
+
+    async def scenario():
+        await gw.start_telemetry()
+        await asyncio.sleep(0.05)  # 让后台任务消费预设消息
+        s = gw.own_state()
+        assert s.latitude_deg == 22.0 and s.longitude_deg == 114.0
+        assert s.yaw_rad == pytest.approx(0.5)
+        assert s.position_valid is True
+        assert s.ground_speed_mps == pytest.approx(2.0)
+        await gw.stop_telemetry()
+        assert gw._tasks == []
+
+    asyncio.run(scenario())
+
+
+def test_default_own_state_is_invalid():
+    s = OwnState()
+    assert s.has_position is False and s.position_valid is False
+    assert s.time_ms == 0 and s.position_time_ms == 0
+
+
+
+def test_position_timestamp_not_refreshed_by_other_streams():
+    """缺陷①回归:position 流断更后,attitude/gps/velocity 高频流继续刷新
+    time_ms 时,position_time_ms 必须保持原值(位置超龄判定才有效)。"""
+    clock = [1000]
+    gw = Px4Gateway(FakeDrone(), now_ms_fn=lambda: clock[0])
+    gw.update_position(_pos())
+    assert gw.own_state().position_time_ms == 1000
+    clock[0] = 2000
+    gw.update_attitude(_att())
+    gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=9))
+    gw.update_velocity_body(SimpleNamespace(forward_m_s=1.0, right_m_s=0.0,
+                                            down_m_s=0.0))
+    s = gw.own_state()
+    assert s.time_ms == 2000            # 诊断时间戳照常刷新
+    assert s.position_time_ms == 1000   # 位置时间戳停留 → 超龄判定可触发
+    clock[0] = 2500
+    gw.update_position(_pos(lat=32.0))  # position 流恢复 → 时间戳跟进
+    assert gw.own_state().position_time_ms == 2500