155 lines
5.0 KiB
Python
155 lines
5.0 KiB
Python
# app/obd2.py — OBD-II Responder (SocketCAN) + Payload-Helpers
|
|
from __future__ import annotations
|
|
|
|
import logging
|
|
import threading
|
|
import time
|
|
from typing import Callable, Dict, Optional
|
|
|
|
import can
|
|
|
|
from .can import link_state # nur für "IF ist UP?"-Check
|
|
|
|
# 11-bit OBD-II IDs (ISO 15765-4)
|
|
OBD_REQ_ID = 0x7DF
|
|
PID_SPEED = 0x0D
|
|
PID_RPM = 0x0C
|
|
|
|
class ObdResponder:
|
|
"""
|
|
OBD-II Mode 01 PID-Responder (11-bit) über SocketCAN.
|
|
- eigener Polling-RX-Loop (kein Notifier -> keine Stacktraces beim Link DOWN)
|
|
- rebind(interface/resp_id) schließt Bus; Loop öffnet neu, sobald IF=UP
|
|
"""
|
|
def __init__(
|
|
self,
|
|
interface: str,
|
|
resp_id: int,
|
|
timeout_ms: int = 200,
|
|
logger: Optional[logging.Logger] = None,
|
|
):
|
|
self.interface = interface
|
|
self.resp_id = resp_id
|
|
self.timeout_ms = timeout_ms
|
|
self.log = logger or logging.getLogger("obdcan")
|
|
|
|
self.providers: Dict[int, Callable[[], bytes]] = {}
|
|
self._run = threading.Event(); self._run.set()
|
|
|
|
self.bus: Optional[can.BusABC] = None
|
|
self._thread = threading.Thread(target=self._service_loop, name="OBD-SVC", daemon=True)
|
|
self._thread.start()
|
|
|
|
# ---------- Lifecycle ----------
|
|
def _open_bus(self) -> None:
|
|
self._close_bus()
|
|
self.bus = can.interface.Bus(channel=self.interface, interface="socketcan")
|
|
self.log.info("OBD responder started on %s (resp_id=0x%03X)", self.interface, self.resp_id)
|
|
|
|
def _close_bus(self) -> None:
|
|
try:
|
|
if self.bus:
|
|
self.bus.shutdown()
|
|
except Exception:
|
|
pass
|
|
self.bus = None
|
|
|
|
def stop(self) -> None:
|
|
self._run.clear()
|
|
try:
|
|
self._thread.join(timeout=1.0)
|
|
except RuntimeError:
|
|
pass
|
|
self._close_bus()
|
|
|
|
def rebind(self, interface: Optional[str] = None, resp_id: Optional[int] = None) -> None:
|
|
if interface is not None:
|
|
self.interface = interface
|
|
if resp_id is not None:
|
|
self.resp_id = resp_id
|
|
self._close_bus()
|
|
self.log.info("Rebind requested: %s, resp=0x%03X", self.interface, self.resp_id)
|
|
|
|
# ---------- API ----------
|
|
def register_pid(self, pid: int, provider: Callable[[], bytes]) -> None:
|
|
self.providers[pid] = provider
|
|
|
|
# ---------- RX/Service ----------
|
|
def _service_loop(self) -> None:
|
|
backoff = 0.5
|
|
while self._run.is_set():
|
|
if self.bus is None:
|
|
# Öffnen nur, wenn Interface UP ist
|
|
if link_state(self.interface) == "UP":
|
|
try:
|
|
self._open_bus()
|
|
backoff = 0.5
|
|
except Exception as e:
|
|
self.log.warning("Bus open failed: %s", e)
|
|
time.sleep(backoff); backoff = min(5.0, backoff * 1.7)
|
|
continue
|
|
else:
|
|
time.sleep(0.5); continue
|
|
|
|
try:
|
|
msg = self.bus.recv(0.05)
|
|
if msg is not None:
|
|
self._handle(msg)
|
|
except (can.CanOperationError, OSError):
|
|
# IF ging down -> Bus schließen und später neu öffnen
|
|
self.log.info("CAN went DOWN — closing bus, will retry…")
|
|
self._close_bus()
|
|
time.sleep(0.5)
|
|
except Exception as e:
|
|
self.log.warning("CAN recv error: %s", e)
|
|
time.sleep(0.1)
|
|
|
|
def _handle(self, msg: can.Message) -> None:
|
|
if msg.is_extended_id or msg.arbitration_id != OBD_REQ_ID:
|
|
return
|
|
data = bytes(msg.data)
|
|
if len(data) < 2:
|
|
return
|
|
|
|
# tolerant: 02 01 <pid> ... oder 01 <pid> ...
|
|
if data[0] == 0x02 and len(data) >= 3:
|
|
mode, pid = data[1], data[2]
|
|
else:
|
|
mode, pid = data[0], (data[1] if len(data) >= 2 else None)
|
|
|
|
if mode != 0x01 or pid is None:
|
|
return
|
|
|
|
provider = self.providers.get(pid)
|
|
if not provider:
|
|
return
|
|
|
|
try:
|
|
payload = provider()
|
|
if not isinstance(payload, (bytes, bytearray)) or len(payload) != 8:
|
|
return
|
|
out = can.Message(
|
|
arbitration_id=self.resp_id,
|
|
is_extended_id=False,
|
|
data=payload,
|
|
dlc=8
|
|
)
|
|
if self.bus:
|
|
self.bus.send(out)
|
|
except can.CanError:
|
|
self.log.warning("CAN send failed (bus off?)")
|
|
except Exception as e:
|
|
self.log.exception("Provider error: %s", e)
|
|
|
|
# ------------------------- Payload-Helpers ----------------------------------
|
|
|
|
def make_speed_response(speed_kmh: int) -> bytes:
|
|
A = max(0, min(255, int(speed_kmh)))
|
|
return bytes([0x03, 0x41, PID_SPEED, A, 0x00, 0x00, 0x00, 0x00])
|
|
|
|
def make_rpm_response(rpm: int) -> bytes:
|
|
raw = max(0, int(rpm)) * 4
|
|
A = (raw >> 8) & 0xFF
|
|
B = raw & 0xFF
|
|
return bytes([0x04, 0x41, PID_RPM, A, B, 0x00, 0x00, 0x00])
|