添加无线模块
This commit is contained in:
minghanchen
@@ -0,0 +1,244 @@
|
||||
"""Persistent read-only CH1 vehicle-state receiver for the Linux ZLG backend."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from collections.abc import Callable
|
||||
from copy import deepcopy
|
||||
from dataclasses import dataclass, replace
|
||||
from datetime import UTC, datetime
|
||||
from typing import Any
|
||||
|
||||
from cognitive_agent.application.native_can_feedback import decode_feedback
|
||||
from cognitive_agent.infrastructure.can.gateway import CanFrame, CanGatewayError
|
||||
from cognitive_agent.infrastructure.can.hardware import VehicleCanSettings
|
||||
|
||||
REQUIRED_SEAT_IDS = frozenset({0x4FA, 0x494})
|
||||
REQUIRED_BODY_IDS = frozenset({0x342, 0x4EB, 0x407})
|
||||
REQUIRED_UI_IDS = REQUIRED_SEAT_IDS | REQUIRED_BODY_IDS
|
||||
OBSERVABLE_IDS = REQUIRED_UI_IDS | frozenset(
|
||||
{0x388, 0x3F3, 0x3FF, 0x404, 0x41A, 0x42E}
|
||||
)
|
||||
PERSISTENT_MONITOR_POLL_INTERVAL_MS = 500
|
||||
|
||||
|
||||
def _now() -> str:
|
||||
return datetime.now(UTC).isoformat().replace("+00:00", "Z")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class VehicleStateSnapshot:
|
||||
feedback_states: tuple[dict[str, Any], ...]
|
||||
observed_ids: frozenset[int]
|
||||
captured_at: str | None
|
||||
capture_sequence: int
|
||||
|
||||
|
||||
class VehicleStateMonitor:
|
||||
"""Keep one monitor-only adapter owner online and cache fresh decoded state."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
settings: VehicleCanSettings,
|
||||
*,
|
||||
decoder: Callable[[CanFrame], dict[str, Any]] = decode_feedback,
|
||||
reconnect_delay_s: float = 1.0,
|
||||
) -> None:
|
||||
self.settings = settings
|
||||
self.decoder = decoder
|
||||
self.reconnect_delay_s = max(0.01, float(reconnect_delay_s))
|
||||
self.initial_snapshot_timeout_s = max(0, settings.feedback_timeout_ms) / 1000
|
||||
self.freshness_s = max(3.0, self.initial_snapshot_timeout_s * 2)
|
||||
|
||||
self._condition = threading.Condition(threading.RLock())
|
||||
self._stop_event = threading.Event()
|
||||
self._thread: threading.Thread | None = None
|
||||
self._bridge: Any | None = None
|
||||
self._latest: dict[int, tuple[float, dict[str, Any]]] = {}
|
||||
self._baseline_ready = False
|
||||
self._capture_sequence = 0
|
||||
self._state = "idle"
|
||||
self._last_error: str | None = None
|
||||
|
||||
def start(self) -> None:
|
||||
with self._condition:
|
||||
if self._thread is not None and self._thread.is_alive():
|
||||
return
|
||||
self._stop_event.clear()
|
||||
self._state = "connecting"
|
||||
self._thread = threading.Thread(
|
||||
target=self._run,
|
||||
name="zlg-linux-vehicle-state-monitor",
|
||||
daemon=True,
|
||||
)
|
||||
self._thread.start()
|
||||
|
||||
def close(self) -> None:
|
||||
self._stop_event.set()
|
||||
with self._condition:
|
||||
self._condition.notify_all()
|
||||
thread = self._thread
|
||||
if thread is not None and thread is not threading.current_thread():
|
||||
thread.join(timeout=2.0)
|
||||
with self._condition:
|
||||
bridge = self._bridge
|
||||
if thread is not None and thread.is_alive() and bridge is not None:
|
||||
bridge.close()
|
||||
thread.join(timeout=1.0)
|
||||
with self._condition:
|
||||
if self._thread is None or not self._thread.is_alive():
|
||||
self._state = "stopped"
|
||||
self._thread = None
|
||||
self._condition.notify_all()
|
||||
|
||||
def snapshot(self) -> VehicleStateSnapshot:
|
||||
"""Return cached fresh state, waiting only for the first required baseline."""
|
||||
|
||||
self.start()
|
||||
deadline = time.monotonic() + self.initial_snapshot_timeout_s
|
||||
with self._condition:
|
||||
while not self._baseline_ready:
|
||||
fresh_ids = self._fresh_ids(time.monotonic())
|
||||
if REQUIRED_UI_IDS <= fresh_ids:
|
||||
self._baseline_ready = True
|
||||
break
|
||||
if self._state == "error" and not fresh_ids:
|
||||
raise CanGatewayError(
|
||||
self._last_error or "Linux vehicle-state monitor is unavailable"
|
||||
)
|
||||
remaining_s = deadline - time.monotonic()
|
||||
if remaining_s <= 0 or self._stop_event.is_set():
|
||||
break
|
||||
self._condition.wait(timeout=remaining_s)
|
||||
|
||||
now = time.monotonic()
|
||||
observed_ids = self._fresh_ids(now)
|
||||
if self._state == "error" and not observed_ids:
|
||||
raise CanGatewayError(
|
||||
self._last_error or "Linux vehicle-state monitor is unavailable"
|
||||
)
|
||||
feedback_states = tuple(
|
||||
deepcopy(self._latest[frame_id][1])
|
||||
for frame_id in sorted(observed_ids)
|
||||
)
|
||||
captured_at = max(
|
||||
(
|
||||
str(state.get("captured_at"))
|
||||
for state in feedback_states
|
||||
if state.get("captured_at")
|
||||
),
|
||||
default=None,
|
||||
)
|
||||
return VehicleStateSnapshot(
|
||||
feedback_states=feedback_states,
|
||||
observed_ids=frozenset(observed_ids),
|
||||
captured_at=captured_at,
|
||||
capture_sequence=self._capture_sequence,
|
||||
)
|
||||
|
||||
def public_info(self) -> dict[str, Any]:
|
||||
with self._condition:
|
||||
return {
|
||||
"mode": "persistent_monitor",
|
||||
"state": self._state,
|
||||
"read_only": True,
|
||||
"poll_interval_ms": PERSISTENT_MONITOR_POLL_INTERVAL_MS,
|
||||
"snapshot_freshness_ms": round(self.freshness_s * 1000),
|
||||
"capture_sequence": self._capture_sequence,
|
||||
"observed_frame_ids": [
|
||||
f"0x{frame_id:03X}"
|
||||
for frame_id in sorted(self._fresh_ids(time.monotonic()))
|
||||
],
|
||||
"last_error": self._last_error,
|
||||
}
|
||||
|
||||
def _fresh_ids(self, now: float) -> set[int]:
|
||||
return {
|
||||
frame_id
|
||||
for frame_id, (captured_monotonic, _) in self._latest.items()
|
||||
if now - captured_monotonic <= self.freshness_s
|
||||
}
|
||||
|
||||
def _record(self, frame: CanFrame) -> None:
|
||||
if (
|
||||
frame.channel != self.settings.channel
|
||||
or frame.arbitration_id not in OBSERVABLE_IDS
|
||||
or len(frame.data) < 8
|
||||
):
|
||||
return
|
||||
decoded = self.decoder(frame)
|
||||
if not any(
|
||||
key in decoded
|
||||
for key in ("actuator_state", "domain_state", "vehicle_safety")
|
||||
):
|
||||
# 0x4EB is multiplexed; only subtype 0x07 contains verified speed.
|
||||
return
|
||||
frame_id = str(decoded.get("frame_id", ""))
|
||||
decoded.update(
|
||||
{
|
||||
"valid": True,
|
||||
"feedback_frame_id": frame_id,
|
||||
"captured_at": _now(),
|
||||
"verification": {
|
||||
"authoritative": True,
|
||||
"channel": 1,
|
||||
"simulated": False,
|
||||
"read_only": True,
|
||||
"frame_id": frame_id,
|
||||
"payload_size": len(frame.data),
|
||||
},
|
||||
}
|
||||
)
|
||||
with self._condition:
|
||||
self._capture_sequence += 1
|
||||
self._latest[frame.arbitration_id] = (time.monotonic(), decoded)
|
||||
if REQUIRED_UI_IDS <= self._fresh_ids(time.monotonic()):
|
||||
self._baseline_ready = True
|
||||
self._condition.notify_all()
|
||||
|
||||
def _run(self) -> None:
|
||||
while not self._stop_event.is_set():
|
||||
bridge = None
|
||||
try:
|
||||
with self._condition:
|
||||
self._state = "connecting"
|
||||
self._condition.notify_all()
|
||||
channel_config = replace(
|
||||
self.settings.channel_config(),
|
||||
receive_ids=tuple(sorted(OBSERVABLE_IDS)),
|
||||
)
|
||||
bridge = self.settings.bridge(
|
||||
[channel_config],
|
||||
monitor_only=True,
|
||||
)
|
||||
bus = bridge.channel_bus(self.settings.channel)
|
||||
with self._condition:
|
||||
self._bridge = bridge
|
||||
self._latest.clear()
|
||||
self._baseline_ready = False
|
||||
self._last_error = None
|
||||
self._state = "online"
|
||||
self._condition.notify_all()
|
||||
while not self._stop_event.is_set():
|
||||
frame = bus.receive(100, include_fd=True)
|
||||
if frame is not None:
|
||||
self._record(frame)
|
||||
except Exception as exc: # noqa: BLE001 - normalize vendor/runtime failures
|
||||
if not self._stop_event.is_set():
|
||||
with self._condition:
|
||||
self._state = "error"
|
||||
self._last_error = str(exc)
|
||||
self._condition.notify_all()
|
||||
finally:
|
||||
with self._condition:
|
||||
if self._bridge is bridge:
|
||||
self._bridge = None
|
||||
if bridge is not None:
|
||||
bridge.close()
|
||||
if not self._stop_event.wait(self.reconnect_delay_s):
|
||||
continue
|
||||
|
||||
with self._condition:
|
||||
self._state = "stopped"
|
||||
self._condition.notify_all()
|
||||
Reference in New Issue
Block a user