minghanchen
1000 lines
39 KiB
Python
1000 lines
39 KiB
Python
"""MQTT transport for relaying validated raw CAN0/CAN1 frames.
|
|
|
|
The MQTT topics carry CAN frames, never decoded vehicle-state patches. Every
|
|
incoming frame is therefore subjected to the same allow-list and byte decoder
|
|
as a frame received from the wired ZLG adapter.
|
|
|
|
CAN0 is the classic-CAN passenger-seat protocol defined by
|
|
``/home/orin/project511/seat-code-file/seat-ota`` and remains raw for the seat
|
|
domain decoder. CAN1 is the rest-of-vehicle protocol defined by
|
|
``/home/orin/project511/project-code/canfd/CAN矩阵表_filled.xlsx`` and is
|
|
decoded by :mod:`cognitive_agent.application.native_can_feedback`.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
import math
|
|
import threading
|
|
import time
|
|
import uuid
|
|
from collections import deque
|
|
from collections.abc import Callable
|
|
from copy import deepcopy
|
|
from datetime import UTC, datetime
|
|
from typing import Any
|
|
|
|
from cognitive_agent.application.native_can_feedback import decode_feedback
|
|
from cognitive_agent.application.vehicle_state_monitor import (
|
|
OBSERVABLE_IDS,
|
|
VehicleStateSnapshot,
|
|
)
|
|
from cognitive_agent.infrastructure.can.gateway import (
|
|
CanFrame,
|
|
CanGatewayError,
|
|
)
|
|
from cognitive_agent.infrastructure.can.hardware import VehicleMqttSettings
|
|
|
|
MQTT_CAN_SCHEMA = "n7.can.frame.v1"
|
|
MQTT_POLL_INTERVAL_MS = 500
|
|
WIRELESS_MQTT_CH1_SOURCE = "wireless_mqtt_ch1"
|
|
MQTT_VALID_FD_PAYLOAD_LENGTHS = frozenset(
|
|
{*range(9), 12, 16, 20, 24, 32, 48, 64}
|
|
)
|
|
MQTT_SAFETY_FEEDBACK_IDS = frozenset({0x342, 0x4EB})
|
|
MQTT_VEHICLE_FEEDBACK_IDS = OBSERVABLE_IDS | frozenset({0x2F5, 0x37B})
|
|
# CAN0 accepts both state formats currently emitted by the passenger-seat
|
|
# stack. The production firmware publishes its authoritative classic-CAN
|
|
# status directly on 0x301..0x306/0x4FA. Older head-unit builds expose the
|
|
# read-only seat-ota request/response exchange on 0x6A0/0x6A2; that exchange is
|
|
# retained and translated to an internal 0x302 position frame below.
|
|
MQTT_SEAT_NATIVE_FEEDBACK_IDS = frozenset(
|
|
{0x301, 0x302, 0x303, 0x304, 0x305, 0x306, 0x4FA}
|
|
)
|
|
MQTT_SEAT_WIRE_FEEDBACK_IDS = MQTT_SEAT_NATIVE_FEEDBACK_IDS | frozenset(
|
|
{0x6A0, 0x6A2}
|
|
)
|
|
MQTT_SEAT_FEEDBACK_IDS = MQTT_SEAT_NATIVE_FEEDBACK_IDS | frozenset({0x6A2})
|
|
MQTT_ACCEPTED_FEEDBACK_IDS = {
|
|
0: MQTT_SEAT_FEEDBACK_IDS,
|
|
1: MQTT_VEHICLE_FEEDBACK_IDS,
|
|
}
|
|
MQTT_APPROVED_CONTROL_IDS = {
|
|
0: frozenset({0x29E, 0x3CE, 0x6A0}),
|
|
1: frozenset({0x1DE, 0x32B, 0x3FC, 0x431, 0x4C1, 0x4EF, 0x4F5}),
|
|
}
|
|
MQTT_OTA_SEAT_RUNTIME_OPCODES = frozenset({0x63, 0x64, 0x65, 0x66})
|
|
_ID_KEYS = ("arbitration_id", "can_id", "canId", "frame_id", "frameId", "id")
|
|
_DATA_KEYS = ("data_hex", "dataHex", "data", "payload", "bytes")
|
|
|
|
|
|
def _now() -> str:
|
|
return datetime.now(UTC).isoformat().replace("+00:00", "Z")
|
|
|
|
|
|
def _parse_frame_id(value: Any) -> int:
|
|
if isinstance(value, bool):
|
|
raise ValueError("CAN frame ID must not be boolean") # noqa: TRY004
|
|
if isinstance(value, int):
|
|
frame_id = value
|
|
elif isinstance(value, str):
|
|
raw = value.strip()
|
|
if not raw:
|
|
raise ValueError("CAN frame ID is empty")
|
|
if raw.lower().startswith("0x") or any(character in "abcdefABCDEF" for character in raw):
|
|
frame_id = int(raw, 16)
|
|
else:
|
|
frame_id = int(raw, 10)
|
|
else:
|
|
raise ValueError("CAN frame ID must be an integer or string") # noqa: TRY004
|
|
if not 0 <= frame_id <= 0x7FF:
|
|
raise ValueError("only 11-bit CAN frame IDs are accepted")
|
|
return frame_id
|
|
|
|
|
|
def _parse_data(value: Any) -> bytes:
|
|
if isinstance(value, list):
|
|
if not value or len(value) > 64:
|
|
raise ValueError("CAN data must contain between 1 and 64 bytes")
|
|
if any(
|
|
isinstance(item, bool) or not isinstance(item, int) or not 0 <= item <= 255
|
|
for item in value
|
|
):
|
|
raise ValueError("CAN data array must contain only byte integers")
|
|
return bytes(value)
|
|
if not isinstance(value, str):
|
|
raise ValueError("CAN data must be a hex string or byte array") # noqa: TRY004
|
|
compact = value.strip().replace("0x", "").replace("0X", "")
|
|
for separator in (" ", ":", "-", ",", "_"):
|
|
compact = compact.replace(separator, "")
|
|
if not compact or len(compact) % 2 or len(compact) > 128:
|
|
raise ValueError("CAN data hex must encode between 1 and 64 bytes")
|
|
try:
|
|
return bytes.fromhex(compact)
|
|
except ValueError as exc:
|
|
raise ValueError("CAN data contains invalid hexadecimal bytes") from exc
|
|
|
|
|
|
def _parse_channel(value: Any) -> int:
|
|
if isinstance(value, bool):
|
|
raise ValueError("CAN channel must not be boolean") # noqa: TRY004
|
|
if isinstance(value, str):
|
|
raw = value.strip().lower().removeprefix("can")
|
|
value = raw
|
|
try:
|
|
return int(value)
|
|
except (TypeError, ValueError) as exc:
|
|
raise ValueError("CAN channel must be an integer or can1-style name") from exc
|
|
|
|
|
|
def _parse_one_frame(value: Any, *, default_channel: int) -> CanFrame:
|
|
if not isinstance(value, dict):
|
|
raise ValueError("each MQTT CAN frame must be a JSON object") # noqa: TRY004
|
|
frame_id_value = next((value[key] for key in _ID_KEYS if key in value), None)
|
|
data_value = next((value[key] for key in _DATA_KEYS if key in value), None)
|
|
if frame_id_value is None or data_value is None:
|
|
raise ValueError("MQTT CAN frame requires an ID and data")
|
|
frame_id = _parse_frame_id(frame_id_value)
|
|
data = _parse_data(data_value)
|
|
channel_values = [
|
|
value[key]
|
|
for key in ("channelIndex", "channel_index", "channel")
|
|
if key in value
|
|
] or [default_channel]
|
|
channels = {_parse_channel(channel_value) for channel_value in channel_values}
|
|
if len(channels) != 1:
|
|
raise ValueError("MQTT CAN frame channel declarations conflict")
|
|
channel = channels.pop()
|
|
if channel not in MQTT_ACCEPTED_FEEDBACK_IDS:
|
|
raise ValueError("wireless CAN state accepts only CAN0 or CAN1 frames")
|
|
|
|
frame_type = str(value.get("frameType", value.get("frame_type", ""))).strip().lower()
|
|
is_fd = bool(
|
|
value.get(
|
|
"is_fd",
|
|
value.get(
|
|
"isFd",
|
|
frame_type in {"fd", "canfd", "can_fd"} if frame_type else len(data) > 8,
|
|
),
|
|
)
|
|
)
|
|
bitrate_switch = bool(
|
|
value.get("bitrate_switch", value.get("bitrateSwitch", value.get("brs", False)))
|
|
)
|
|
if not is_fd and len(data) > 8:
|
|
raise ValueError("classic CAN data must not exceed 8 bytes")
|
|
if channel == 0 and (is_fd or bitrate_switch or len(data) != 8):
|
|
raise ValueError(
|
|
"CAN0 passenger-seat state requires classic CAN, no BRS, and DLC 8"
|
|
)
|
|
timestamp_value = value.get("timestamp", value.get("timestamp_s"))
|
|
timestamp: float | None = None
|
|
if timestamp_value is not None:
|
|
if isinstance(timestamp_value, bool):
|
|
raise ValueError("CAN timestamp must not be boolean")
|
|
try:
|
|
timestamp = float(timestamp_value)
|
|
except (TypeError, ValueError) as exc:
|
|
raise ValueError("CAN timestamp must be numeric") from exc
|
|
if not math.isfinite(timestamp) or timestamp < 0:
|
|
raise ValueError("CAN timestamp must be a finite non-negative number")
|
|
# n7-can-signals/v1 uses Unix milliseconds while the canonical frame
|
|
# contract uses Unix seconds.
|
|
if timestamp >= 100_000_000_000:
|
|
timestamp /= 1_000
|
|
return CanFrame(
|
|
arbitration_id=frame_id,
|
|
data=data,
|
|
timestamp=timestamp,
|
|
channel=channel,
|
|
is_fd=is_fd,
|
|
bitrate_switch=bitrate_switch,
|
|
)
|
|
|
|
|
|
def _ota_seat_status_frames(frames: list[CanFrame]) -> list[CanFrame]:
|
|
"""Translate read-only 0x6A0/0x6A2 seat telemetry into 0x302 states.
|
|
|
|
The seat-ota runtime returns a selector value in a generic response frame:
|
|
0x6A0 is ``C7 sequence 60 selector ...`` and 0x6A2 is
|
|
``7C sequence 60 status uint32-le``. Only the non-mutating SEAT_INFO
|
|
opcode is paired here. The original 0x6A2 response remains available to
|
|
command-response waiters; the generated 0x302 frame is internal only.
|
|
"""
|
|
|
|
selectors_by_request: dict[tuple[int, int], int] = {}
|
|
for frame in frames:
|
|
if (
|
|
frame.channel == 0
|
|
and frame.arbitration_id == 0x6A0
|
|
and len(frame.data) == 8
|
|
and frame.data[0] == 0xC7
|
|
and frame.data[2] == 0x60
|
|
and frame.data[3] <= 14
|
|
):
|
|
selectors_by_request[(frame.data[1], frame.data[2])] = frame.data[3]
|
|
|
|
values: dict[int, tuple[int, CanFrame]] = {}
|
|
for frame in frames:
|
|
if (
|
|
frame.channel != 0
|
|
or frame.arbitration_id != 0x6A2
|
|
or len(frame.data) != 8
|
|
or frame.data[0] != 0x7C
|
|
or frame.data[2] != 0x60
|
|
or frame.data[3] != 0x00
|
|
):
|
|
continue
|
|
selector = selectors_by_request.get((frame.data[1], frame.data[2]))
|
|
if selector is None:
|
|
continue
|
|
values[selector] = (int.from_bytes(frame.data[4:8], "little"), frame)
|
|
|
|
translated: list[CanFrame] = []
|
|
for axis in range(4):
|
|
position_entry = values.get(1 + axis)
|
|
flags_entry = values.get(5 + axis)
|
|
if position_entry is None:
|
|
continue
|
|
packed_position, position_frame = position_entry
|
|
position = packed_position & 0xFFFF
|
|
hall_range = (packed_position >> 16) & 0xFFFF
|
|
if hall_range <= 0 or position > hall_range:
|
|
continue
|
|
value_x100 = min(10_000, round(position * 10_000 / hall_range))
|
|
if flags_entry is None:
|
|
# The production Android poller intentionally requests selectors
|
|
# 1..4 only. A valid position/range pair is sufficient to expose
|
|
# position, but it does not prove motion or a driver fault.
|
|
flags = 0
|
|
flags_frame = position_frame
|
|
moving = False
|
|
position_valid = True
|
|
driver_fault = False
|
|
direction = 0
|
|
else:
|
|
flags, flags_frame = flags_entry
|
|
moving = bool(flags & (1 << 1))
|
|
position_valid = bool(flags & (1 << 0))
|
|
driver_fault = bool(flags & (1 << 3))
|
|
direction = (flags >> 8) & 0x03
|
|
task_state = 1 if moving else 0
|
|
result = 7 if driver_fault else 0
|
|
state_flags = direction
|
|
if moving:
|
|
state_flags |= 1 << 2
|
|
if position_valid:
|
|
state_flags |= 1 << 3
|
|
if position == 0:
|
|
state_flags |= 1 << 4
|
|
if position == hall_range:
|
|
state_flags |= 1 << 5
|
|
if driver_fault:
|
|
state_flags |= 1 << 7
|
|
sequence = flags_frame.data[1]
|
|
payload = bytes(
|
|
(
|
|
axis,
|
|
task_state | (result << 4),
|
|
state_flags,
|
|
sequence,
|
|
0,
|
|
value_x100 & 0xFF,
|
|
(value_x100 >> 8) & 0xFF,
|
|
0,
|
|
)
|
|
)
|
|
translated.append(
|
|
CanFrame(
|
|
arbitration_id=0x302,
|
|
data=payload,
|
|
timestamp=max(
|
|
value
|
|
for value in (position_frame.timestamp, flags_frame.timestamp)
|
|
if value is not None
|
|
)
|
|
if position_frame.timestamp is not None
|
|
or flags_frame.timestamp is not None
|
|
else None,
|
|
channel=0,
|
|
is_fd=False,
|
|
bitrate_switch=False,
|
|
)
|
|
)
|
|
return translated
|
|
|
|
|
|
def parse_mqtt_can_payload(payload: bytes | str) -> list[CanFrame]:
|
|
"""Parse one frame or a bounded frame batch from the condition topic."""
|
|
|
|
if isinstance(payload, bytes):
|
|
try:
|
|
text = payload.decode("utf-8")
|
|
except UnicodeDecodeError as exc:
|
|
raise ValueError("MQTT CAN payload is not UTF-8") from exc
|
|
else:
|
|
text = payload
|
|
if len(text.encode("utf-8")) > 256 * 1024:
|
|
raise ValueError("MQTT CAN payload exceeds 256 KiB")
|
|
try:
|
|
decoded = json.loads(text)
|
|
except json.JSONDecodeError as exc:
|
|
raise ValueError("MQTT CAN payload must be JSON") from exc
|
|
|
|
batches: list[tuple[Any, int]]
|
|
if isinstance(decoded, dict) and "frames" in decoded:
|
|
batches = [(decoded["frames"], 1)]
|
|
elif isinstance(decoded, dict) and "signals" in decoded:
|
|
# Vehicle-side n7-can-signals/v1 envelope. Its decoded ``parsed``
|
|
# summary is intentionally ignored: raw frames still pass through the
|
|
# same per-channel allow-list and decoders as wired traffic. When the
|
|
# top-level batch is present, nested copies are intentionally ignored.
|
|
batches = [(decoded["signals"], 1)]
|
|
elif isinstance(decoded, dict) and any(
|
|
isinstance(decoded.get(key), dict) and "signals" in decoded[key]
|
|
for key in ("can0Seat", "can1Vehicle")
|
|
):
|
|
batches = []
|
|
for key, default_channel in (("can0Seat", 0), ("can1Vehicle", 1)):
|
|
block = decoded.get(key)
|
|
if isinstance(block, dict) and "signals" in block:
|
|
batches.append((block["signals"], default_channel))
|
|
elif isinstance(decoded, list):
|
|
batches = [(decoded, 1)]
|
|
else:
|
|
batches = [([decoded], 1)]
|
|
if any(not isinstance(items, list) for items, _ in batches):
|
|
raise ValueError("MQTT CAN frame batch must be an array")
|
|
item_count = sum(len(items) for items, _ in batches)
|
|
if not 1 <= item_count <= 256:
|
|
raise ValueError("MQTT CAN frame batch must contain between 1 and 256 frames")
|
|
parsed_frames = [
|
|
_parse_one_frame(item, default_channel=default_channel)
|
|
for items, default_channel in batches
|
|
for item in items
|
|
]
|
|
frames = [
|
|
frame
|
|
for frame in parsed_frames
|
|
if frame.arbitration_id
|
|
in (
|
|
MQTT_SEAT_WIRE_FEEDBACK_IDS
|
|
if frame.channel == 0
|
|
else MQTT_VEHICLE_FEEDBACK_IDS
|
|
)
|
|
]
|
|
return [*frames, *_ota_seat_status_frames(frames)]
|
|
|
|
|
|
def _validate_headunit_command_frame(frame: CanFrame) -> None:
|
|
"""Mirror the Android head unit's strict ``CanCommand`` frame checks."""
|
|
|
|
if frame.channel not in {0, 1}:
|
|
raise CanGatewayError("MQTT CAN command channel must be CAN0 or CAN1")
|
|
if frame.is_fd:
|
|
if not frame.bitrate_switch:
|
|
raise CanGatewayError("MQTT CAN FD command requires BRS")
|
|
if len(frame.data) not in MQTT_VALID_FD_PAYLOAD_LENGTHS:
|
|
raise CanGatewayError(
|
|
"MQTT CAN FD command length must be 0..8/12/16/20/24/32/48/64"
|
|
)
|
|
elif frame.bitrate_switch:
|
|
raise CanGatewayError("MQTT classic CAN command cannot enable BRS")
|
|
elif len(frame.data) > 8:
|
|
raise CanGatewayError("MQTT classic CAN command cannot exceed 8 bytes")
|
|
|
|
|
|
def mqtt_control_payload(
|
|
frame: CanFrame,
|
|
*,
|
|
response_timeout_ms: int = 1_500,
|
|
request_id: str | None = None,
|
|
command: str | None = None,
|
|
) -> str:
|
|
"""Serialize a frame as the Android head unit's strict ``CanCommand`` JSON."""
|
|
|
|
_validate_headunit_command_frame(frame)
|
|
command_request_id = request_id or f"cognitive-agent-{uuid.uuid4().hex}"
|
|
if not command_request_id.strip() or len(command_request_id) > 128:
|
|
raise CanGatewayError("MQTT CAN command requestId must contain 1..128 characters")
|
|
debug_command = "" if command is None else str(command).strip()
|
|
if len(debug_command) > 256:
|
|
raise CanGatewayError("MQTT CAN debug command must contain at most 256 characters")
|
|
bounded_timeout_ms = max(100, min(5_000, int(response_timeout_ms)))
|
|
|
|
return json.dumps(
|
|
{
|
|
"requestId": command_request_id,
|
|
# Debug-only semantic metadata. The head unit continues to execute
|
|
# only the validated channel/ID/frame/data fields below.
|
|
"command": debug_command,
|
|
"channel": f"can{frame.channel}",
|
|
"canId": f"0x{frame.arbitration_id:03X}",
|
|
"frameType": "fd" if frame.is_fd else "classic",
|
|
"brs": frame.bitrate_switch,
|
|
"extended": False,
|
|
"remote": False,
|
|
"data": frame.data.hex(" ").upper(),
|
|
"repeat": 1,
|
|
"intervalMs": 0,
|
|
# Vehicle state is consumed asynchronously from can/state by the
|
|
# Cognitive Agent. Do not serialize Android's command worker on a
|
|
# per-command response wait as well.
|
|
"expectResponse": False,
|
|
"responseTimeoutMs": bounded_timeout_ms,
|
|
},
|
|
ensure_ascii=False,
|
|
separators=(",", ":"),
|
|
)
|
|
|
|
|
|
def _validate_ota_seat_runtime_control(data: bytes) -> None:
|
|
"""Reject every CAN0 OTA opcode except normal bounded seat runtime control."""
|
|
|
|
if len(data) != 8 or data[0] != 0xC7:
|
|
raise CanGatewayError("CAN0 seat-ota control requires C7 and DLC 8")
|
|
opcode = data[2]
|
|
selector = data[3]
|
|
argument = int.from_bytes(data[4:8], "little")
|
|
if opcode not in MQTT_OTA_SEAT_RUNTIME_OPCODES:
|
|
raise CanGatewayError(
|
|
"CAN0 seat-ota control allows only STOP, MOVE, HEARTBEAT, or DYNAMIC"
|
|
)
|
|
if opcode == 0x63 and (selector not in {*range(4), 0xFF} or argument != 0):
|
|
raise CanGatewayError("CAN0 seat STOP selector or argument is invalid")
|
|
if opcode == 0x64 and (selector not in range(4) or argument > 1_000):
|
|
raise CanGatewayError("CAN0 seat MOVE requires axis 0..3 and target 0..1000")
|
|
if opcode == 0x65 and (selector not in {*range(4), 0xFF} or argument != 0):
|
|
raise CanGatewayError("CAN0 seat HEARTBEAT selector or argument is invalid")
|
|
if opcode == 0x66 and (
|
|
selector not in {1, 2}
|
|
or (argument != 0 and not 5 <= argument <= 300)
|
|
):
|
|
raise CanGatewayError(
|
|
"CAN0 seat DYNAMIC requires mode 1/2 and duration 5..300 seconds"
|
|
)
|
|
|
|
|
|
def _validate_native_seat_thermal_control(frame: CanFrame) -> None:
|
|
"""Validate the two runtime thermal frames implemented by the CAN0 firmware."""
|
|
|
|
if len(frame.data) != 8:
|
|
raise CanGatewayError("CAN0 passenger-seat thermal control requires DLC 8")
|
|
if frame.arbitration_id == 0x3CE:
|
|
mode = (frame.data[0] >> 4) & 0x03
|
|
if mode not in {1, 2} or frame.data[0] & 0xCF or any(frame.data[1:]):
|
|
raise CanGatewayError("CAN0 0x3CE thermal mode payload is invalid")
|
|
return
|
|
if frame.arbitration_id != 0x29E:
|
|
raise CanGatewayError("CAN0 native thermal frame ID is invalid")
|
|
fan_on = frame.data[0] & 0x03
|
|
fan_mode = (frame.data[0] >> 2) & 0x03
|
|
heating_on = (frame.data[0] >> 4) & 0x03
|
|
fan_level = frame.data[2] & 0x0F
|
|
heating_level = (frame.data[2] >> 4) & 0x0F
|
|
if (
|
|
fan_on not in {1, 2}
|
|
or fan_mode != 1
|
|
or heating_on not in {1, 2}
|
|
or fan_level not in range(4)
|
|
or heating_level not in range(4)
|
|
or (fan_on == 1 and fan_level == 0)
|
|
or (fan_on == 2 and fan_level != 0)
|
|
or (heating_on == 1 and heating_level == 0)
|
|
or (heating_on == 2 and heating_level != 0)
|
|
or frame.data[1] != 0
|
|
or any(frame.data[3:])
|
|
):
|
|
raise CanGatewayError("CAN0 0x29E thermal control payload is invalid")
|
|
|
|
|
|
class MqttCanTransport:
|
|
"""One process-wide MQTT connection serving snapshots and executor buses."""
|
|
|
|
def __init__(
|
|
self,
|
|
settings: VehicleMqttSettings,
|
|
*,
|
|
client_factory: Callable[[], Any] | None = None,
|
|
decoder: Callable[[CanFrame], dict[str, Any]] = decode_feedback,
|
|
) -> None:
|
|
self.settings = settings
|
|
self.client_factory = client_factory
|
|
self.decoder = decoder
|
|
self.initial_snapshot_timeout_s = settings.feedback_timeout_ms / 1_000
|
|
self.freshness_s = max(3.0, self.initial_snapshot_timeout_s * 2)
|
|
|
|
self._condition = threading.Condition(threading.RLock())
|
|
self._client: Any | None = None
|
|
self._started = False
|
|
self._state = "disabled" if not settings.enabled else "idle"
|
|
self._last_error: str | None = None
|
|
self._parse_errors = 0
|
|
self._stale_frames = 0
|
|
self._capture_sequence = 0
|
|
self._initial_snapshot_attempted = False
|
|
self._frames: deque[tuple[int, CanFrame]] = deque(maxlen=4_096)
|
|
self._latest: dict[int, tuple[float, dict[str, Any]]] = {}
|
|
self._latest_seen: dict[tuple[int, int], float] = {}
|
|
self._latest_bus_frames: dict[
|
|
tuple[int, int, int], tuple[float, CanFrame]
|
|
] = {}
|
|
|
|
def _build_client(self) -> Any:
|
|
if self.client_factory is not None:
|
|
return self.client_factory()
|
|
try:
|
|
import paho.mqtt.client as mqtt # type: ignore[import-not-found]
|
|
except ImportError as exc:
|
|
raise CanGatewayError("paho-mqtt is not installed") from exc
|
|
client_id = f"n7-cognitive-agent-{uuid.uuid4().hex[:12]}"
|
|
try:
|
|
return mqtt.Client(
|
|
callback_api_version=mqtt.CallbackAPIVersion.VERSION2,
|
|
client_id=client_id,
|
|
protocol=mqtt.MQTTv311,
|
|
)
|
|
except (AttributeError, TypeError):
|
|
return mqtt.Client(client_id=client_id, protocol=mqtt.MQTTv311)
|
|
|
|
def start(self) -> None:
|
|
with self._condition:
|
|
if self._started or not self.settings.state_configured:
|
|
return
|
|
self._started = True
|
|
self._state = "connecting"
|
|
try:
|
|
client = self._build_client()
|
|
client.on_connect = self._on_connect
|
|
client.on_disconnect = self._on_disconnect
|
|
client.on_message = self._on_message
|
|
if self.settings.username:
|
|
client.username_pw_set(self.settings.username, self.settings.password)
|
|
if self.settings.tls_enabled:
|
|
client.tls_set()
|
|
if hasattr(client, "reconnect_delay_set"):
|
|
client.reconnect_delay_set(min_delay=1, max_delay=10)
|
|
with self._condition:
|
|
self._client = client
|
|
client.connect_async(
|
|
self.settings.host,
|
|
self.settings.port,
|
|
self.settings.keepalive_s,
|
|
)
|
|
client.loop_start()
|
|
except Exception as exc: # noqa: BLE001 - normalize broker/client failures
|
|
with self._condition:
|
|
self._state = "error"
|
|
self._last_error = str(exc)
|
|
self._condition.notify_all()
|
|
|
|
def close(self) -> None:
|
|
with self._condition:
|
|
client = self._client
|
|
self._client = None
|
|
self._started = False
|
|
if client is not None:
|
|
try:
|
|
client.disconnect()
|
|
except Exception: # noqa: BLE001, S110 - best-effort service shutdown
|
|
pass
|
|
try:
|
|
client.loop_stop()
|
|
except Exception: # noqa: BLE001, S110 - best-effort service shutdown
|
|
pass
|
|
with self._condition:
|
|
self._state = "stopped"
|
|
self._condition.notify_all()
|
|
|
|
@staticmethod
|
|
def _reason_code_value(reason_code: Any) -> int:
|
|
try:
|
|
return int(reason_code)
|
|
except (TypeError, ValueError):
|
|
return int(getattr(reason_code, "value", 1))
|
|
|
|
def _on_connect(
|
|
self,
|
|
client: Any,
|
|
_userdata: Any,
|
|
_flags: Any,
|
|
reason_code: Any,
|
|
_properties: Any = None,
|
|
) -> None:
|
|
code = self._reason_code_value(reason_code)
|
|
if code != 0:
|
|
with self._condition:
|
|
self._state = "error"
|
|
self._last_error = f"MQTT connection refused with reason {code}"
|
|
self._condition.notify_all()
|
|
return
|
|
result = client.subscribe(self.settings.condition_topic, qos=self.settings.qos)
|
|
result_code = result[0] if isinstance(result, tuple) else getattr(result, "rc", 0)
|
|
with self._condition:
|
|
if int(result_code) == 0:
|
|
self._state = "online"
|
|
self._last_error = None
|
|
else:
|
|
self._state = "error"
|
|
self._last_error = f"MQTT subscribe failed with code {result_code}"
|
|
self._condition.notify_all()
|
|
|
|
def _on_disconnect(
|
|
self,
|
|
_client: Any,
|
|
_userdata: Any,
|
|
*args: Any,
|
|
) -> None:
|
|
reason_code = args[-2] if len(args) >= 2 else args[-1] if args else 0
|
|
code = self._reason_code_value(reason_code)
|
|
with self._condition:
|
|
if self._started:
|
|
self._state = "connecting"
|
|
self._last_error = None if code == 0 else f"MQTT disconnected: {code}"
|
|
else:
|
|
self._state = "stopped"
|
|
self._condition.notify_all()
|
|
|
|
def _on_message(self, _client: Any, _userdata: Any, message: Any) -> None:
|
|
if str(getattr(message, "topic", "")) != self.settings.condition_topic:
|
|
return
|
|
try:
|
|
frames = parse_mqtt_can_payload(bytes(message.payload))
|
|
for frame in frames:
|
|
self._record_frame(frame)
|
|
except (TypeError, ValueError) as exc:
|
|
with self._condition:
|
|
self._parse_errors += 1
|
|
self._last_error = str(exc)
|
|
self._condition.notify_all()
|
|
|
|
def _record_frame(self, frame: CanFrame) -> None:
|
|
if (
|
|
frame.channel not in MQTT_ACCEPTED_FEEDBACK_IDS
|
|
or frame.arbitration_id not in MQTT_ACCEPTED_FEEDBACK_IDS[frame.channel]
|
|
or len(frame.data) < 8
|
|
):
|
|
return
|
|
cache_age_s = (
|
|
max(0.0, time.time() - frame.timestamp)
|
|
if frame.timestamp is not None and frame.timestamp >= 1_000_000_000
|
|
else 0.0
|
|
)
|
|
stale = cache_age_s > self.freshness_s
|
|
with self._condition:
|
|
self._capture_sequence += 1
|
|
sequence = self._capture_sequence
|
|
self._latest_seen[(frame.channel, frame.arbitration_id)] = time.monotonic()
|
|
if stale:
|
|
self._stale_frames += 1
|
|
else:
|
|
self._frames.append((sequence, frame))
|
|
discriminator = (
|
|
frame.data[0]
|
|
if frame.channel == 0
|
|
and frame.arbitration_id in {0x302, 0x303, 0x304}
|
|
and frame.data
|
|
else 0
|
|
)
|
|
self._latest_bus_frames[
|
|
(frame.channel, frame.arbitration_id, discriminator)
|
|
] = (time.monotonic(), frame)
|
|
self._condition.notify_all()
|
|
if stale and (
|
|
frame.channel != 1
|
|
or frame.arbitration_id in MQTT_SAFETY_FEEDBACK_IDS
|
|
or frame.arbitration_id not in OBSERVABLE_IDS
|
|
):
|
|
# Cached frames never enter an executor bus. In particular, old
|
|
# gear/speed evidence cannot satisfy the stationary-vehicle gate.
|
|
return
|
|
if frame.channel != 1 or frame.arbitration_id not in OBSERVABLE_IDS:
|
|
return
|
|
decoded = self.decoder(frame)
|
|
if not any(
|
|
key in decoded
|
|
for key in ("actuator_state", "domain_state", "vehicle_safety")
|
|
):
|
|
return
|
|
# The matrix decoder is shared with the wired CH1 monitor and therefore
|
|
# emits its wired source marker by default. Preserve the common byte
|
|
# decoder while making the actual transport explicit before this state
|
|
# reaches the conversation store or browser.
|
|
decoded["source"] = WIRELESS_MQTT_CH1_SOURCE
|
|
frame_id = str(decoded.get("frame_id", ""))
|
|
captured_at = (
|
|
datetime.fromtimestamp(frame.timestamp, UTC)
|
|
.isoformat()
|
|
.replace("+00:00", "Z")
|
|
if frame.timestamp is not None and frame.timestamp >= 1_000_000_000
|
|
else _now()
|
|
)
|
|
decoded.update(
|
|
{
|
|
"valid": True,
|
|
"feedback_frame_id": frame_id,
|
|
"captured_at": captured_at,
|
|
"verification": {
|
|
"authoritative": True,
|
|
"channel": 1,
|
|
"simulated": False,
|
|
"read_only": True,
|
|
"transport": "wireless_mqtt",
|
|
"topic": self.settings.condition_topic,
|
|
"frame_id": frame_id,
|
|
"payload_size": len(frame.data),
|
|
"fresh": not stale,
|
|
"display_only": stale,
|
|
"control_eligible": not stale,
|
|
"cache_age_ms": round(cache_age_s * 1_000),
|
|
},
|
|
}
|
|
)
|
|
with self._condition:
|
|
self._latest[frame.arbitration_id] = (time.monotonic(), decoded)
|
|
self._condition.notify_all()
|
|
|
|
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 snapshot(self) -> VehicleStateSnapshot:
|
|
self.start()
|
|
deadline = time.monotonic() + self.initial_snapshot_timeout_s
|
|
with self._condition:
|
|
while not self._initial_snapshot_attempted:
|
|
fresh_ids = self._fresh_ids(time.monotonic())
|
|
# A partial live body snapshot is immediately useful to the
|
|
# dashboard. Completeness remains explicit in the API; only a
|
|
# completely empty first subscription waits for its bounded
|
|
# initial frame window.
|
|
if fresh_ids:
|
|
break
|
|
if self._state == "error":
|
|
raise CanGatewayError(self._last_error or "MQTT CAN relay is unavailable")
|
|
remaining_s = deadline - time.monotonic()
|
|
if remaining_s <= 0:
|
|
break
|
|
self._condition.wait(timeout=remaining_s)
|
|
self._initial_snapshot_attempted = True
|
|
observed_ids = self._fresh_ids(time.monotonic())
|
|
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 bus(self, channel: int = 1, *, replay_latest: bool = False) -> MqttCanBus:
|
|
if channel not in MQTT_ACCEPTED_FEEDBACK_IDS:
|
|
raise CanGatewayError("MQTT CAN bus channel must be 0 or 1")
|
|
self.start()
|
|
with self._condition:
|
|
cursor = self._capture_sequence
|
|
now = time.monotonic()
|
|
initial_frames = tuple(
|
|
frame
|
|
for (seen_channel, _frame_id, _discriminator),
|
|
(captured_monotonic, frame) in sorted(
|
|
self._latest_bus_frames.items(),
|
|
key=lambda item: (
|
|
0 if item[0][1] == 0x306 else
|
|
1 if item[0][1] == 0x4FA else
|
|
2,
|
|
item[0],
|
|
),
|
|
)
|
|
if replay_latest
|
|
and seen_channel == channel
|
|
and now - captured_monotonic <= self.freshness_s
|
|
)
|
|
return MqttCanBus(self, cursor, channel, initial_frames=initial_frames)
|
|
|
|
def publish(
|
|
self,
|
|
frame: CanFrame,
|
|
*,
|
|
command: str | None = None,
|
|
) -> dict[str, Any]:
|
|
if (
|
|
frame.channel not in MQTT_APPROVED_CONTROL_IDS
|
|
or frame.arbitration_id not in MQTT_APPROVED_CONTROL_IDS[frame.channel]
|
|
):
|
|
raise CanGatewayError("MQTT CAN frame is outside the approved channel policy")
|
|
if frame.channel == 0 and (frame.is_fd or frame.bitrate_switch):
|
|
raise CanGatewayError("CAN0 passenger-seat control requires classic CAN")
|
|
_validate_headunit_command_frame(frame)
|
|
if frame.channel == 0 and frame.arbitration_id == 0x6A0:
|
|
_validate_ota_seat_runtime_control(frame.data)
|
|
elif frame.channel == 0:
|
|
_validate_native_seat_thermal_control(frame)
|
|
with self._condition:
|
|
client = self._client
|
|
state = self._state
|
|
if client is None or state != "online":
|
|
raise CanGatewayError("MQTT CAN relay is not connected")
|
|
raw_payload = mqtt_control_payload(
|
|
frame,
|
|
response_timeout_ms=self.settings.feedback_timeout_ms,
|
|
command=command,
|
|
)
|
|
result = client.publish(
|
|
self.settings.control_topic,
|
|
raw_payload,
|
|
qos=self.settings.qos,
|
|
retain=False,
|
|
)
|
|
if int(getattr(result, "rc", 0)) != 0:
|
|
raise CanGatewayError(f"MQTT CAN publish failed with code {result.rc}")
|
|
# Paho already accepted the payload into its network-loop queue when
|
|
# publish() returns rc == MQTT_ERR_SUCCESS. Waiting here serializes
|
|
# every repeated CAN frame and previously added up to two seconds per
|
|
# frame. Observe an already-available acknowledgement without ever
|
|
# blocking the control pipeline.
|
|
is_published = getattr(result, "is_published", None)
|
|
broker_confirmed = bool(is_published()) if callable(is_published) else False
|
|
return {
|
|
"topic": self.settings.control_topic,
|
|
"qos": self.settings.qos,
|
|
"retain": False,
|
|
"delivery_status": "published" if broker_confirmed else "queued",
|
|
"message_id": getattr(result, "mid", None),
|
|
"payload": json.loads(raw_payload),
|
|
}
|
|
|
|
def _receive_after(
|
|
self,
|
|
cursor: int,
|
|
timeout_ms: int,
|
|
*,
|
|
channel: int,
|
|
include_fd: bool,
|
|
) -> tuple[int, CanFrame | None]:
|
|
deadline = time.monotonic() + max(0, timeout_ms) / 1_000
|
|
with self._condition:
|
|
while True:
|
|
for sequence, frame in self._frames:
|
|
if sequence <= cursor:
|
|
continue
|
|
cursor = sequence
|
|
if frame.channel == channel and (include_fd or not frame.is_fd):
|
|
return cursor, frame
|
|
remaining_s = deadline - time.monotonic()
|
|
if timeout_ms <= 0 or remaining_s <= 0:
|
|
return cursor, None
|
|
self._condition.wait(timeout=remaining_s)
|
|
|
|
def public_info(self) -> dict[str, Any]:
|
|
with self._condition:
|
|
info = self.settings.public_info()
|
|
now = time.monotonic()
|
|
observed_by_channel = {
|
|
str(channel): [
|
|
f"0x{frame_id:03X}"
|
|
for (seen_channel, frame_id), captured_monotonic in sorted(
|
|
self._latest_seen.items()
|
|
)
|
|
if seen_channel == channel
|
|
and now - captured_monotonic <= self.freshness_s
|
|
]
|
|
for channel in MQTT_ACCEPTED_FEEDBACK_IDS
|
|
}
|
|
info["state_sync"] = {
|
|
"mode": "persistent_mqtt_subscription",
|
|
"state": self._state,
|
|
"read_only": True,
|
|
"poll_interval_ms": MQTT_POLL_INTERVAL_MS,
|
|
"snapshot_freshness_ms": round(self.freshness_s * 1_000),
|
|
"capture_sequence": self._capture_sequence,
|
|
"observed_frame_ids": [
|
|
f"0x{frame_id:03X}"
|
|
for frame_id in sorted(self._fresh_ids(now))
|
|
],
|
|
"observed_frame_ids_by_channel": observed_by_channel,
|
|
"parse_errors": self._parse_errors,
|
|
"stale_frames": self._stale_frames,
|
|
"last_error": self._last_error,
|
|
}
|
|
return info
|
|
|
|
|
|
class MqttCanBus:
|
|
"""Cursor-based bus view over the shared MQTT subscription."""
|
|
|
|
def __init__(
|
|
self,
|
|
transport: MqttCanTransport,
|
|
cursor: int,
|
|
channel: int,
|
|
*,
|
|
initial_frames: tuple[CanFrame, ...] = (),
|
|
) -> None:
|
|
self._transport = transport
|
|
self._cursor = cursor
|
|
self.channel = channel
|
|
self._initial_frames = deque(initial_frames)
|
|
self._debug_command = ""
|
|
self._published_messages: list[dict[str, Any]] = []
|
|
self._closed = False
|
|
|
|
@property
|
|
def published_messages(self) -> tuple[dict[str, Any], ...]:
|
|
"""Return this bus view's accepted MQTT publications in send order."""
|
|
|
|
return tuple(deepcopy(self._published_messages))
|
|
|
|
def set_debug_command(self, command: str | None) -> None:
|
|
"""Attach semantic debug metadata to subsequent MQTT publications."""
|
|
|
|
value = "" if command is None else str(command).strip()
|
|
if len(value) > 256:
|
|
raise CanGatewayError("MQTT CAN debug command must contain at most 256 characters")
|
|
self._debug_command = value
|
|
|
|
def send(
|
|
self,
|
|
arbitration_id: int,
|
|
data: bytes,
|
|
*,
|
|
is_fd: bool = False,
|
|
bitrate_switch: bool = False,
|
|
) -> CanFrame:
|
|
if self._closed:
|
|
raise CanGatewayError("MQTT CAN bus is closed")
|
|
if arbitration_id not in MQTT_APPROVED_CONTROL_IDS[self.channel]:
|
|
raise CanGatewayError(
|
|
f"CAN frame ID is not in the approved CAN{self.channel} control set"
|
|
)
|
|
if not 1 <= len(data) <= 64 or (not is_fd and len(data) > 8):
|
|
raise CanGatewayError("invalid CAN payload length")
|
|
if self.channel == 0 and (is_fd or bitrate_switch):
|
|
raise CanGatewayError("CAN0 passenger-seat control requires classic CAN")
|
|
frame = CanFrame(
|
|
arbitration_id=arbitration_id,
|
|
data=bytes(data),
|
|
channel=self.channel,
|
|
is_fd=is_fd,
|
|
bitrate_switch=bitrate_switch,
|
|
)
|
|
publication = self._transport.publish(frame, command=self._debug_command)
|
|
self._published_messages.append(publication)
|
|
return frame
|
|
|
|
def receive(
|
|
self,
|
|
timeout_ms: int,
|
|
*,
|
|
include_fd: bool = False,
|
|
) -> CanFrame | None:
|
|
if self._closed:
|
|
raise CanGatewayError("MQTT CAN bus is closed")
|
|
while self._initial_frames:
|
|
frame = self._initial_frames.popleft()
|
|
if include_fd or not frame.is_fd:
|
|
return frame
|
|
self._cursor, frame = self._transport._receive_after(
|
|
self._cursor,
|
|
timeout_ms,
|
|
channel=self.channel,
|
|
include_fd=include_fd,
|
|
)
|
|
return frame
|
|
|
|
def close(self) -> None:
|
|
self._closed = True
|