80 lines
2.4 KiB
Python
80 lines
2.4 KiB
Python
"""Addressed SETProtocol v2 PING exchange over segmented classic CAN."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
from time import perf_counter
|
|
|
|
from setprotocol.can import CanAddress, CanFrame, CanReassembler, segment
|
|
from setprotocol.core import (
|
|
Frame as SetFrame,
|
|
FrameFlag,
|
|
MessageType,
|
|
SetProtocolError,
|
|
Status,
|
|
build_frame,
|
|
decode_datagram,
|
|
decode_response,
|
|
)
|
|
|
|
from . import can_transport as tr
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class CanPingReply:
|
|
node_id: int
|
|
uptime_ms: int
|
|
protocol_version: int = 2
|
|
|
|
|
|
class CanPingExchange:
|
|
def __init__(self, node_id: int, sequence: int = 1, channel: int = 1) -> None:
|
|
if not 0 <= node_id <= 0xFF:
|
|
raise ValueError("SETP node ID вне диапазона 0..255")
|
|
self.node_id = node_id
|
|
self.sequence = sequence & 0xFFFF or 1
|
|
self.channel = channel
|
|
self._reassembler = CanReassembler()
|
|
|
|
def request(self) -> list[tr.Frame]:
|
|
packet = build_frame(
|
|
SetFrame(
|
|
MessageType.PING,
|
|
self.sequence,
|
|
flags=FrameFlag.ACK_REQUIRED | FrameFlag.PRIORITY,
|
|
source=0,
|
|
destination=self.node_id,
|
|
)
|
|
)
|
|
return [
|
|
tr.build_frame(item.can_id, item.data, to_can=True)
|
|
for item in segment(packet, CanAddress(self.node_id, 0, 1, self.channel))
|
|
]
|
|
|
|
def feed(self, frame: tr.Frame) -> CanPingReply | None:
|
|
if frame.to_can or not frame.ide:
|
|
return None
|
|
try:
|
|
address = CanAddress.unpack(frame.can_id)
|
|
if address.source != self.node_id or address.destination != 0:
|
|
return None
|
|
packet = self._reassembler.feed(
|
|
CanFrame(frame.can_id, frame.data), int(perf_counter() * 1000)
|
|
)
|
|
if packet is None:
|
|
return None
|
|
response = decode_datagram(packet)
|
|
status, body = decode_response(response)
|
|
except SetProtocolError:
|
|
return None
|
|
if (
|
|
response.message_type != MessageType.PING
|
|
or response.sequence != self.sequence
|
|
or response.source != self.node_id
|
|
or response.destination != 0
|
|
or status != Status.OK
|
|
or len(body) != 4
|
|
):
|
|
return None
|
|
return CanPingReply(self.node_id, int.from_bytes(body, "little"))
|