mirror of
https://github.com/controllerzz/carbus_lib.git
synced 2026-07-15 16:30:16 +03:00
controllerzz
1441 lines
45 KiB
Python
1441 lines
45 KiB
Python
from __future__ import annotations
|
|
|
|
import asyncio
|
|
import contextlib
|
|
import logging
|
|
import struct
|
|
from dataclasses import dataclass, field
|
|
from typing import Dict, Optional, Tuple, List, Awaitable, Callable
|
|
|
|
import serial_asyncio
|
|
|
|
from .exceptions import CarBusError, SyncError, CommandError
|
|
from .messages import CanMessage
|
|
|
|
|
|
NOMINAL_BITRATE_INDEX: Dict[int, int] = {
|
|
10_000: 0,
|
|
20_000: 1,
|
|
33_300: 2,
|
|
50_000: 3,
|
|
62_500: 4,
|
|
83_300: 5,
|
|
95_200: 6,
|
|
100_000: 7,
|
|
125_000: 8,
|
|
250_000: 9,
|
|
400_000: 10,
|
|
500_000: 11,
|
|
800_000: 12,
|
|
1_000_000: 13,
|
|
# 0xFF = detect,
|
|
}
|
|
|
|
|
|
DATA_BITRATE_INDEX: Dict[int, int] = {
|
|
500_000: 0,
|
|
1_000_000: 1,
|
|
2_000_000: 2,
|
|
4_000_000: 3,
|
|
5_000_000: 4,
|
|
}
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class CanTiming:
|
|
prescaler: int
|
|
tq_seg1: int
|
|
tq_seg2: int
|
|
sjw: int
|
|
|
|
|
|
DEFAULT_CAN_CLOCK_HZ = 120_000_000
|
|
|
|
# Проверенные bit-timing'и для CAN-модуля с тактовой 120 МГц (точка выборки ~80%).
|
|
# open_can_channel() для FD-канала задаёт скорость именно таймингами, а не
|
|
# одиночным индексом data-скорости (CC_BUS_SPEED_D): прошивка этих адаптеров
|
|
# применяет явные тайминги надёжно, а индекс data-скорости не отрабатывает —
|
|
# канал открывается, но FD/BRS-кадры не уходят в шину.
|
|
_NOMINAL_TIMINGS_120M: Dict[int, CanTiming] = {
|
|
1_000_000: CanTiming(prescaler=6, tq_seg1=15, tq_seg2=4, sjw=1), # 120/(6*20)
|
|
500_000: CanTiming(prescaler=15, tq_seg1=12, tq_seg2=3, sjw=1), # 120/(15*16)
|
|
250_000: CanTiming(prescaler=30, tq_seg1=12, tq_seg2=3, sjw=1), # 120/(30*16)
|
|
125_000: CanTiming(prescaler=60, tq_seg1=12, tq_seg2=3, sjw=1), # 120/(60*16)
|
|
}
|
|
_DATA_TIMINGS_120M: Dict[int, CanTiming] = {
|
|
5_000_000: CanTiming(prescaler=3, tq_seg1=5, tq_seg2=2, sjw=1), # 120/(3*8)
|
|
4_000_000: CanTiming(prescaler=3, tq_seg1=7, tq_seg2=2, sjw=1), # 120/(3*10)
|
|
2_000_000: CanTiming(prescaler=6, tq_seg1=7, tq_seg2=2, sjw=1), # 120/(6*10)
|
|
1_000_000: CanTiming(prescaler=12, tq_seg1=7, tq_seg2=2, sjw=1), # 120/(12*10)
|
|
500_000: CanTiming(prescaler=24, tq_seg1=7, tq_seg2=2, sjw=1), # 120/(24*10)
|
|
}
|
|
|
|
|
|
def _resolve_timing(
|
|
table: Dict[int, CanTiming], bitrate: int, clock_hz: int, kind: str
|
|
) -> CanTiming:
|
|
if clock_hz == DEFAULT_CAN_CLOCK_HZ and bitrate in table:
|
|
return table[bitrate]
|
|
raise ValueError(
|
|
f"Нет встроенного FD bit-timing для {kind}={bitrate} bps при тактовой "
|
|
f"{clock_hz} Гц. Доступно (для 120 МГц): {sorted(table)}. "
|
|
f"Задайте скорость явными таймингами через open_can_channel_custom(...)."
|
|
)
|
|
|
|
|
|
@dataclass
|
|
class DeviceInfoParam:
|
|
header: int
|
|
data: List[int]
|
|
|
|
from .protocol import (
|
|
Command,
|
|
CommandHeader,
|
|
MsgCommandHeader,
|
|
HeaderFlags,
|
|
BusMessageFlags,
|
|
CC_MULTIWORD,
|
|
DI_HARDWARE_ID,
|
|
DI_FIRMWARE_VERSION,
|
|
DI_DEVICE_SERIAL,
|
|
DI_FEATURES,
|
|
DI_CHANNEL_MAP,
|
|
DI_CHANNEL_FEATURES,
|
|
DI_FILTER,
|
|
DI_GATEWAY,
|
|
DI_CHANNEL_FREQUENCY,
|
|
DI_ISOTP,
|
|
DI_TX_BUFFER,
|
|
DI_TX_TASK,
|
|
is_ack,
|
|
base_command_from_ack,
|
|
need_extended_header,
|
|
)
|
|
|
|
HW_ID_NAMES: Dict[int, str] = {
|
|
0xFF: "HW_CH30",
|
|
0x02: "HW_ODB_OLD",
|
|
0x01: "HW_CH32",
|
|
0x04: "HW_ODB",
|
|
0x03: "HW_CHP",
|
|
0x11: "HW_CH33",
|
|
0x13: "HW_CHPM03",
|
|
0x14: "HW_ODB_FD",
|
|
0x06: "HW_FDL2_M02",
|
|
0x16: "HW_FDL2_M05",
|
|
}
|
|
|
|
# DI_FEATURES
|
|
DI_FEATURE_GATEWAY = 0x00000001
|
|
DI_FEATURE_ISOTP = 0x00000002
|
|
DI_FEATURE_TX_BUFFER = 0x00000004
|
|
DI_FEATURE_TX_TASK = 0x00000008
|
|
|
|
FLAG_CONFIG_TERMINATOR = 0x05
|
|
|
|
# ChannelType (DI_CHANNEL_MAP)
|
|
CHANNEL_TYPE_MAP: Dict[int, str] = {
|
|
0x00: "NONE", # CTE_None
|
|
0x01: "CAN", # CTE_CAN
|
|
0x02: "CANFD", # CTE_CANFD
|
|
0x10: "LIN", # CTE_LIN
|
|
}
|
|
|
|
# DI_CHANNEL_FEATURES
|
|
DI_CHANNEL_CHANNEL_MASK = 0x00FF0000
|
|
DI_CHANNEL_CHANNEL_SHIFT = 16
|
|
|
|
DI_CHANNEL_FEATURE_ALC = 0x00000001
|
|
DI_CHANNEL_FEATURE_TERMINATOR = 0x00000002
|
|
DI_CHANNEL_FEATURE_PULLUP = 0x00000004
|
|
DI_CHANNEL_FEATURE_CSD = 0x00000008
|
|
DI_CHANNEL_FEATURE_IDLE = 0x00000010
|
|
DI_CHANNEL_FEATURE_DSD = 0x00000020
|
|
DI_CHANNEL_FEATURE_NONISO = 0x00000040
|
|
|
|
# DI_FILTER
|
|
DI_FILTER_CHANNEL_MASK = 0x00FF0000
|
|
DI_FILTER_CHANNEL_SHIFT = 16
|
|
|
|
DI_FILTER_TYPE_MASK = 0x0000FF00
|
|
DI_FILTER_TYPE_SHIFT = 8
|
|
DI_FILTER_SIZE_MASK = 0x000000FF
|
|
DI_FILTER_SIZE_SHIFT = 0
|
|
|
|
DI_FILTER_TYPE_8BIT = 0x01
|
|
DI_FILTER_TYPE_11BIT = 0x02
|
|
DI_FILTER_TYPE_29BIT = 0x04
|
|
|
|
# DI_GATEWAY
|
|
DI_GATEWAY_SRC_MASK = 0x00FF0000
|
|
DI_GATEWAY_SRC_SHIFT = 16
|
|
DI_GATEWAY_DST_MASK = 0x0000FF00
|
|
DI_GATEWAY_DST_SHIFT = 8
|
|
DI_GATEWAY_FILTER_MASK = 0x000000FF
|
|
DI_GATEWAY_FILTER_SHIFT = 0
|
|
|
|
@dataclass
|
|
class DeviceInfo:
|
|
params: List[DeviceInfoParam]
|
|
raw_payload: bytes
|
|
|
|
@classmethod
|
|
def from_payload(cls, payload: bytes) -> "DeviceInfo":
|
|
if len(payload) % 4 != 0:
|
|
raise ValueError(
|
|
f"DEVICE_INFO payload length {len(payload)} is not multiple of 4"
|
|
)
|
|
|
|
words = [
|
|
int.from_bytes(payload[i: i + 4], "little")
|
|
for i in range(0, len(payload), 4)
|
|
]
|
|
|
|
params: List[DeviceInfoParam] = []
|
|
i = 0
|
|
n = len(words)
|
|
|
|
while i < n:
|
|
header = words[i]
|
|
i += 1
|
|
|
|
if header & CC_MULTIWORD:
|
|
length = (header >> 16) & 0xFF
|
|
data = words[i: i + length]
|
|
i += length
|
|
else:
|
|
data = []
|
|
|
|
params.append(DeviceInfoParam(header=header, data=data))
|
|
|
|
return cls(params=params, raw_payload=payload)
|
|
|
|
@staticmethod
|
|
def _param_code(value: int) -> int:
|
|
return value & 0x7F000000
|
|
|
|
def _find_first(self, base: int) -> Optional[DeviceInfoParam]:
|
|
base_code = self._param_code(base)
|
|
for p in self.params:
|
|
if self._param_code(p.header) == base_code:
|
|
return p
|
|
return None
|
|
|
|
def _find_all(self, base: int) -> List[DeviceInfoParam]:
|
|
base_code = self._param_code(base)
|
|
return [
|
|
p for p in self.params
|
|
if self._param_code(p.header) == base_code
|
|
]
|
|
|
|
def find_by_prefix(self, prefix: int) -> List[DeviceInfoParam]:
|
|
base_code = self._param_code(prefix)
|
|
return [
|
|
p for p in self.params
|
|
if self._param_code(p.header) == base_code
|
|
]
|
|
|
|
@property
|
|
def hardware_id(self) -> Optional[int]:
|
|
p = self._find_first(DI_HARDWARE_ID)
|
|
if not p:
|
|
return None
|
|
return p.header & 0xFF
|
|
|
|
@property
|
|
def hardware_name(self) -> Optional[str]:
|
|
hw_id = self.hardware_id
|
|
if hw_id is None:
|
|
return None
|
|
return HW_ID_NAMES.get(hw_id, f"0x{hw_id:02X}")
|
|
|
|
@property
|
|
def firmware_version(self) -> Optional[str]:
|
|
p = self._find_first(DI_FIRMWARE_VERSION)
|
|
if not p:
|
|
return None
|
|
if not p.data:
|
|
return ""
|
|
|
|
b = b"".join(w.to_bytes(4, "little") for w in p.data)
|
|
# Обрезаем по 0x00
|
|
if b"\x00" in b:
|
|
b = b.split(b"\x00", 1)[0]
|
|
try:
|
|
return b.decode("ascii", errors="ignore")
|
|
except Exception:
|
|
return b.hex()
|
|
|
|
@property
|
|
def serial_bytes(self) -> Optional[bytes]:
|
|
p = self._find_first(DI_DEVICE_SERIAL)
|
|
if not p or not p.data:
|
|
return None
|
|
return b"".join(w.to_bytes(4, "little") for w in p.data)
|
|
|
|
@property
|
|
def serial_int(self) -> Optional[int]:
|
|
b = self.serial_bytes
|
|
if b is None:
|
|
return None
|
|
return int.from_bytes(b, "big")
|
|
|
|
@property
|
|
def features_mask(self) -> int:
|
|
p = self._find_first(DI_FEATURES)
|
|
if not p:
|
|
return 0
|
|
return p.header & 0x00FFFFFF
|
|
|
|
@property
|
|
def feature_gateway(self) -> bool:
|
|
return bool(self.features_mask & DI_FEATURE_GATEWAY)
|
|
|
|
@property
|
|
def feature_isotp(self) -> bool:
|
|
return bool(self.features_mask & DI_FEATURE_ISOTP)
|
|
|
|
@property
|
|
def feature_tx_buffer(self) -> bool:
|
|
return bool(self.features_mask & DI_FEATURE_TX_BUFFER)
|
|
|
|
@property
|
|
def feature_tx_task(self) -> bool:
|
|
return bool(self.features_mask & DI_FEATURE_TX_TASK)
|
|
|
|
@property
|
|
def channel_types(self) -> Dict[int, str]:
|
|
p = self._find_first(DI_CHANNEL_MAP)
|
|
if not p:
|
|
return {}
|
|
|
|
value = p.header
|
|
b0 = (value >> 0) & 0xFF
|
|
b1 = (value >> 8) & 0xFF
|
|
b2 = (value >> 16) & 0xFF
|
|
out: Dict[int, str] = {}
|
|
|
|
for idx, v in enumerate((b0, b1, b2), start=1):
|
|
out[idx] = CHANNEL_TYPE_MAP.get(v, f"0x{v:02X}")
|
|
|
|
return out
|
|
|
|
@property
|
|
def channel_features(self) -> Dict[int, Dict[str, bool]]:
|
|
res: Dict[int, Dict[str, bool]] = {}
|
|
for p in self._find_all(DI_CHANNEL_FEATURES):
|
|
ch = (p.header & DI_CHANNEL_CHANNEL_MASK) >> DI_CHANNEL_CHANNEL_SHIFT
|
|
mask = p.header & 0x0000FFFF
|
|
if ch == 0:
|
|
continue
|
|
res[ch] = {
|
|
"raw_mask": mask,
|
|
"alc": bool(mask & DI_CHANNEL_FEATURE_ALC),
|
|
"terminator": bool(mask & DI_CHANNEL_FEATURE_TERMINATOR),
|
|
"pullup": bool(mask & DI_CHANNEL_FEATURE_PULLUP),
|
|
"csd": bool(mask & DI_CHANNEL_FEATURE_CSD),
|
|
"idle": bool(mask & DI_CHANNEL_FEATURE_IDLE),
|
|
"dsd": bool(mask & DI_CHANNEL_FEATURE_DSD),
|
|
"noniso": bool(mask & DI_CHANNEL_FEATURE_NONISO),
|
|
}
|
|
return res
|
|
|
|
@property
|
|
def filters_info(self) -> List[Dict[str, int]]:
|
|
out: List[Dict[str, int]] = []
|
|
for p in self._find_all(DI_FILTER):
|
|
h = p.header
|
|
ch = (h & DI_FILTER_CHANNEL_MASK) >> DI_FILTER_CHANNEL_SHIFT
|
|
type_mask = (h & DI_FILTER_TYPE_MASK) >> DI_FILTER_TYPE_SHIFT
|
|
size = (h & DI_FILTER_SIZE_MASK) >> DI_FILTER_SIZE_SHIFT
|
|
|
|
if ch == 0:
|
|
continue
|
|
|
|
info = {
|
|
"channel": ch,
|
|
"type_mask": type_mask,
|
|
"size": size,
|
|
"has_8bit": bool(type_mask & DI_FILTER_TYPE_8BIT),
|
|
"has_11bit": bool(type_mask & DI_FILTER_TYPE_11BIT),
|
|
"has_29bit": bool(type_mask & DI_FILTER_TYPE_29BIT),
|
|
}
|
|
out.append(info)
|
|
return out
|
|
|
|
@property
|
|
def gateway_info(self) -> List[Dict[str, int]]:
|
|
out: List[Dict[str, int]] = []
|
|
for p in self._find_all(DI_GATEWAY):
|
|
h = p.header
|
|
src = (h & DI_GATEWAY_SRC_MASK) >> DI_GATEWAY_SRC_SHIFT
|
|
dst = (h & DI_GATEWAY_DST_MASK) >> DI_GATEWAY_DST_SHIFT
|
|
flt = (h & DI_GATEWAY_FILTER_MASK) >> DI_GATEWAY_FILTER_SHIFT
|
|
if src == 0 or dst == 0:
|
|
continue
|
|
out.append({"src": src, "dst": dst, "filters": flt})
|
|
return out
|
|
|
|
@property
|
|
def channel_frequencies(self) -> Dict[int, int]:
|
|
out: Dict[int, int] = {}
|
|
for p in self._find_all(DI_CHANNEL_FREQUENCY):
|
|
h = p.header
|
|
ch = (h & DI_GATEWAY_SRC_MASK) >> DI_GATEWAY_SRC_SHIFT # по доке тот же формат
|
|
freq = h & 0x0000FFFF # в примере 0x15010078 → 0x0078 → 120 (МГц)
|
|
if ch == 0:
|
|
continue
|
|
# по доке пример: 0x78 (120МГц) — домножаем до Hz
|
|
out[ch] = freq * 1_000_000
|
|
return out
|
|
|
|
@property
|
|
def isotp_buffer_size(self) -> Optional[int]:
|
|
p = self._find_first(DI_ISOTP)
|
|
if not p:
|
|
return None
|
|
return p.header & 0x00FFFFFF
|
|
|
|
@property
|
|
def tx_buffer_size(self) -> Optional[int]:
|
|
p = self._find_first(DI_TX_BUFFER)
|
|
if not p:
|
|
return None
|
|
return p.header & 0x00FFFFFF
|
|
|
|
@property
|
|
def tx_task_count(self) -> Optional[int]:
|
|
p = self._find_first(DI_TX_TASK)
|
|
if not p:
|
|
return None
|
|
return p.header & 0x00FFFFFF
|
|
|
|
def find_by_prefix(self, prefix: int) -> List[DeviceInfoParam]:
|
|
return [
|
|
p for p in self.params
|
|
if (p.header & 0xFF000000) == (prefix & 0xFF000000)
|
|
]
|
|
|
|
def find_by_prefix(self, prefix: int) -> List[DeviceInfoParam]:
|
|
"""
|
|
Вернуть все параметры, у которых старший байт совпадает с prefix & 0xFF000000.
|
|
Например, prefix=0x01000000 вернёт все DI_*, начинающиеся с 0x01.
|
|
"""
|
|
return [
|
|
p
|
|
for p in self.params
|
|
if (p.header & 0xFF000000) == (prefix & 0xFF000000)
|
|
]
|
|
|
|
|
|
@dataclass
|
|
class _PendingRequest:
|
|
future: asyncio.Future
|
|
command: int
|
|
|
|
|
|
CanHook = Callable[[int, CanMessage], Awaitable[None]]
|
|
CanPred = Callable[[int, CanMessage], bool]
|
|
|
|
@dataclass(frozen=True)
|
|
class _CanHookRule:
|
|
can_id: int | None # None => любой ID
|
|
value: bytes | None # None => матч только по ID/predicate
|
|
mask: bytes | None
|
|
offset: int
|
|
handler: CanHook
|
|
predicate: CanPred | None = None
|
|
|
|
|
|
def _match_masked(data: bytes, *, offset: int, value: bytes, mask: bytes) -> bool:
|
|
if len(value) != len(mask):
|
|
raise ValueError("mask and value must have same length")
|
|
end = offset + len(value)
|
|
if offset < 0 or len(data) < end:
|
|
return False
|
|
for i in range(len(value)):
|
|
if (data[offset + i] & mask[i]) != (value[i] & mask[i]):
|
|
return False
|
|
return True
|
|
|
|
@dataclass
|
|
class CarBusDevice:
|
|
port: str
|
|
baudrate: int = 115200
|
|
loop: Optional[asyncio.AbstractEventLoop] = None
|
|
|
|
_reader: asyncio.StreamReader = field(init=False, repr=False)
|
|
_writer: asyncio.StreamWriter = field(init=False, repr=False)
|
|
_rx_queue: "asyncio.Queue[tuple[int, CanMessage]]" = field(init=False, repr=False)
|
|
_rx_channel_queues: Dict[int, "asyncio.Queue[CanMessage]"] = field(init=False, repr=False)
|
|
_pending: Dict[int, _PendingRequest] = field(init=False, repr=False)
|
|
_seq_counter: int = field(init=False, default=0, repr=False)
|
|
_reader_task: Optional[asyncio.Task] = field(init=False, default=None, repr=False)
|
|
_closed: bool = field(init=False, default=False, repr=False)
|
|
_can_hooks: List[_CanHookRule] = field(init=False, repr=False)
|
|
_can_hook_sem: asyncio.Semaphore = field(init=False, repr=False)
|
|
|
|
_log: logging.Logger = field(init=False, repr=False)
|
|
_wire_log: logging.Logger = field(init=False, repr=False)
|
|
|
|
def _ensure_channel_queue(self, channel: int) -> "asyncio.Queue[CanMessage]":
|
|
q = self._rx_channel_queues.get(channel)
|
|
if q is None:
|
|
q = asyncio.Queue()
|
|
self._rx_channel_queues[channel] = q
|
|
return q
|
|
|
|
@classmethod
|
|
async def open(
|
|
cls,
|
|
port: str,
|
|
baudrate: int = 115200,
|
|
*,
|
|
loop: Optional[asyncio.AbstractEventLoop] = None,
|
|
use_can: bool = True,
|
|
use_lin: bool = False,
|
|
) -> "CarBusDevice":
|
|
self = cls(port=port, baudrate=baudrate, loop=loop)
|
|
await self._connect()
|
|
await self.sync()
|
|
self._start_reader()
|
|
await self.device_open(use_can=use_can, use_lin=use_lin)
|
|
return self
|
|
|
|
@classmethod
|
|
async def open_tcp(cls, host: str, port: int, **kwargs) -> "CarBusDevice":
|
|
return await cls.open(f"socket://{host}:{port}", **kwargs)
|
|
|
|
@classmethod
|
|
async def open_stream(
|
|
cls,
|
|
reader: asyncio.StreamReader,
|
|
writer: asyncio.StreamWriter,
|
|
*,
|
|
logical_port: str = "stream://remote",
|
|
baudrate: int = 115200,
|
|
loop: Optional[asyncio.AbstractEventLoop] = None,
|
|
use_can: bool = True,
|
|
use_lin: bool = False,
|
|
) -> "CarBusDevice":
|
|
self = cls(port=logical_port, baudrate=baudrate, loop=loop)
|
|
|
|
self._log = logging.getLogger(f"carbus_async.device.{logical_port}")
|
|
self._wire_log = logging.getLogger(f"carbus_async.wire.{logical_port}")
|
|
|
|
self._reader = reader
|
|
self._writer = writer
|
|
|
|
self._rx_queue = asyncio.Queue()
|
|
self._rx_channel_queues = {}
|
|
self._pending = {}
|
|
self._seq_counter = 0
|
|
self._reader_task = None
|
|
self._closed = False
|
|
|
|
await self.sync()
|
|
self._start_reader()
|
|
await self.device_open(use_can=use_can, use_lin=use_lin)
|
|
return self
|
|
|
|
|
|
async def _connect(self) -> None:
|
|
loop = self.loop or asyncio.get_running_loop()
|
|
|
|
self._log = logging.getLogger(f"carbus_async.device.{self.port}")
|
|
self._wire_log = logging.getLogger(f"carbus_async.wire.{self.port}")
|
|
|
|
if self.port.startswith("socket://"):
|
|
addr = self.port[len("socket://") :]
|
|
if ":" not in addr:
|
|
raise ValueError(
|
|
f"Invalid socket URL '{self.port}'. Expected 'socket://host:port'"
|
|
)
|
|
|
|
host, port_str = addr.rsplit(":", 1)
|
|
try:
|
|
tcp_port = int(port_str)
|
|
except ValueError as e:
|
|
raise ValueError(
|
|
f"Invalid TCP port in '{self.port}': {port_str!r}"
|
|
) from e
|
|
|
|
self._log.debug(
|
|
"Connecting via TCP to %s:%d (tcp_bridge)", host, tcp_port
|
|
)
|
|
|
|
self._reader, self._writer = await asyncio.open_connection(
|
|
host=host,
|
|
port=tcp_port,
|
|
)
|
|
|
|
self._log.debug(
|
|
"Connected to TCP %s:%d (logical port=%s)",
|
|
host,
|
|
tcp_port,
|
|
self.port,
|
|
)
|
|
|
|
else:
|
|
self._log.debug(
|
|
"Connecting to serial port %s @ %d using serial_asyncio",
|
|
self.port,
|
|
self.baudrate,
|
|
)
|
|
self._reader, self._writer = await serial_asyncio.open_serial_connection(
|
|
loop=loop,
|
|
url=self.port,
|
|
baudrate=self.baudrate,
|
|
)
|
|
self._log.debug("Connected to %s @ %d", self.port, self.baudrate)
|
|
|
|
self._rx_queue = asyncio.Queue()
|
|
self._rx_channel_queues = {}
|
|
self._pending = {}
|
|
self._seq_counter = 0
|
|
self._reader_task = None
|
|
self._closed = False
|
|
self._can_hooks = []
|
|
self._can_hook_sem = asyncio.Semaphore(200)
|
|
|
|
|
|
async def close(self) -> None:
|
|
if self._closed:
|
|
return
|
|
|
|
try:
|
|
try:
|
|
await self.device_close()
|
|
except Exception:
|
|
self._log.debug("DEVICE_CLOSE failed, ignoring", exc_info=True)
|
|
|
|
if self._reader_task:
|
|
self._reader_task.cancel()
|
|
with contextlib.suppress(asyncio.CancelledError):
|
|
await self._reader_task
|
|
|
|
self._writer.close()
|
|
await self._writer.wait_closed()
|
|
finally:
|
|
self._closed = True
|
|
|
|
for pending in list(self._pending.values()):
|
|
if not pending.future.done():
|
|
pending.future.set_exception(
|
|
CarBusError("Device closed before response was received")
|
|
)
|
|
self._pending.clear()
|
|
|
|
def on_can_id(self, can_id: int, *, predicate: CanPred | None = None):
|
|
"""Хук на каждый принятый CAN кадр с данным can_id."""
|
|
def deco(fn: CanHook) -> CanHook:
|
|
self._can_hooks.append(_CanHookRule(
|
|
can_id=can_id,
|
|
value=None, mask=None, offset=0,
|
|
handler=fn,
|
|
predicate=predicate,
|
|
))
|
|
return fn
|
|
return deco
|
|
|
|
def on_can_match(
|
|
self,
|
|
*,
|
|
can_id: int | None = None,
|
|
value: bytes,
|
|
mask: bytes | None = None,
|
|
offset: int = 0,
|
|
predicate: CanPred | None = None,
|
|
):
|
|
"""
|
|
Хук по CAN-ID (или любой) + совпадение по маске.
|
|
Проверка: (data[offset+i] & mask[i]) == (value[i] & mask[i])
|
|
"""
|
|
if mask is None:
|
|
mask = bytes([0xFF]) * len(value)
|
|
|
|
def deco(fn: CanHook) -> CanHook:
|
|
self._can_hooks.append(_CanHookRule(
|
|
can_id=can_id,
|
|
value=value,
|
|
mask=mask,
|
|
offset=offset,
|
|
handler=fn,
|
|
predicate=predicate,
|
|
))
|
|
return fn
|
|
return deco
|
|
|
|
def _fire_can_hooks(self, channel: int, msg: CanMessage) -> None:
|
|
if not self._can_hooks:
|
|
return
|
|
|
|
data = bytes(msg.data)
|
|
for rule in self._can_hooks:
|
|
if rule.can_id is not None and rule.can_id != msg.can_id:
|
|
continue
|
|
if rule.predicate is not None and not rule.predicate(channel, msg):
|
|
continue
|
|
if rule.value is not None:
|
|
if not _match_masked(data, offset=rule.offset, value=rule.value, mask=rule.mask or b""):
|
|
continue
|
|
|
|
asyncio.create_task(self._run_can_hook(rule.handler, channel, msg))
|
|
|
|
async def _run_can_hook(self, fn: CanHook, channel: int, msg: CanMessage) -> None:
|
|
async with self._can_hook_sem:
|
|
try:
|
|
await fn(channel, msg)
|
|
except Exception:
|
|
self._log.exception("CAN hook failed (ch=%s id=0x%X)", channel, msg.can_id)
|
|
|
|
def _start_reader(self) -> None:
|
|
if self._reader_task is None or self._reader_task.done():
|
|
self._reader_task = asyncio.create_task(
|
|
self._read_loop(),
|
|
name=f"carbus_read_loop_{self.port}",
|
|
)
|
|
|
|
def _next_seq(self) -> int:
|
|
self._seq_counter = (self._seq_counter + 1) & 0xFF
|
|
if self._seq_counter == 0:
|
|
self._seq_counter = 1
|
|
return self._seq_counter
|
|
|
|
async def sync(self) -> None:
|
|
frame = bytes((Command.SYNC, 0x00, Command.SYNC, 0x00))
|
|
self._wire_log.debug("TX SYNC: %s", frame.hex(" "))
|
|
|
|
self._writer.write(frame)
|
|
await self._writer.drain()
|
|
|
|
resp = await self._reader.readexactly(4)
|
|
self._wire_log.debug("RX SYNC: %s", resp.hex(" "))
|
|
|
|
if resp != bytes((0x5A, 0x00, 0x5A, 0x00)):
|
|
raise SyncError(f"Unexpected SYNC response: {resp!r}")
|
|
|
|
async def _send_raw(
|
|
self,
|
|
command: int,
|
|
*,
|
|
header_flags: int = 0,
|
|
payload: bytes = b"",
|
|
expect_response: bool = True,
|
|
) -> Tuple[int, int, bytes]:
|
|
if self._closed:
|
|
raise CarBusError("Device is closed")
|
|
|
|
seq = self._next_seq()
|
|
dsize = len(payload)
|
|
|
|
if need_extended_header(command):
|
|
header = MsgCommandHeader(
|
|
command=command,
|
|
sequence=seq,
|
|
flags=header_flags,
|
|
dsize=dsize,
|
|
)
|
|
else:
|
|
if dsize > 0xFF:
|
|
raise ValueError(
|
|
f"dsize={dsize} too big for CommandHeader (max 255)"
|
|
)
|
|
header = CommandHeader(
|
|
command=command,
|
|
sequence=seq,
|
|
flags=header_flags,
|
|
dsize=dsize,
|
|
)
|
|
|
|
header_bytes = header.to_bytes()
|
|
frame = header_bytes + payload
|
|
|
|
fut: asyncio.Future = asyncio.get_running_loop().create_future()
|
|
if expect_response:
|
|
self._pending[seq] = _PendingRequest(future=fut, command=command)
|
|
|
|
self._wire_log.debug(
|
|
"TX cmd=0x%02X seq=%d flags=0x%04X dsize=%d :: %s",
|
|
command,
|
|
seq,
|
|
header_flags,
|
|
dsize,
|
|
frame.hex(" "),
|
|
)
|
|
|
|
self._writer.write(frame)
|
|
await self._writer.drain()
|
|
|
|
if not expect_response:
|
|
return 0, 0, b""
|
|
|
|
cmd_resp, flags_resp, payload_resp = await fut
|
|
return cmd_resp, flags_resp, payload_resp
|
|
|
|
async def get_device_info(self) -> DeviceInfo:
|
|
cmd, flags, payload = await self._send_raw(
|
|
Command.DEVICE_INFO,
|
|
header_flags=0,
|
|
payload=b"",
|
|
expect_response=True,
|
|
)
|
|
|
|
if cmd != Command.DEVICE_INFO:
|
|
raise CommandError(
|
|
f"Unexpected DEVICE_INFO response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
return DeviceInfo.from_payload(payload)
|
|
|
|
async def has_terminator(self, channel=1) -> bool:
|
|
info = await self.get_device_info()
|
|
feat = info.channel_features.get(channel)
|
|
return bool(feat and feat.get("terminator", False))
|
|
|
|
async def ensure_terminator(self, channel: int = 1, enabled: bool = True):
|
|
if not await self.has_terminator(channel):
|
|
return False
|
|
await self.set_terminator(channel, enabled=enabled)
|
|
|
|
async def get_serial(self) -> bool:
|
|
info = await self.get_device_info()
|
|
return info.serial_int;
|
|
|
|
|
|
|
|
async def device_open(self, *, use_can: bool = True, use_lin: bool = False) -> None:
|
|
if use_can and use_lin:
|
|
mode_val = 0x00 # FULL
|
|
elif use_can and not use_lin:
|
|
mode_val = 0x01 # CAN only
|
|
elif not use_can and use_lin:
|
|
mode_val = 0x02 # LIN only
|
|
else:
|
|
mode_val = 0x00 # FULL по умолчанию
|
|
|
|
dc_mode = 0x01000000 | mode_val
|
|
payload = dc_mode.to_bytes(4, "little")
|
|
|
|
cmd, flags, resp_payload = await self._send_raw(
|
|
Command.DEVICE_OPEN,
|
|
header_flags=0,
|
|
payload=payload,
|
|
expect_response=True,
|
|
)
|
|
|
|
if not is_ack(cmd) or base_command_from_ack(cmd) != Command.DEVICE_OPEN:
|
|
raise CommandError(
|
|
f"Unexpected DEVICE_OPEN response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
async def device_close(self) -> None:
|
|
cmd, flags, _ = await self._send_raw(
|
|
Command.DEVICE_CLOSE,
|
|
header_flags=0,
|
|
payload=b"",
|
|
expect_response=True,
|
|
)
|
|
|
|
if not is_ack(cmd) or base_command_from_ack(cmd) != Command.DEVICE_CLOSE:
|
|
raise CommandError(
|
|
f"Unexpected DEVICE_CLOSE response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
async def open_can_channel(
|
|
self,
|
|
channel: int = 1,
|
|
*,
|
|
nominal_bitrate: int = 500_000,
|
|
fd: bool = False,
|
|
data_bitrate: Optional[int] = None,
|
|
brs: bool = False,
|
|
listen_only: bool = False,
|
|
loopback: bool = False,
|
|
auto_detect: bool = False,
|
|
retransmit: bool = False,
|
|
non_iso: bool = False,
|
|
nominal_index: Optional[int] = None,
|
|
data_index: Optional[int] = None,
|
|
can_clock_hz: Optional[int] = None,
|
|
) -> None:
|
|
|
|
# FD-канал настраиваем явными таймингами (рабочий путь): одиночный индекс
|
|
# data-скорости прошивкой не применяется, канал открывается, но FD/BRS-кадры
|
|
# не уходят в шину. Явное переопределение индексами/auto_detect не трогаем.
|
|
if fd and not (auto_detect or nominal_index is not None or data_index is not None):
|
|
if data_bitrate is None:
|
|
raise ValueError("fd=True требует указать data_bitrate")
|
|
clock = can_clock_hz or DEFAULT_CAN_CLOCK_HZ
|
|
await self.open_can_channel_custom(
|
|
channel,
|
|
nominal_timing=_resolve_timing(
|
|
_NOMINAL_TIMINGS_120M, nominal_bitrate, clock, "nominal"
|
|
),
|
|
data_timing=_resolve_timing(
|
|
_DATA_TIMINGS_120M, data_bitrate, clock, "data"
|
|
),
|
|
fd=True,
|
|
brs=brs,
|
|
listen_only=listen_only,
|
|
loopback=loopback,
|
|
retransmit=retransmit,
|
|
non_iso=non_iso,
|
|
)
|
|
return
|
|
|
|
if loopback:
|
|
mode_val = 0x02
|
|
elif listen_only:
|
|
mode_val = 0x01
|
|
else:
|
|
mode_val = 0x00
|
|
|
|
cc_can_mode = 0x11000000 | mode_val
|
|
|
|
if not fd:
|
|
frame_mode = 0x00
|
|
else:
|
|
frame_mode = 0x02 if brs else 0x01
|
|
|
|
cc_can_frame = 0x12000000 | frame_mode
|
|
|
|
if auto_detect:
|
|
n_index = 0xFF
|
|
elif nominal_index is not None:
|
|
n_index = nominal_index & 0xFF
|
|
else:
|
|
try:
|
|
n_index = NOMINAL_BITRATE_INDEX[nominal_bitrate]
|
|
except KeyError:
|
|
raise ValueError(
|
|
f"Unsupported nominal bitrate {nominal_bitrate}. "
|
|
f"Known: {sorted(NOMINAL_BITRATE_INDEX.keys())}"
|
|
) from None
|
|
|
|
cc_bus_speed_n = 0x01000000 | (n_index & 0xFF)
|
|
|
|
params: List[int] = [cc_can_mode, cc_can_frame, cc_bus_speed_n]
|
|
|
|
if fd:
|
|
if data_index is not None:
|
|
d_index = data_index & 0xFF
|
|
else:
|
|
if data_bitrate is None:
|
|
raise ValueError("fd=True требует указать data_bitrate или data_index")
|
|
try:
|
|
d_index = DATA_BITRATE_INDEX[data_bitrate]
|
|
except KeyError:
|
|
raise ValueError(
|
|
f"Unsupported data bitrate {data_bitrate}. "
|
|
f"Known: {sorted(DATA_BITRATE_INDEX.keys())}"
|
|
) from None
|
|
|
|
cc_bus_speed_d = 0x02000000 | (d_index & 0xFF)
|
|
params.append(cc_bus_speed_d)
|
|
|
|
if retransmit:
|
|
params.append(0x13000001)
|
|
|
|
if fd and non_iso:
|
|
params.append(0x14000001)
|
|
|
|
payload = b"".join(p.to_bytes(4, "little") for p in params)
|
|
|
|
header_flags = (channel & 0x0F) * 0x20
|
|
|
|
cmd, flags, resp_payload = await self._send_raw(
|
|
Command.CHANNEL_OPEN,
|
|
header_flags=header_flags,
|
|
payload=payload,
|
|
expect_response=True,
|
|
)
|
|
|
|
if not is_ack(cmd) or base_command_from_ack(cmd) != Command.CHANNEL_OPEN:
|
|
raise CommandError(
|
|
f"Unexpected CHANNEL_OPEN response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
async def open_can_channel_custom(
|
|
self,
|
|
channel: int = 1,
|
|
*,
|
|
nominal_timing: CanTiming | None,
|
|
data_timing: CanTiming | None = None,
|
|
fd: bool = False,
|
|
brs: bool = False,
|
|
listen_only: bool = False,
|
|
loopback: bool = False,
|
|
retransmit: bool = False,
|
|
non_iso: bool = False,
|
|
) -> None:
|
|
|
|
def _build_bus_custom_baudrate_words(
|
|
base_cc: int,
|
|
prescaler: int,
|
|
seg1: int,
|
|
seg2: int,
|
|
sjw: int,
|
|
) -> list[int]:
|
|
|
|
packed = struct.pack("<HHHH", prescaler, seg1, seg2, sjw)
|
|
word1 = int.from_bytes(packed[0:4], "little")
|
|
word2 = int.from_bytes(packed[4:8], "little")
|
|
|
|
length_words = 2
|
|
header = base_cc | CC_MULTIWORD | ((length_words & 0xFF) << 16)
|
|
return [header, word1, word2]
|
|
|
|
# 1) CAN mode
|
|
if loopback:
|
|
mode_val = 0x02
|
|
elif listen_only:
|
|
mode_val = 0x01
|
|
else:
|
|
mode_val = 0x00
|
|
cc_can_mode = 0x11000000 | mode_val
|
|
|
|
# 2) CAN frame (classic/FD/BRS)
|
|
if not fd:
|
|
frame_mode = 0x00 # classic
|
|
else:
|
|
frame_mode = 0x02 if brs else 0x01
|
|
cc_can_frame = 0x12000000 | frame_mode
|
|
|
|
params: list[int] = [cc_can_mode, cc_can_frame]
|
|
|
|
# 3) Nominal custom bitrate (CC_BUS_SPEED_N)
|
|
if nominal_timing is not None:
|
|
presc=nominal_timing.prescaler
|
|
seg1=nominal_timing.tq_seg1
|
|
seg2=nominal_timing.tq_seg2
|
|
sjw=nominal_timing.sjw
|
|
header_n = 0x01000000 | CC_MULTIWORD | (2 << 16)
|
|
params.append(header_n)
|
|
|
|
b = struct.pack("<HHHH", presc, seg1, seg2, sjw) # BusCustomBaudRate
|
|
params.append(int.from_bytes(b[0:4], "little"))
|
|
params.append(int.from_bytes(b[4:8], "little"))
|
|
|
|
# 4) Data custom bitrate (CC_BUS_SPEED_D)
|
|
if fd and data_timing is not None:
|
|
presc=data_timing.prescaler
|
|
seg1=data_timing.tq_seg1
|
|
seg2=data_timing.tq_seg2
|
|
sjw=data_timing.sjw
|
|
header_d = 0x02000000 | CC_MULTIWORD | (2 << 16)
|
|
params.append(header_d)
|
|
|
|
b = struct.pack("<HHHH", presc, seg1, seg2, sjw)
|
|
params.append(int.from_bytes(b[0:4], "little"))
|
|
params.append(int.from_bytes(b[4:8], "little"))
|
|
|
|
# доп. опции
|
|
if retransmit:
|
|
params.append(0x13000001)
|
|
if non_iso:
|
|
params.append(0x14000001)
|
|
|
|
payload = b"".join(p.to_bytes(4, "little") for p in params)
|
|
|
|
header_flags = (channel & 0x0F) * 0x20
|
|
cmd, flags, resp_payload = await self._send_raw(
|
|
Command.CHANNEL_OPEN,
|
|
header_flags=header_flags,
|
|
payload=payload,
|
|
expect_response=True,
|
|
)
|
|
|
|
if not is_ack(cmd) or base_command_from_ack(cmd) != Command.CHANNEL_OPEN:
|
|
raise CommandError(
|
|
f"Unexpected CHANNEL_OPEN response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
|
|
async def set_can_filter(
|
|
self,
|
|
channel: int,
|
|
index: int,
|
|
*,
|
|
can_id: int,
|
|
mask: int | None = None,
|
|
extended: bool = False,
|
|
) -> None:
|
|
|
|
if index < 0:
|
|
raise ValueError("filter index must be >= 0")
|
|
|
|
filter_type = 0x01 if extended else 0x00
|
|
|
|
if mask is None:
|
|
mask = 0x1FFFFFFF if extended else 0x7FF
|
|
|
|
payload = struct.pack("<IIII", index, filter_type, can_id, mask)
|
|
|
|
header_flags = (channel & 0x0F) * 0x20
|
|
|
|
cmd, flags, resp_payload = await self._send_raw(
|
|
Command.FILTER_SET,
|
|
header_flags=header_flags,
|
|
payload=payload,
|
|
expect_response=True,
|
|
)
|
|
|
|
if not is_ack(cmd) or base_command_from_ack(cmd) != Command.FILTER_SET:
|
|
raise CommandError(
|
|
f"Unexpected FILTER_SET response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
async def set_std_id_filter(
|
|
self,
|
|
channel: int,
|
|
index: int,
|
|
can_id: int,
|
|
mask: int = 0x7FF,
|
|
) -> None:
|
|
|
|
if can_id < 0 or can_id > 0x7FF:
|
|
raise ValueError("can_id for standard ID must be in 0x000..0x7FF")
|
|
|
|
await self.set_can_filter(
|
|
channel=channel,
|
|
index=index,
|
|
can_id=can_id,
|
|
mask=mask,
|
|
extended=False,
|
|
)
|
|
|
|
async def set_ext_id_filter(
|
|
self,
|
|
channel: int,
|
|
index: int,
|
|
can_id: int,
|
|
mask: int = 0x1FFFFFFF,
|
|
) -> None:
|
|
|
|
if can_id < 0 or can_id > 0x1FFFFFFF:
|
|
raise ValueError("can_id for extended ID must be in 0x00000000..0x1FFFFFFF")
|
|
|
|
await self.set_can_filter(
|
|
channel=channel,
|
|
index=index,
|
|
can_id=can_id,
|
|
mask=mask,
|
|
extended=True,
|
|
)
|
|
|
|
async def clear_all_filters(
|
|
self,
|
|
channel: int,
|
|
*,
|
|
max_filters: int = 64,
|
|
stop_on_error: bool = True,
|
|
) -> int:
|
|
|
|
cleared = 0
|
|
for idx in range(max_filters):
|
|
try:
|
|
await self.clear_can_filter(channel=channel, index=idx)
|
|
cleared += 1
|
|
except CommandError as e:
|
|
if stop_on_error:
|
|
self._log.debug(
|
|
"Stop clearing filters on channel %d at index %d due to error: %s",
|
|
channel,
|
|
idx,
|
|
e,
|
|
)
|
|
break
|
|
else:
|
|
self._log.warning(
|
|
"Error while clearing filter %d on channel %d: %s",
|
|
idx,
|
|
channel,
|
|
e,
|
|
)
|
|
continue
|
|
|
|
return cleared
|
|
|
|
async def clear_can_filter(
|
|
self,
|
|
channel: int,
|
|
index: int,
|
|
) -> None:
|
|
|
|
if index < 0:
|
|
raise ValueError("filter index must be >= 0")
|
|
|
|
payload = struct.pack("<I", index)
|
|
header_flags = (channel & 0x0F) * 0x20
|
|
|
|
cmd, flags, resp_payload = await self._send_raw(
|
|
Command.FILTER_CLEAR,
|
|
header_flags=header_flags,
|
|
payload=payload,
|
|
expect_response=True,
|
|
)
|
|
|
|
if not is_ack(cmd) or base_command_from_ack(cmd) != Command.FILTER_CLEAR:
|
|
raise CommandError(
|
|
f"Unexpected FILTER_CLEAR response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
async def set_terminator(self, channel: int, enabled: bool) -> None:
|
|
|
|
state = 0x01 if enabled else 0x00
|
|
channel_flag = (channel & 0x0F) * 0x20
|
|
header_flags = channel_flag | FLAG_CONFIG_TERMINATOR
|
|
|
|
payload = bytes((state,))
|
|
|
|
cmd, flags, resp_payload = await self._send_raw(
|
|
Command.CHANNEL_CONFIG,
|
|
header_flags=header_flags,
|
|
payload=payload,
|
|
expect_response=True,
|
|
)
|
|
|
|
if not is_ack(cmd) or base_command_from_ack(cmd) != Command.CHANNEL_CONFIG:
|
|
raise CommandError(
|
|
f"Unexpected TERMINATOR response: cmd=0x{cmd:02X}, flags=0x{flags:04X}"
|
|
)
|
|
|
|
|
|
async def send_can(
|
|
self,
|
|
msg: CanMessage,
|
|
*,
|
|
channel: int,
|
|
confirm: bool = False,
|
|
echo: bool = False,
|
|
) -> None:
|
|
|
|
if channel == 1:
|
|
hflags = int(HeaderFlags.CHANNEL_1)
|
|
elif channel == 2:
|
|
hflags = int(HeaderFlags.CHANNEL_2)
|
|
elif channel == 3:
|
|
hflags = int(HeaderFlags.CHANNEL_3)
|
|
elif channel == 4:
|
|
hflags = int(HeaderFlags.CHANNEL_4)
|
|
else:
|
|
hflags = (channel & 0x0F) * 0x20
|
|
|
|
if confirm:
|
|
hflags |= int(HeaderFlags.CONFIRM_REQUIRED)
|
|
|
|
mflags = BusMessageFlags.NONE
|
|
if msg.extended:
|
|
mflags |= BusMessageFlags.EXTID
|
|
if msg.rtr:
|
|
mflags |= BusMessageFlags.RTR
|
|
if msg.fd:
|
|
mflags |= BusMessageFlags.FDF
|
|
if msg.brs:
|
|
mflags |= BusMessageFlags.BRS
|
|
if not echo:
|
|
mflags |= BusMessageFlags.BLOCK_TX
|
|
|
|
if msg.extended:
|
|
raw_id = msg.can_id & 0x1FFFFFFF
|
|
else:
|
|
raw_id = (msg.can_id & 0x7FF) #<< 16
|
|
|
|
timestamp = 0
|
|
dlc = len(msg.data) & 0xFF
|
|
|
|
header_struct = struct.pack(
|
|
"<IIII",
|
|
int(mflags),
|
|
int(timestamp),
|
|
int(raw_id),
|
|
int(dlc),
|
|
)
|
|
payload = header_struct + msg.data
|
|
|
|
await self._send_raw(
|
|
Command.MESSAGE,
|
|
header_flags=hflags,
|
|
payload=payload,
|
|
expect_response=confirm,
|
|
)
|
|
|
|
async def receive_can(self) -> tuple[int, CanMessage]:
|
|
return await self._rx_queue.get()
|
|
|
|
async def receive_can_on(self, channel: int = 1) -> CanMessage:
|
|
q = self._ensure_channel_queue(channel)
|
|
return await q.get()
|
|
|
|
async def receive_can_on_timeout(
|
|
self,
|
|
channel: int = 1,
|
|
timeout: float = 1.0,
|
|
) -> CanMessage | None:
|
|
try:
|
|
return await asyncio.wait_for(self.receive_can_on(channel), timeout=timeout)
|
|
except asyncio.TimeoutError:
|
|
return None
|
|
|
|
async def _read_loop(self) -> None:
|
|
try:
|
|
while not self._closed:
|
|
cmd_bytes = await self._reader.readexactly(1)
|
|
if not cmd_bytes:
|
|
break
|
|
cmd = cmd_bytes[0]
|
|
|
|
if need_extended_header(cmd):
|
|
header_rest = await self._reader.readexactly(5)
|
|
header = MsgCommandHeader.from_bytes(cmd_bytes + header_rest)
|
|
flags = header.flags
|
|
dsize = header.dsize
|
|
seq = header.sequence
|
|
else:
|
|
header_rest = await self._reader.readexactly(3)
|
|
header = CommandHeader.from_bytes(cmd_bytes + header_rest)
|
|
flags = header.flags
|
|
dsize = header.dsize
|
|
seq = header.sequence
|
|
|
|
payload = b""
|
|
if dsize:
|
|
payload = await self._reader.readexactly(dsize)
|
|
|
|
full_frame = cmd_bytes + header_rest + payload
|
|
self._wire_log.debug(
|
|
"RX cmd=0x%02X seq=%d flags=0x%04X dsize=%d :: %s",
|
|
cmd,
|
|
seq,
|
|
flags,
|
|
dsize,
|
|
full_frame.hex(" "),
|
|
)
|
|
|
|
if cmd == Command.ERROR:
|
|
pending = self._pending.pop(seq, None)
|
|
if pending is not None and not pending.future.done():
|
|
pending.future.set_exception(
|
|
CommandError(
|
|
f"Device ERROR for seq={seq}, "
|
|
f"flags=0x{flags:04X}, payload={payload.hex()}"
|
|
)
|
|
)
|
|
continue
|
|
|
|
if is_ack(cmd):
|
|
base_cmd = base_command_from_ack(cmd)
|
|
pending = self._pending.pop(seq, None)
|
|
if pending is not None and not pending.future.done():
|
|
pending.future.set_result((cmd, flags, payload))
|
|
continue
|
|
|
|
if seq in self._pending:
|
|
pending = self._pending.pop(seq)
|
|
if not pending.future.done():
|
|
pending.future.set_result((cmd, flags, payload))
|
|
continue
|
|
|
|
if cmd == Command.MESSAGE:
|
|
await self._handle_bus_message(flags, payload)
|
|
elif cmd == Command.BUS_ERROR:
|
|
await self._handle_bus_error(flags, payload)
|
|
else:
|
|
self._log.debug(
|
|
"Unhandled async command: cmd=0x%02X, flags=0x%04X, payload=%s",
|
|
cmd,
|
|
flags,
|
|
payload.hex(" "),
|
|
)
|
|
continue
|
|
|
|
except asyncio.IncompleteReadError:
|
|
self._closed = True
|
|
except Exception as e:
|
|
self._closed = True
|
|
self._log.exception("Read loop exception: %s", e)
|
|
finally:
|
|
for pending in list(self._pending.values()):
|
|
if not pending.future.done():
|
|
pending.future.set_exception(
|
|
CarBusError("Read loop terminated before response was received")
|
|
)
|
|
self._pending.clear()
|
|
|
|
async def _handle_bus_message(self, header_flags: int, payload: bytes) -> None:
|
|
if len(payload) < 16:
|
|
return
|
|
|
|
flags_val, timestamp_us, _reserved, id_raw, dlc = struct.unpack_from("<IIIII", payload, 0)
|
|
data = payload[20:20 + dlc]
|
|
|
|
bus_flags = BusMessageFlags(flags_val)
|
|
|
|
if bus_flags & BusMessageFlags.EXTID:
|
|
can_id = id_raw & 0x1FFFFFFF
|
|
else:
|
|
can_id = (id_raw) & 0x7FF
|
|
|
|
if header_flags & int(HeaderFlags.CHANNEL_1):
|
|
channel = 1
|
|
elif header_flags & int(HeaderFlags.CHANNEL_2):
|
|
channel = 2
|
|
elif header_flags & int(HeaderFlags.CHANNEL_3):
|
|
channel = 3
|
|
elif header_flags & int(HeaderFlags.CHANNEL_4):
|
|
channel = 4
|
|
else:
|
|
channel = 0
|
|
|
|
msg = CanMessage.from_bus_payload(
|
|
flags=bus_flags,
|
|
timestamp_us=timestamp_us,
|
|
can_id=can_id,
|
|
dlc=dlc,
|
|
data=data,
|
|
)
|
|
|
|
self._fire_can_hooks(channel, msg)
|
|
|
|
await self._rx_queue.put((channel, msg))
|
|
|
|
if channel != 0:
|
|
q = self._rx_channel_queues.get(channel)
|
|
if q is not None:
|
|
await q.put(msg)
|
|
|
|
|
|
async def _handle_bus_error(self, header_flags: int, payload: bytes) -> None:
|
|
self._log.warning(
|
|
"BUS_ERROR: flags=0x%04X, payload=%s", header_flags, payload.hex(" ")
|
|
)
|
|
|
|
|
|
async def _example() -> None:
|
|
dev = await CarBusDevice.open("COM6", baudrate=115200)
|
|
|
|
info = await dev.get_device_info()
|
|
print("DEVICE_INFO raw:", info.raw_payload.hex(" "))
|
|
for p in info.params:
|
|
print(f"DI header=0x{p.header:08X}, data={[f'0x{w:08X}' for w in p.data]}")
|
|
|
|
await dev.open_can_channel(channel=1, nominal_bitrate=500_000, fd=False)
|
|
|
|
msg = CanMessage(can_id=0x123, data=b"\x01\x02\x03\x04", dlc=4, channel=1)
|
|
await dev.send_can(msg, confirm=False, echo=True)
|
|
|
|
rx = await dev.receive_can()
|
|
print("RX CAN:", rx)
|
|
|
|
await dev.close()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
logging.basicConfig(
|
|
level=logging.DEBUG,
|
|
format="%(asctime)s [%(levelname)s] %(name)s: %(message)s",
|
|
)
|
|
with contextlib.suppress(KeyboardInterrupt):
|
|
asyncio.run(_example())
|