mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-31 05:03:42 +08:00
OBDyssey
This commit is contained in:
@@ -139,6 +139,19 @@ def test_primary_device_action_is_pair_then_connect_then_manage():
|
||||
assert managed == [ADDRESS]
|
||||
|
||||
|
||||
def test_obd_device_action_opens_obdyssey_directly_when_paired():
|
||||
obd_device = BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDLink MX+", paired=True, serial=True)
|
||||
manager = FakeBluetoothManager(BluetoothStatus(offroad=True, devices=(obd_device,)))
|
||||
ui = make_ui(manager)
|
||||
|
||||
opened = []
|
||||
ui._open_obdyssey = lambda: opened.append(True)
|
||||
ui._select_device("AA:BB:CC:DD:EE:FF")
|
||||
|
||||
assert opened == [True]
|
||||
assert manager.calls == []
|
||||
|
||||
|
||||
def test_scan_is_only_requested_when_the_existing_daemon_policy_allows_it():
|
||||
manager = FakeBluetoothManager(BluetoothStatus(enabled=True, offroad=True))
|
||||
ui = make_ui(manager)
|
||||
|
||||
@@ -0,0 +1,223 @@
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
os.environ.setdefault("SP_HEADLESS_TEST", "1")
|
||||
|
||||
from openpilot.starpilot.system.bluetooth.protocol import BluetoothDevice, BluetoothStatus, looks_like_obd_device
|
||||
from openpilot.starpilot.system.bluetooth.tests.test_bluetooth import FakeParams
|
||||
from openpilot.starpilot.system.obdyssey.protocol import OBDysseyStatus
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets.bluetooth import device_status_text
|
||||
from openpilot.system.ui.widgets.obdyssey import OBDysseyScreen
|
||||
from openpilot.system.ui.widgets.obdyssey import DTC_STATE_CLEAN, DTC_STATE_FAULTS, DTC_STATE_IN_PROGRESS, DTC_STATE_UNAVAILABLE
|
||||
|
||||
|
||||
class FakeOBDysseyClient:
|
||||
def __init__(self, connected: bool = True):
|
||||
self.is_connected = connected
|
||||
self.cleared = False
|
||||
self.connect_calls = 0
|
||||
self.dtcs = [{"code": "P0133", "source": "OBD_STORED", "description": "O2 Sensor Slow Response"}]
|
||||
self.signals = {
|
||||
"SAE_ENGINE_RPM": 2100,
|
||||
"SAE_VEHICLE_SPEED": 65,
|
||||
"SAE_ENGINE_COOLANT_TEMP": 90,
|
||||
"BOLT_HVBAT_SOC": 78.5,
|
||||
}
|
||||
|
||||
def status(self) -> OBDysseyStatus:
|
||||
return OBDysseyStatus(
|
||||
connected=self.is_connected,
|
||||
state="ready" if self.is_connected else "disconnected",
|
||||
adapter_name="OBDLink MX+",
|
||||
elm_identity="ELM327 v1.5",
|
||||
adapter_voltage=13.9,
|
||||
profile="Chevrolet-Bolt-EV",
|
||||
)
|
||||
|
||||
def connect(self) -> dict:
|
||||
self.connect_calls += 1
|
||||
self.is_connected = True
|
||||
return {"ok": True}
|
||||
|
||||
def list_signals(self) -> list[dict]:
|
||||
return [{"id": k, "name": k} for k in self.signals.keys()]
|
||||
|
||||
def read_signals(self, ids: list[str]) -> dict:
|
||||
return {k: self.signals[k] for k in ids if k in self.signals}
|
||||
|
||||
def read_dtcs(self) -> list[dict]:
|
||||
return self.dtcs
|
||||
|
||||
def clear_dtcs(self) -> dict:
|
||||
self.cleared = True
|
||||
self.dtcs = []
|
||||
return {"ok": True}
|
||||
|
||||
|
||||
def make_obdyssey_screen(client: FakeOBDysseyClient, params: FakeParams) -> OBDysseyScreen:
|
||||
screen = object.__new__(OBDysseyScreen)
|
||||
screen._client = client
|
||||
screen.params = params
|
||||
screen._stop_event = threading.Event()
|
||||
screen._poller_thread = None
|
||||
screen._status = None
|
||||
screen._available_signals = []
|
||||
screen._live_telemetry = {}
|
||||
screen._dtcs = []
|
||||
screen._dtc_state = DTC_STATE_UNAVAILABLE
|
||||
screen._dtc_scan_in_progress = False
|
||||
screen._clear_in_progress = False
|
||||
screen._retry_in_progress = False
|
||||
screen._last_error = ""
|
||||
return screen
|
||||
|
||||
|
||||
def test_device_status_text_includes_obd_capability():
|
||||
dev_obd = BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDII", connected=True, serial=True)
|
||||
status_str = device_status_text(dev_obd, "", "")
|
||||
assert "OBD-II" in status_str
|
||||
assert "Connected" in status_str
|
||||
|
||||
dev_obd_paired = BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDII", paired=True, serial=True)
|
||||
status_paired_str = device_status_text(dev_obd_paired, "", "")
|
||||
assert "Paired - tap to open OBDyssey" in status_paired_str
|
||||
|
||||
|
||||
def test_obd_device_detection_for_bluetooth_header():
|
||||
# Unpaired, not connected OBD device
|
||||
status_unpaired = BluetoothStatus(
|
||||
enabled=True,
|
||||
offroad=True,
|
||||
devices=(
|
||||
BluetoothDevice("11:22:33:44:55:66", "Headphones", connected=True, audio=True),
|
||||
BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDII", connected=False, paired=False, serial=True),
|
||||
)
|
||||
)
|
||||
has_obd_unpaired = any((d.connected or d.paired) and (d.serial or looks_like_obd_device(d.name)) for d in status_unpaired.devices)
|
||||
assert has_obd_unpaired is False
|
||||
|
||||
# Paired OBD device
|
||||
status_paired = BluetoothStatus(
|
||||
enabled=True,
|
||||
offroad=True,
|
||||
devices=(
|
||||
BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDII", connected=False, paired=True, serial=True),
|
||||
)
|
||||
)
|
||||
has_obd_paired = any((d.connected or d.paired) and (d.serial or looks_like_obd_device(d.name)) for d in status_paired.devices)
|
||||
assert has_obd_paired is True
|
||||
|
||||
# Connected OBD device
|
||||
status_connected = BluetoothStatus(
|
||||
enabled=True,
|
||||
offroad=True,
|
||||
devices=(
|
||||
BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDII", connected=True, paired=True, serial=True),
|
||||
)
|
||||
)
|
||||
has_obd_connected = any((d.connected or d.paired) and (d.serial or looks_like_obd_device(d.name)) for d in status_connected.devices)
|
||||
assert has_obd_connected is True
|
||||
|
||||
|
||||
def test_obdyssey_screen_polling_logic():
|
||||
fake_client = FakeOBDysseyClient(connected=True)
|
||||
params = FakeParams(IsOffroad=True)
|
||||
screen = make_obdyssey_screen(fake_client, params)
|
||||
|
||||
# Fetch initial state
|
||||
screen._status = fake_client.status()
|
||||
screen._available_signals = fake_client.list_signals()
|
||||
screen._dtcs = fake_client.read_dtcs()
|
||||
readings = fake_client.read_signals([s["id"] for s in screen._available_signals])
|
||||
screen._live_telemetry.update(readings)
|
||||
|
||||
assert screen._status.connected is True
|
||||
assert screen._live_telemetry["SAE_ENGINE_RPM"] == 2100
|
||||
assert screen._live_telemetry["BOLT_HVBAT_SOC"] == 78.5
|
||||
assert len(screen._dtcs) == 1
|
||||
assert screen._dtcs[0]["code"] == "P0133"
|
||||
|
||||
|
||||
def test_obdyssey_screen_clear_codes_safety_gating(monkeypatch):
|
||||
fake_client = FakeOBDysseyClient(connected=True)
|
||||
params = FakeParams(IsOffroad=False)
|
||||
screen = make_obdyssey_screen(fake_client, params)
|
||||
|
||||
pushed_widgets = []
|
||||
monkeypatch.setattr(gui_app, "push_widget", lambda w: pushed_widgets.append(w))
|
||||
monkeypatch.setattr("openpilot.system.ui.widgets.obdyssey.alert_dialog", lambda msg: SimpleNamespace(message=msg))
|
||||
monkeypatch.setattr(
|
||||
"openpilot.system.ui.widgets.obdyssey.ConfirmDialog",
|
||||
lambda msg, text, callback=None: SimpleNamespace(message=msg, text=text, callback=callback),
|
||||
)
|
||||
|
||||
# 1. Onroad attempt -> Rejected with alert
|
||||
screen._confirm_clear_dtcs()
|
||||
assert len(pushed_widgets) == 1
|
||||
assert "offroad" in str(pushed_widgets[0].message).lower()
|
||||
assert not fake_client.cleared
|
||||
|
||||
# 2. Offroad attempt -> Shows confirmation dialog
|
||||
params.values["IsOffroad"] = True
|
||||
pushed_widgets.clear()
|
||||
screen._confirm_clear_dtcs()
|
||||
assert len(pushed_widgets) == 1
|
||||
assert "Clear Codes" in str(pushed_widgets[0].text)
|
||||
|
||||
# Trigger clear worker directly
|
||||
screen._clear_dtcs_worker()
|
||||
# Wait for worker thread
|
||||
deadline = threading.Event()
|
||||
deadline.wait(0.6)
|
||||
assert fake_client.cleared is True
|
||||
assert len(screen._dtcs) == 0
|
||||
|
||||
|
||||
def test_obdyssey_dtc_state_requires_successful_scan():
|
||||
fake_client = FakeOBDysseyClient(connected=True)
|
||||
params = FakeParams(IsOffroad=True)
|
||||
screen = make_obdyssey_screen(fake_client, params)
|
||||
screen._status = fake_client.status()
|
||||
|
||||
assert screen._dtc_state == DTC_STATE_UNAVAILABLE
|
||||
|
||||
screen._dtc_state = DTC_STATE_IN_PROGRESS
|
||||
fake_client.dtcs = []
|
||||
screen._scan_dtcs_worker()
|
||||
assert screen._dtc_state == DTC_STATE_CLEAN
|
||||
|
||||
fake_client.dtcs = [{"code": "P0133"}]
|
||||
screen._dtc_state = DTC_STATE_IN_PROGRESS
|
||||
screen._scan_dtcs_worker()
|
||||
assert screen._dtc_state == DTC_STATE_FAULTS
|
||||
|
||||
|
||||
def test_obdyssey_failed_dtc_scan_is_unavailable_not_clean():
|
||||
fake_client = FakeOBDysseyClient(connected=True)
|
||||
params = FakeParams(IsOffroad=True)
|
||||
screen = make_obdyssey_screen(fake_client, params)
|
||||
fake_client.read_dtcs = lambda: (_ for _ in ()).throw(RuntimeError("adapter unavailable"))
|
||||
screen._dtc_state = DTC_STATE_IN_PROGRESS
|
||||
|
||||
screen._scan_dtcs_worker()
|
||||
|
||||
assert screen._dtc_state == DTC_STATE_UNAVAILABLE
|
||||
assert screen._dtcs == []
|
||||
assert "adapter unavailable" in screen._last_error
|
||||
|
||||
|
||||
def test_obdyssey_retry_uses_explicit_daemon_connect():
|
||||
fake_client = FakeOBDysseyClient(connected=False)
|
||||
params = FakeParams(IsOffroad=True)
|
||||
screen = make_obdyssey_screen(fake_client, params)
|
||||
|
||||
screen._retry_connection()
|
||||
deadline = time.monotonic() + 1.0
|
||||
while fake_client.connect_calls == 0 and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
|
||||
assert fake_client.connect_calls == 1
|
||||
assert screen._retry_in_progress is False
|
||||
@@ -1,3 +1,5 @@
|
||||
import os
|
||||
import socket
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
@@ -5,11 +7,13 @@ import uuid
|
||||
from typing import Any
|
||||
|
||||
from jeepney import DBusAddress, MatchRule, new_error, new_method_call, new_method_return
|
||||
from jeepney.fds import FileDescriptor
|
||||
from jeepney.io.threading import DBusRouter, open_dbus_connection
|
||||
from jeepney.low_level import HeaderFields, MessageType
|
||||
from jeepney.wrappers import Properties
|
||||
|
||||
from openpilot.starpilot.system.bluetooth.protocol import device_capabilities, show_pairing_device
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.starpilot.system.bluetooth.protocol import SPP_UUID, device_capabilities, show_pairing_device
|
||||
|
||||
|
||||
BLUEZ = "org.bluez"
|
||||
@@ -19,6 +23,50 @@ DEVICE_IFACE = "org.bluez.Device1"
|
||||
AGENT_MANAGER_IFACE = "org.bluez.AgentManager1"
|
||||
AGENT_IFACE = "org.bluez.Agent1"
|
||||
AGENT_PATH = "/link/firestar/starpilot/agent"
|
||||
PROFILE_MANAGER_IFACE = "org.bluez.ProfileManager1"
|
||||
PROFILE_IFACE = "org.bluez.Profile1"
|
||||
OBDYSSEY_PROFILE_PATH = "/link/firestar/starpilot/obdyssey"
|
||||
|
||||
|
||||
class BlueZError(RuntimeError):
|
||||
"""A BlueZ D-Bus error with both its stable name and human detail."""
|
||||
|
||||
def __init__(self, error_name: str, detail: str, *, path: str = "", interface: str = "", member: str = ""):
|
||||
self.error_name = error_name
|
||||
self.detail = detail
|
||||
self.path = path
|
||||
self.interface = interface
|
||||
self.member = member
|
||||
message = error_name if not detail or detail == error_name else f"{error_name}: {detail}"
|
||||
super().__init__(message)
|
||||
|
||||
@property
|
||||
def method(self) -> str:
|
||||
return f"{self.interface}.{self.member}" if self.interface and self.member else self.member
|
||||
|
||||
|
||||
def _socket_from_dbus_fd(fd: Any) -> socket.socket:
|
||||
"""Take ownership of a Profile1 NewConnection file descriptor."""
|
||||
if isinstance(fd, FileDescriptor):
|
||||
try:
|
||||
return fd.to_socket()
|
||||
except Exception:
|
||||
try:
|
||||
fd.close()
|
||||
except Exception:
|
||||
pass
|
||||
raise
|
||||
if isinstance(fd, int):
|
||||
# Raw integer FDs are not owned by the message wrapper, so duplicate them
|
||||
# before handing ownership to the socket object.
|
||||
return socket.socket(fileno=os.dup(fd))
|
||||
raise TypeError(f"Unsupported D-Bus file descriptor: {type(fd).__name__}")
|
||||
|
||||
|
||||
def _dbus_text(value: Any) -> str:
|
||||
if isinstance(value, bytes):
|
||||
return value.decode("utf-8", errors="replace")
|
||||
return str(value)
|
||||
|
||||
|
||||
def unwrap_variant(value: Any) -> Any:
|
||||
@@ -78,9 +126,73 @@ class PairingAgent:
|
||||
return True
|
||||
|
||||
|
||||
class BlueZClient:
|
||||
class _BlueZConnection:
|
||||
def __init__(self, enable_fds: bool = False):
|
||||
self.router = DBusRouter(open_dbus_connection(bus="SYSTEM", enable_fds=enable_fds))
|
||||
|
||||
def close(self) -> None:
|
||||
self.router.close()
|
||||
|
||||
def _call(self, path: str, interface: str, member: str, signature: str | None = None, body: tuple = (), timeout: float = 15.0):
|
||||
address = DBusAddress(path, bus_name=BLUEZ, interface=interface)
|
||||
message = new_method_call(address, member, signature, body) if signature is not None else new_method_call(address, member)
|
||||
reply = self.router.send_and_get_reply(message, timeout=timeout)
|
||||
if reply.header.message_type == MessageType.error:
|
||||
error_name = _dbus_text(reply.header.fields.get(HeaderFields.error_name, "org.bluez.Error.Failed"))
|
||||
detail = _dbus_text(reply.body[0]) if reply.body else error_name
|
||||
raise BlueZError(error_name, detail, path=path, interface=interface, member=member)
|
||||
return reply.body
|
||||
|
||||
def managed_objects(self) -> dict[str, dict[str, dict[str, Any]]]:
|
||||
body = self._call("/", OBJECT_MANAGER, "GetManagedObjects")
|
||||
return unwrap_variant(body[0]) if body else {}
|
||||
|
||||
def adapter(self, objects: dict[str, Any] | None = None) -> tuple[str, dict[str, Any]]:
|
||||
objects = self.managed_objects() if objects is None else objects
|
||||
for path, interfaces in objects.items():
|
||||
if ADAPTER_IFACE in interfaces:
|
||||
return path, interfaces[ADAPTER_IFACE]
|
||||
raise RuntimeError("Bluetooth adapter is not available")
|
||||
|
||||
def devices(self, objects: dict[str, Any] | None = None) -> list[dict[str, Any]]:
|
||||
objects = self.managed_objects() if objects is None else objects
|
||||
devices = []
|
||||
for path, interfaces in objects.items():
|
||||
if DEVICE_IFACE not in interfaces:
|
||||
continue
|
||||
props = interfaces[DEVICE_IFACE]
|
||||
uuids = [str(value).lower() for value in props.get("UUIDs", [])]
|
||||
audio, controller, serial = device_capabilities(uuids, int(props.get("Class", 0)), str(props.get("Icon", "")))
|
||||
device = {
|
||||
"path": path,
|
||||
"address": str(props.get("Address", "")),
|
||||
"name": str(props.get("Alias") or props.get("Name") or props.get("Address") or "Unknown device"),
|
||||
"paired": bool(props.get("Paired", False)),
|
||||
"trusted": bool(props.get("Trusted", False)),
|
||||
"connected": bool(props.get("Connected", False)),
|
||||
"blocked": bool(props.get("Blocked", False)),
|
||||
"rssi": int(props["RSSI"]) if "RSSI" in props else None,
|
||||
"uuids": uuids,
|
||||
"audio": audio,
|
||||
"controller": controller,
|
||||
"serial": serial,
|
||||
}
|
||||
if show_pairing_device(device["address"], device["name"], device["paired"], device["trusted"], device["connected"],
|
||||
device["blocked"], audio, controller, serial):
|
||||
devices.append(device)
|
||||
return sorted(devices, key=lambda device: (not device["connected"], not device["paired"], -(device["rssi"] or -127), device["name"].lower()))
|
||||
|
||||
def device_for_address(self, address: str) -> dict[str, Any]:
|
||||
normalized = address.upper()
|
||||
for device in self.devices():
|
||||
if device["address"].upper() == normalized:
|
||||
return device
|
||||
raise RuntimeError(f"Bluetooth device {address} was not found")
|
||||
|
||||
|
||||
class BlueZClient(_BlueZConnection):
|
||||
def __init__(self):
|
||||
self.router = DBusRouter(open_dbus_connection(bus="SYSTEM"))
|
||||
super().__init__(enable_fds=False)
|
||||
self.agent = PairingAgent()
|
||||
self._agent_filter = self.router.filter(MatchRule(type="method_call", interface=AGENT_IFACE, path=AGENT_PATH), bufsize=20)
|
||||
self._agent_queue = self._agent_filter.__enter__()
|
||||
@@ -94,17 +206,7 @@ class BlueZClient:
|
||||
except Exception:
|
||||
pass
|
||||
self._agent_filter.__exit__(None, None, None)
|
||||
self.router.close()
|
||||
|
||||
def _call(self, path: str, interface: str, member: str, signature: str | None = None, body: tuple = (), timeout: float = 15.0):
|
||||
address = DBusAddress(path, bus_name=BLUEZ, interface=interface)
|
||||
message = new_method_call(address, member, signature, body) if signature is not None else new_method_call(address, member)
|
||||
reply = self.router.send_and_get_reply(message, timeout=timeout)
|
||||
if reply.header.message_type == MessageType.error:
|
||||
error_name = reply.header.fields.get(HeaderFields.error_name, "org.bluez.Error.Failed")
|
||||
detail = reply.body[0] if reply.body else error_name
|
||||
raise RuntimeError(str(detail))
|
||||
return reply.body
|
||||
super().close()
|
||||
|
||||
def _register_agent(self) -> None:
|
||||
self._call("/org/bluez", AGENT_MANAGER_IFACE, "RegisterAgent", "os", (AGENT_PATH, "KeyboardDisplay"))
|
||||
@@ -152,44 +254,6 @@ class BlueZClient:
|
||||
except Exception as error:
|
||||
self.router.send(new_error(message, "org.bluez.Error.Canceled", "s", (str(error),)))
|
||||
|
||||
def managed_objects(self) -> dict[str, dict[str, dict[str, Any]]]:
|
||||
body = self._call("/", OBJECT_MANAGER, "GetManagedObjects")
|
||||
return unwrap_variant(body[0]) if body else {}
|
||||
|
||||
def adapter(self, objects: dict[str, Any] | None = None) -> tuple[str, dict[str, Any]]:
|
||||
objects = self.managed_objects() if objects is None else objects
|
||||
for path, interfaces in objects.items():
|
||||
if ADAPTER_IFACE in interfaces:
|
||||
return path, interfaces[ADAPTER_IFACE]
|
||||
raise RuntimeError("Bluetooth adapter is not available")
|
||||
|
||||
def devices(self, objects: dict[str, Any] | None = None) -> list[dict[str, Any]]:
|
||||
objects = self.managed_objects() if objects is None else objects
|
||||
devices = []
|
||||
for path, interfaces in objects.items():
|
||||
if DEVICE_IFACE not in interfaces:
|
||||
continue
|
||||
props = interfaces[DEVICE_IFACE]
|
||||
uuids = [str(value).lower() for value in props.get("UUIDs", [])]
|
||||
audio, controller = device_capabilities(uuids, int(props.get("Class", 0)), str(props.get("Icon", "")))
|
||||
device = {
|
||||
"path": path,
|
||||
"address": str(props.get("Address", "")),
|
||||
"name": str(props.get("Alias") or props.get("Name") or props.get("Address") or "Unknown device"),
|
||||
"paired": bool(props.get("Paired", False)),
|
||||
"trusted": bool(props.get("Trusted", False)),
|
||||
"connected": bool(props.get("Connected", False)),
|
||||
"blocked": bool(props.get("Blocked", False)),
|
||||
"rssi": int(props["RSSI"]) if "RSSI" in props else None,
|
||||
"uuids": uuids,
|
||||
"audio": audio,
|
||||
"controller": controller,
|
||||
}
|
||||
if show_pairing_device(device["address"], device["name"], device["paired"], device["trusted"], device["connected"],
|
||||
device["blocked"], audio, controller):
|
||||
devices.append(device)
|
||||
return sorted(devices, key=lambda device: (not device["connected"], not device["paired"], -(device["rssi"] or -127), device["name"].lower()))
|
||||
|
||||
def status(self) -> dict[str, Any]:
|
||||
objects = self.managed_objects()
|
||||
_, adapter = self.adapter(objects)
|
||||
@@ -216,13 +280,6 @@ class BlueZClient:
|
||||
if props.get("Discovering", False):
|
||||
self._call(path, ADAPTER_IFACE, "StopDiscovery")
|
||||
|
||||
def device_for_address(self, address: str) -> dict[str, Any]:
|
||||
normalized = address.upper()
|
||||
for device in self.devices():
|
||||
if device["address"].upper() == normalized:
|
||||
return device
|
||||
raise RuntimeError(f"Bluetooth device {address} was not found")
|
||||
|
||||
def set_device_property(self, address: str, name: str, signature: str, value: Any) -> None:
|
||||
device = self.device_for_address(address)
|
||||
dbus_address = DBusAddress(device["path"], bus_name=BLUEZ, interface=DEVICE_IFACE)
|
||||
@@ -248,3 +305,194 @@ class BlueZClient:
|
||||
adapter_path, _ = self.adapter()
|
||||
device = self.device_for_address(address)
|
||||
self._call(adapter_path, ADAPTER_IFACE, "RemoveDevice", "o", (device["path"],))
|
||||
|
||||
|
||||
class BlueZProfileClient(_BlueZConnection):
|
||||
def __init__(self, profile_path: str = OBDYSSEY_PROFILE_PATH, profile_uuid: str = SPP_UUID):
|
||||
super().__init__(enable_fds=True)
|
||||
self.profile_path = profile_path
|
||||
self.profile_uuid = profile_uuid
|
||||
self._lock = threading.Lock()
|
||||
self._new_conn_event = threading.Event()
|
||||
self._closed = threading.Event()
|
||||
self._last_conn_path = ""
|
||||
self._last_conn_sock: socket.socket | None = None
|
||||
self._active_conn_path = ""
|
||||
self._active_conn_sock: socket.socket | None = None
|
||||
self._profile_filter = None
|
||||
self._profile_queue = None
|
||||
self._profile_registered = False
|
||||
self._close_lock = threading.Lock()
|
||||
self._profile_thread: threading.Thread | None = None
|
||||
|
||||
try:
|
||||
self._profile_filter = self.router.filter(MatchRule(type="method_call", interface=PROFILE_IFACE, path=self.profile_path), bufsize=20)
|
||||
self._profile_queue = self._profile_filter.__enter__()
|
||||
self._profile_thread = threading.Thread(target=self._profile_loop, daemon=True)
|
||||
self._profile_thread.start()
|
||||
self._register_profile()
|
||||
except Exception:
|
||||
self.close()
|
||||
raise
|
||||
|
||||
def close(self) -> None:
|
||||
with self._close_lock:
|
||||
if self._closed.is_set():
|
||||
return
|
||||
if self._profile_registered:
|
||||
self._unregister_profile()
|
||||
self._profile_registered = False
|
||||
self._closed.set()
|
||||
profile_queue = self._profile_queue
|
||||
if profile_queue is not None:
|
||||
try:
|
||||
profile_queue.put(None, timeout=1.0)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if self._profile_filter is not None:
|
||||
self._profile_filter.__exit__(None, None, None)
|
||||
self._profile_filter = None
|
||||
self._profile_queue = None
|
||||
finally:
|
||||
if self._profile_thread is not None and self._profile_thread is not threading.current_thread():
|
||||
self._profile_thread.join(timeout=1.0)
|
||||
self._close_connection_sockets()
|
||||
super().close()
|
||||
|
||||
def _register_profile(self) -> None:
|
||||
options = {"Role": ("s", "client"), "Name": ("s", "OBDyssey")}
|
||||
# Keep cleanup armed before the remote call. A timeout can happen after
|
||||
# BlueZ has registered the profile but before its reply reaches us.
|
||||
self._profile_registered = True
|
||||
self._call("/org/bluez", PROFILE_MANAGER_IFACE, "RegisterProfile", "osa{sv}", (self.profile_path, self.profile_uuid, options))
|
||||
|
||||
def _unregister_profile(self) -> None:
|
||||
try:
|
||||
self._call("/org/bluez", PROFILE_MANAGER_IFACE, "UnregisterProfile", "o", (self.profile_path,))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _profile_loop(self) -> None:
|
||||
profile_queue = self._profile_queue
|
||||
while profile_queue is not None:
|
||||
message = profile_queue.get()
|
||||
if message is None:
|
||||
break
|
||||
member = message.header.fields.get(HeaderFields.member, "")
|
||||
if self._closed.is_set():
|
||||
if member == "NewConnection" and len(message.body) > 1:
|
||||
try:
|
||||
_socket_from_dbus_fd(message.body[1]).close()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self.router.send(new_error(message, "org.bluez.Error.Canceled", "s", ("Profile is closed",)))
|
||||
except Exception:
|
||||
pass
|
||||
continue
|
||||
sock: socket.socket | None = None
|
||||
try:
|
||||
if member == "NewConnection":
|
||||
device_path = str(message.body[0]) if len(message.body) > 0 else ""
|
||||
fd = message.body[1] if len(message.body) > 1 else None
|
||||
if fd is None:
|
||||
raise RuntimeError("Profile1 NewConnection did not include a file descriptor")
|
||||
sock = _socket_from_dbus_fd(fd)
|
||||
self._store_connection_socket(device_path, sock)
|
||||
sock = None
|
||||
self.router.send(new_method_return(message))
|
||||
elif member == "RequestDisconnection":
|
||||
device_path = str(message.body[0]) if message.body else ""
|
||||
self._close_connection_sockets(device_path)
|
||||
self.router.send(new_method_return(message))
|
||||
elif member == "Release":
|
||||
self._close_connection_sockets()
|
||||
self.router.send(new_method_return(message))
|
||||
else:
|
||||
self.router.send(new_method_return(message))
|
||||
except Exception as error:
|
||||
if sock is not None:
|
||||
try:
|
||||
sock.close()
|
||||
except Exception:
|
||||
pass
|
||||
elif member == "NewConnection":
|
||||
self._close_connection_sockets(device_path)
|
||||
self.router.send(new_error(message, "org.bluez.Error.Failed", "s", (str(error),)))
|
||||
|
||||
def _store_connection_socket(self, device_path: str, sock: socket.socket) -> None:
|
||||
old_socks: list[socket.socket] = []
|
||||
with self._lock:
|
||||
if self._last_conn_sock is not None:
|
||||
old_socks.append(self._last_conn_sock)
|
||||
if self._active_conn_sock is not None:
|
||||
old_socks.append(self._active_conn_sock)
|
||||
self._last_conn_path = device_path
|
||||
self._last_conn_sock = sock
|
||||
self._active_conn_path = ""
|
||||
self._active_conn_sock = None
|
||||
self._new_conn_event.set()
|
||||
for old_sock in old_socks:
|
||||
try:
|
||||
old_sock.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def _close_connection_sockets(self, device_path: str | None = None) -> None:
|
||||
sockets: list[socket.socket] = []
|
||||
with self._lock:
|
||||
if self._last_conn_sock is not None and (device_path is None or self._last_conn_path == device_path):
|
||||
sockets.append(self._last_conn_sock)
|
||||
self._last_conn_sock = None
|
||||
self._last_conn_path = ""
|
||||
if self._active_conn_sock is not None and (device_path is None or self._active_conn_path == device_path):
|
||||
if self._active_conn_sock not in sockets:
|
||||
sockets.append(self._active_conn_sock)
|
||||
self._active_conn_sock = None
|
||||
self._active_conn_path = ""
|
||||
self._new_conn_event.set()
|
||||
for sock in sockets:
|
||||
try:
|
||||
sock.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def connect_profile(self, address: str, timeout: float = 30.0) -> socket.socket:
|
||||
device = self.device_for_address(address)
|
||||
device_path = device["path"]
|
||||
self._close_connection_sockets(device_path)
|
||||
with self._lock:
|
||||
self._new_conn_event.clear()
|
||||
|
||||
try:
|
||||
self._call(device_path, DEVICE_IFACE, "ConnectProfile", "s", (self.profile_uuid,), timeout=timeout)
|
||||
|
||||
deadline = time.monotonic() + timeout
|
||||
while time.monotonic() < deadline and not self._closed.is_set():
|
||||
remaining = deadline - time.monotonic()
|
||||
if self._new_conn_event.wait(timeout=min(0.2, max(0.01, remaining))):
|
||||
with self._lock:
|
||||
if self._last_conn_path == device_path and self._last_conn_sock is not None:
|
||||
sock = self._last_conn_sock
|
||||
self._last_conn_sock = None
|
||||
self._last_conn_path = ""
|
||||
self._active_conn_path = device_path
|
||||
self._active_conn_sock = sock
|
||||
return sock
|
||||
self._new_conn_event.clear()
|
||||
raise TimeoutError(f"Timed out waiting for SPP connection to {address}")
|
||||
except Exception as err:
|
||||
method = err.method if isinstance(err, BlueZError) else f"{DEVICE_IFACE}.ConnectProfile"
|
||||
error_name = err.error_name if isinstance(err, BlueZError) else type(err).__name__
|
||||
detail = err.detail if isinstance(err, BlueZError) else str(err)
|
||||
cloudlog.error(
|
||||
f"OBDyssey SPP connection failed method={method} address={address} device_path={device_path} "
|
||||
+ f"device_uuids={device.get('uuids', [])} error_name={error_name} detail={detail}"
|
||||
)
|
||||
self._close_connection_sockets()
|
||||
raise
|
||||
|
||||
def disconnect_profile(self, address: str, timeout: float = 15.0) -> None:
|
||||
device = self.device_for_address(address)
|
||||
self._call(device["path"], DEVICE_IFACE, "DisconnectProfile", "s", (self.profile_uuid,), timeout=timeout)
|
||||
|
||||
@@ -14,6 +14,7 @@ from openpilot.common.params import Params
|
||||
BLUETOOTH_SOCKET_PATH = "/tmp/starpilot-bluetooth.sock"
|
||||
BLUETOOTH_RADIO_HELPER = "/usr/comma/bluetooth-radio"
|
||||
A2DP_SINK_UUID = "0000110b-0000-1000-8000-00805f9b34fb"
|
||||
SPP_UUID = "00001101-0000-1000-8000-00805f9b34fb"
|
||||
HID_UUID = "00001124-0000-1000-8000-00805f9b34fb"
|
||||
HOG_UUID = "00001812-0000-1000-8000-00805f9b34fb"
|
||||
COMMAND_TIMEOUTS = {
|
||||
@@ -26,6 +27,7 @@ COMMAND_TIMEOUTS = {
|
||||
"test_audio": 10.0,
|
||||
}
|
||||
TRUE_VALUES = {"1", "true", "yes", "on"}
|
||||
OBD_DEVICE_PATTERNS = ("OBD", "OBDII", "ELM", "V-LINK", "VGATE", "OBDLINK", "VLINKER", "VIEOCAR")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -40,6 +42,7 @@ class BluetoothDevice:
|
||||
uuids: tuple[str, ...] = ()
|
||||
audio: bool = False
|
||||
controller: bool = False
|
||||
serial: bool = False
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, value: dict[str, Any]) -> "BluetoothDevice":
|
||||
@@ -54,6 +57,7 @@ class BluetoothDevice:
|
||||
uuids=tuple(str(uuid).lower() for uuid in value.get("uuids", ())),
|
||||
audio=bool(value.get("audio", False)),
|
||||
controller=bool(value.get("controller", False)),
|
||||
serial=bool(value.get("serial", False)),
|
||||
)
|
||||
|
||||
|
||||
@@ -86,19 +90,25 @@ class BluetoothStatus:
|
||||
)
|
||||
|
||||
|
||||
def device_capabilities(uuids: list[str] | tuple[str, ...], bluetooth_class: int = 0, icon: str = "") -> tuple[bool, bool]:
|
||||
def looks_like_obd_device(name: str) -> bool:
|
||||
upper = name.upper()
|
||||
return any(pattern in upper for pattern in OBD_DEVICE_PATTERNS)
|
||||
|
||||
|
||||
def device_capabilities(uuids: list[str] | tuple[str, ...], bluetooth_class: int = 0, icon: str = "") -> tuple[bool, bool, bool]:
|
||||
normalized = {str(uuid).lower() for uuid in uuids}
|
||||
major_class = (int(bluetooth_class) >> 8) & 0x1F
|
||||
audio = A2DP_SINK_UUID in normalized or major_class == 0x04 or icon in {"audio-card", "audio-headphones", "audio-headset"}
|
||||
controller = HID_UUID in normalized or HOG_UUID in normalized or major_class == 0x05 or icon in {"input-gaming", "input-mouse", "input-keyboard"}
|
||||
return audio, controller
|
||||
serial = SPP_UUID in normalized
|
||||
return audio, controller, serial
|
||||
|
||||
|
||||
def show_pairing_device(address: str, name: str, paired: bool, trusted: bool, connected: bool, blocked: bool,
|
||||
audio: bool, controller: bool) -> bool:
|
||||
audio: bool, controller: bool, serial: bool = False) -> bool:
|
||||
known = paired or trusted or connected
|
||||
named = bool(name) and name not in {address, "Unknown device"}
|
||||
return known or (named and not blocked and (audio or controller))
|
||||
return known or (named and not blocked and (audio or controller or serial or looks_like_obd_device(name)))
|
||||
|
||||
|
||||
class _DesktopFakeBluetooth:
|
||||
@@ -206,8 +216,8 @@ class BluetoothClient:
|
||||
os.getenv("NOBOARD", "0").lower() in TRUE_VALUES and
|
||||
not os.path.exists(self.socket_path)):
|
||||
return None
|
||||
from openpilot.system.hardware import PC
|
||||
if not PC:
|
||||
from openpilot.system import hardware
|
||||
if not hardware.PC:
|
||||
return None
|
||||
if self._desktop_fake is None:
|
||||
self._desktop_fake = _DesktopFakeBluetooth()
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
import io
|
||||
import os
|
||||
import socket
|
||||
import threading
|
||||
import time
|
||||
|
||||
@@ -6,12 +8,14 @@ import numpy as np
|
||||
import pytest
|
||||
|
||||
from openpilot.starpilot.system.bluetooth.audio import BluetoothAudioSink
|
||||
from openpilot.starpilot.system.bluetooth.bluez import PairingAgent
|
||||
from openpilot.starpilot.system.bluetooth.bluez import BlueZError, PairingAgent, _BlueZConnection, _socket_from_dbus_fd
|
||||
from openpilot.starpilot.system.bluetooth.daemon import BluetoothController
|
||||
from openpilot.starpilot.system.bluetooth.protocol import (A2DP_SINK_UUID, HID_UUID, BluetoothClient, BluetoothDevice, BluetoothStatus,
|
||||
device_capabilities, show_pairing_device)
|
||||
from openpilot.starpilot.system.bluetooth.protocol import (A2DP_SINK_UUID, HID_UUID, SPP_UUID, BluetoothClient, BluetoothDevice, BluetoothStatus,
|
||||
device_capabilities, looks_like_obd_device, show_pairing_device)
|
||||
from openpilot.system import hardware
|
||||
from openpilot.system.ui.lib.bluetooth_manager import BluetoothManager
|
||||
from jeepney.fds import FileDescriptor
|
||||
from jeepney.low_level import HeaderFields, MessageType
|
||||
|
||||
|
||||
class FakeParams:
|
||||
@@ -59,6 +63,7 @@ class FakeBlueZ:
|
||||
"connected": False,
|
||||
"audio": True,
|
||||
"controller": False,
|
||||
"serial": False,
|
||||
}
|
||||
|
||||
def close(self):
|
||||
@@ -155,21 +160,35 @@ class FakeProcess:
|
||||
|
||||
|
||||
def test_protocol_round_trip_and_capabilities():
|
||||
audio, controller = device_capabilities([A2DP_SINK_UUID, HID_UUID])
|
||||
assert audio and controller
|
||||
audio, controller, serial = device_capabilities([A2DP_SINK_UUID, HID_UUID, SPP_UUID])
|
||||
assert audio and controller and serial
|
||||
status = BluetoothStatus.from_dict({
|
||||
"available": True,
|
||||
"enabled": True,
|
||||
"devices": [{"address": "00:11:22:33:44:55", "name": "Combo", "uuids": [A2DP_SINK_UUID, HID_UUID], "audio": True, "controller": True}],
|
||||
"devices": [
|
||||
{"address": "00:11:22:33:44:55", "name": "Combo", "uuids": [A2DP_SINK_UUID, HID_UUID], "audio": True, "controller": True},
|
||||
{"address": "AA:BB:CC:DD:EE:FF", "name": "OBDLink MX+", "uuids": [SPP_UUID], "serial": True},
|
||||
],
|
||||
})
|
||||
assert status.devices == (BluetoothDevice("00:11:22:33:44:55", "Combo", uuids=(A2DP_SINK_UUID, HID_UUID), audio=True, controller=True),)
|
||||
assert status.devices == (
|
||||
BluetoothDevice("00:11:22:33:44:55", "Combo", uuids=(A2DP_SINK_UUID, HID_UUID), audio=True, controller=True, serial=False),
|
||||
BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDLink MX+", uuids=(SPP_UUID,), audio=False, controller=False, serial=True),
|
||||
)
|
||||
|
||||
|
||||
def test_pairing_list_filters_anonymous_and_irrelevant_advertisements():
|
||||
assert not show_pairing_device("00:11:22:33:44:55", "00:11:22:33:44:55", False, False, False, False, False, False)
|
||||
assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "Known device", True, True, False, False, False, False)
|
||||
assert not show_pairing_device("00:11:22:33:44:55", "00:11:22:33:44:55", False, False, False, False, False, False, False)
|
||||
assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False, False)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True, False)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "Known device", True, True, False, False, False, False, False)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "OBDII Adapter", False, False, False, False, False, False, False)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "vLinker MC", False, False, False, False, False, False, False)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "Vgate iCar Pro", False, False, False, False, False, False, False)
|
||||
assert show_pairing_device("00:11:22:33:44:55", "Serial Dongle", False, False, False, False, False, False, True)
|
||||
assert looks_like_obd_device("OBDII")
|
||||
assert looks_like_obd_device("VGATE iCar")
|
||||
assert looks_like_obd_device("vLinker MC+")
|
||||
assert not looks_like_obd_device("Sony WH-1000XM4")
|
||||
|
||||
|
||||
def test_desktop_fake_bluetooth_is_stateful_and_interactive(monkeypatch, tmp_path):
|
||||
@@ -233,6 +252,44 @@ def test_pairing_agent_accept_reject_and_timeout():
|
||||
assert agent.request("pin", "/device", timeout=0.01) == (False, "")
|
||||
|
||||
|
||||
def test_profile_file_descriptor_is_converted_to_owned_socket():
|
||||
peer, source = socket.socketpair()
|
||||
wrapped = FileDescriptor(os.dup(source.fileno()))
|
||||
converted = _socket_from_dbus_fd(wrapped)
|
||||
|
||||
peer.sendall(b"hello")
|
||||
assert converted.recv(5) == b"hello"
|
||||
|
||||
converted.close()
|
||||
peer.close()
|
||||
source.close()
|
||||
|
||||
|
||||
def test_bluez_dbus_error_preserves_name_and_detail():
|
||||
class FakeRouter:
|
||||
def send_and_get_reply(self, _message, timeout):
|
||||
assert timeout == 3.0
|
||||
return type("Reply", (), {
|
||||
"header": type("Header", (), {
|
||||
"message_type": MessageType.error,
|
||||
"fields": {HeaderFields.error_name: "org.bluez.Error.NotSupported"},
|
||||
})(),
|
||||
"body": ("br-connection-profile-unavailable",),
|
||||
})()
|
||||
|
||||
connection = object.__new__(_BlueZConnection)
|
||||
connection.router = FakeRouter()
|
||||
|
||||
with pytest.raises(BlueZError) as exc_info:
|
||||
connection._call("/org/bluez/hci0/dev_test", "org.bluez.Device1", "ConnectProfile", "s", ("uuid",), timeout=3.0)
|
||||
|
||||
assert exc_info.value.error_name == "org.bluez.Error.NotSupported"
|
||||
assert exc_info.value.detail == "br-connection-profile-unavailable"
|
||||
assert exc_info.value.method == "org.bluez.Device1.ConnectProfile"
|
||||
assert exc_info.value.path == "/org/bluez/hci0/dev_test"
|
||||
assert str(exc_info.value) == "org.bluez.Error.NotSupported: br-connection-profile-unavailable"
|
||||
|
||||
|
||||
def test_disabled_status_does_not_start_radio_or_bluez():
|
||||
params = FakeParams(IsOffroad=True, BluetoothEnabled=False)
|
||||
radio = FakeRadio()
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
from openpilot.starpilot.system.obdyssey.elm327 import (
|
||||
AdapterInfo,
|
||||
DiagnosticResponse,
|
||||
Elm327,
|
||||
ElmBusError,
|
||||
ElmCommandError,
|
||||
ElmContext,
|
||||
ElmDisconnectedError,
|
||||
ElmError,
|
||||
ElmNoDataError,
|
||||
ElmTimeoutError,
|
||||
ElmUnsupportedError,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.diagnostics import DiagnosticTroubleCode
|
||||
from openpilot.starpilot.system.obdyssey.obdb import (
|
||||
DiagnosticCommand,
|
||||
SignalDefinition,
|
||||
SignalFormat,
|
||||
VehicleProfile,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.protocol import OBDYSSEY_SOCKET_PATH, OBDysseyClient, OBDysseyStatus
|
||||
from openpilot.starpilot.system.obdyssey.transport import BluezSppTransport, ElmTransport, FakeElmTransport
|
||||
|
||||
__all__ = [
|
||||
"AdapterInfo",
|
||||
"BluezSppTransport",
|
||||
"DiagnosticCommand",
|
||||
"DiagnosticResponse",
|
||||
"DiagnosticTroubleCode",
|
||||
"Elm327",
|
||||
"ElmBusError",
|
||||
"ElmCommandError",
|
||||
"ElmContext",
|
||||
"ElmDisconnectedError",
|
||||
"ElmError",
|
||||
"ElmNoDataError",
|
||||
"ElmTimeoutError",
|
||||
"ElmTransport",
|
||||
"ElmUnsupportedError",
|
||||
"FakeElmTransport",
|
||||
"OBDYSSEY_SOCKET_PATH",
|
||||
"OBDysseyClient",
|
||||
"OBDysseyStatus",
|
||||
"SignalDefinition",
|
||||
"SignalFormat",
|
||||
"VehicleProfile",
|
||||
]
|
||||
@@ -0,0 +1,126 @@
|
||||
{
|
||||
"metadata": {
|
||||
"id": "Chevrolet-Bolt-EV",
|
||||
"name": "Chevrolet Bolt EV / EUV",
|
||||
"provider": "OBDb",
|
||||
"revision": "v3.1.0",
|
||||
"protocol": "ISO 15765-4 (CAN 11/500)",
|
||||
"description": "OEM Mode 22 High-Voltage Battery & Inverter definitions for Chevrolet Bolt EV"
|
||||
},
|
||||
"commands": [
|
||||
{
|
||||
"id": "CMD_BOLT_BMS_MAIN",
|
||||
"hdr": "7E4",
|
||||
"rax": "7EC",
|
||||
"service": 34,
|
||||
"pid": "8334",
|
||||
"freq": 2.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "BOLT_HVBAT_SOC",
|
||||
"name": "HV Battery State of Charge",
|
||||
"path": "Battery",
|
||||
"suggested_metric": "stateOfCharge",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 0.5,
|
||||
"div": 1.0,
|
||||
"unit": "%"
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": "BOLT_HVBAT_VOLTAGE",
|
||||
"name": "HV Battery Pack Voltage",
|
||||
"path": "Battery",
|
||||
"suggested_metric": "batteryVoltage",
|
||||
"fmt": {
|
||||
"bix": 16,
|
||||
"len": 16,
|
||||
"mul": 0.05,
|
||||
"div": 1.0,
|
||||
"unit": "V"
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": "BOLT_HVBAT_CURRENT",
|
||||
"name": "HV Battery Pack Current",
|
||||
"path": "Battery",
|
||||
"suggested_metric": "batteryCurrent",
|
||||
"fmt": {
|
||||
"bix": 32,
|
||||
"len": 16,
|
||||
"sign": true,
|
||||
"mul": 0.05,
|
||||
"div": 1.0,
|
||||
"unit": "A"
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": "BOLT_HVBAT_TEMP",
|
||||
"name": "HV Battery Average Temperature",
|
||||
"path": "Battery",
|
||||
"fmt": {
|
||||
"bix": 48,
|
||||
"len": 8,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"add": -40.0,
|
||||
"unit": "°C"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_BOLT_MOTOR",
|
||||
"hdr": "7E2",
|
||||
"rax": "7EA",
|
||||
"service": 34,
|
||||
"pid": "4080",
|
||||
"freq": 5.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "BOLT_MOTOR_RPM",
|
||||
"name": "Traction Motor RPM",
|
||||
"path": "Motor",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 16,
|
||||
"sign": true,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"unit": "rpm"
|
||||
}
|
||||
},
|
||||
{
|
||||
"id": "BOLT_MOTOR_TEMP",
|
||||
"name": "Motor Temperature",
|
||||
"path": "Motor",
|
||||
"fmt": {
|
||||
"bix": 16,
|
||||
"len": 8,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"add": -40.0,
|
||||
"unit": "°C"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
"signals": [
|
||||
{
|
||||
"id": "BOLT_HVBAT_POWER",
|
||||
"name": "HV Battery Power",
|
||||
"path": "Battery",
|
||||
"synthetic": {
|
||||
"operation": "multiply",
|
||||
"signals": ["BOLT_HVBAT_VOLTAGE", "BOLT_HVBAT_CURRENT"]
|
||||
},
|
||||
"fmt": {
|
||||
"mul": 0.001,
|
||||
"unit": "kW"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,518 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import socketserver
|
||||
import threading
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.starpilot.system.bluetooth.protocol import BluetoothClient, looks_like_obd_device
|
||||
from openpilot.starpilot.system.obdyssey.diagnostics import (
|
||||
clear_dtcs,
|
||||
is_mutating_service,
|
||||
is_read_only_payload,
|
||||
read_stored_dtcs,
|
||||
read_vin,
|
||||
uds_clear_diagnostic_information,
|
||||
uds_diagnostic_session_control,
|
||||
uds_read_dtc_information,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.elm327 import (
|
||||
Elm327,
|
||||
ElmContext,
|
||||
ElmDisconnectedError,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.obdb import (
|
||||
calculate_synthetic_signal,
|
||||
decode_signal,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.profiles import ProfileManager
|
||||
from openpilot.starpilot.system.obdyssey.protocol import (
|
||||
API_VERSION,
|
||||
OBDYSSEY_SOCKET_PATH,
|
||||
OBDysseyStatus,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.transport import BluezSppTransport, ElmTransport
|
||||
|
||||
BACKOFF_DELAYS = [2.0, 5.0, 15.0, 30.0, 60.0]
|
||||
|
||||
|
||||
class OBDysseyController:
|
||||
def __init__(self, params: Params | None = None, transport_factory=None, profile_manager: ProfileManager | None = None,
|
||||
bluetooth_client: BluetoothClient | None = None, sleep=time.sleep):
|
||||
self.params = params or Params()
|
||||
self._transport_factory = transport_factory
|
||||
self._profile_manager = profile_manager or ProfileManager(params=self.params)
|
||||
self._bluetooth_client = bluetooth_client or BluetoothClient()
|
||||
self._sleep = sleep
|
||||
|
||||
self._lock = threading.RLock()
|
||||
self._state = "idle"
|
||||
self._user_disconnected = False
|
||||
self._last_validated_address = ""
|
||||
self._active_address = ""
|
||||
self._active_name = ""
|
||||
self._transport: ElmTransport | None = None
|
||||
self._elm: Elm327 | None = None
|
||||
self._last_error = ""
|
||||
self._last_request_ms = 0
|
||||
self._requests_count = 0
|
||||
self._errors_count = 0
|
||||
self._reconnects_count = 0
|
||||
self._backoff_idx = 0
|
||||
self._status_snapshot: dict[str, Any] = OBDysseyStatus(
|
||||
api_version=API_VERSION,
|
||||
state="idle",
|
||||
enabled=True,
|
||||
).to_dict()
|
||||
|
||||
def _offroad(self) -> bool:
|
||||
return bool(self.params.get_bool("IsOffroad"))
|
||||
|
||||
def _require_offroad(self, action_name: str) -> None:
|
||||
if not self._offroad():
|
||||
raise RuntimeError(f"Operation '{action_name}' is mutating and only allowed when offroad")
|
||||
|
||||
def _require_profile_command_safety(self, command_id: str, service: int, session_in: int | None, session_out: int | None) -> None:
|
||||
"""Keep profile-defined session changes and mutating services offroad."""
|
||||
if session_in is not None or session_out is not None or is_mutating_service(service):
|
||||
self._require_offroad(f"profile command '{command_id}'")
|
||||
|
||||
def _build_status_unlocked(self) -> dict[str, Any]:
|
||||
bt_enabled = self.params.get_bool("BluetoothEnabled")
|
||||
adapter_voltage = self._elm.adapter_info.voltage if (self._elm and self._elm.adapter_info) else None
|
||||
elm_identity = self._elm.adapter_info.identity if (self._elm and self._elm.adapter_info) else ""
|
||||
active_protocol = self._elm.adapter_info.active_protocol if (self._elm and self._elm.adapter_info) else ""
|
||||
active_prof = self._profile_manager.resolve_active_profile()
|
||||
diagnostic_ready = self._state == "ready"
|
||||
|
||||
return OBDysseyStatus(
|
||||
api_version=API_VERSION,
|
||||
state=self._state,
|
||||
enabled=True,
|
||||
bluetooth_enabled=bt_enabled,
|
||||
adapter_address=self._active_address,
|
||||
adapter_name=self._active_name,
|
||||
connected=diagnostic_ready,
|
||||
link_connected=self._transport is not None,
|
||||
diagnostic_ready=diagnostic_ready,
|
||||
elm_identity=elm_identity,
|
||||
adapter_voltage=adapter_voltage,
|
||||
protocol=active_protocol,
|
||||
profile=active_prof.id,
|
||||
signal_count=len(active_prof.signals),
|
||||
last_error=self._last_error,
|
||||
last_request_ms=self._last_request_ms,
|
||||
requests=self._requests_count,
|
||||
errors=self._errors_count,
|
||||
reconnects=self._reconnects_count,
|
||||
).to_dict()
|
||||
|
||||
def _publish_status_snapshot_unlocked(self) -> None:
|
||||
self._status_snapshot = self._build_status_unlocked()
|
||||
|
||||
def status(self) -> dict[str, Any]:
|
||||
# A connection attempt can wait on BlueZ for tens of seconds while holding
|
||||
# the controller lock. Return the last published state instead of making
|
||||
# status callers wait behind that I/O.
|
||||
if not self._lock.acquire(blocking=False):
|
||||
return dict(self._status_snapshot)
|
||||
try:
|
||||
self._publish_status_snapshot_unlocked()
|
||||
return dict(self._status_snapshot)
|
||||
finally:
|
||||
self._lock.release()
|
||||
|
||||
def find_candidate_devices(self) -> list[dict[str, Any]]:
|
||||
"""Inspect paired devices for ELM327 / OBD candidates."""
|
||||
try:
|
||||
bt_status = self._bluetooth_client.status()
|
||||
if not bt_status.enabled:
|
||||
return []
|
||||
candidates = []
|
||||
for dev in bt_status.devices:
|
||||
if dev.paired and dev.trusted:
|
||||
if dev.serial or looks_like_obd_device(dev.name):
|
||||
candidates.append({
|
||||
"address": dev.address,
|
||||
"name": dev.name,
|
||||
"last_known": (dev.address.upper() == self._last_validated_address.upper()),
|
||||
})
|
||||
# Sort last known validated adapter first
|
||||
candidates.sort(key=lambda d: not d["last_known"])
|
||||
return candidates
|
||||
except Exception as err:
|
||||
cloudlog.warning(f"OBDyssey error querying bluetooth status: {err}")
|
||||
return []
|
||||
|
||||
def connect_adapter(self, target_address: str = "", target_name: str = "") -> bool:
|
||||
with self._lock:
|
||||
self._state = "connecting"
|
||||
self._publish_status_snapshot_unlocked()
|
||||
self._disconnect_transport()
|
||||
|
||||
if not target_address:
|
||||
candidates = self.find_candidate_devices()
|
||||
if not candidates:
|
||||
self._state = "idle"
|
||||
self._publish_status_snapshot_unlocked()
|
||||
return False
|
||||
target_address = candidates[0]["address"]
|
||||
target_name = candidates[0]["name"]
|
||||
|
||||
self._active_address = target_address
|
||||
self._active_name = target_name
|
||||
self._publish_status_snapshot_unlocked()
|
||||
|
||||
transport: ElmTransport | None = None
|
||||
try:
|
||||
if self._transport_factory is not None:
|
||||
transport = self._transport_factory(target_address)
|
||||
else:
|
||||
transport = BluezSppTransport(target_address)
|
||||
|
||||
transport.connect()
|
||||
self._transport = transport
|
||||
self._state = "initializing"
|
||||
self._publish_status_snapshot_unlocked()
|
||||
|
||||
elm = Elm327(transport)
|
||||
info = elm.initialize()
|
||||
self._elm = elm
|
||||
self._last_validated_address = target_address
|
||||
self._state = "ready"
|
||||
self._last_error = ""
|
||||
self._backoff_idx = 0
|
||||
self._publish_status_snapshot_unlocked()
|
||||
cloudlog.info(f"OBDyssey connected to {target_address} ({target_name}): {info.identity}")
|
||||
return True
|
||||
except Exception as err:
|
||||
self._state = "error"
|
||||
self._last_error = str(err)
|
||||
attached_transport = transport is not None and transport is self._transport
|
||||
self._disconnect_transport()
|
||||
if transport is not None and not attached_transport:
|
||||
try:
|
||||
transport.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._publish_status_snapshot_unlocked()
|
||||
cloudlog.error(f"OBDyssey failed connecting to {target_address}: {err}")
|
||||
return False
|
||||
|
||||
def _disconnect_transport(self) -> None:
|
||||
if self._transport is not None:
|
||||
try:
|
||||
self._transport.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._transport = None
|
||||
self._elm = None
|
||||
|
||||
def disconnect(self) -> None:
|
||||
with self._lock:
|
||||
self._user_disconnected = True
|
||||
self._disconnect_transport()
|
||||
self._state = "idle"
|
||||
self._active_address = ""
|
||||
self._active_name = ""
|
||||
self._publish_status_snapshot_unlocked()
|
||||
|
||||
def reconnect_step(self) -> None:
|
||||
if self._user_disconnected or not self.params.get_bool("BluetoothEnabled"):
|
||||
return
|
||||
|
||||
with self._lock:
|
||||
if self._state == "ready":
|
||||
return
|
||||
|
||||
candidates = self.find_candidate_devices()
|
||||
if not candidates:
|
||||
with self._lock:
|
||||
self._state = "idle"
|
||||
self._publish_status_snapshot_unlocked()
|
||||
return
|
||||
|
||||
with self._lock:
|
||||
self._reconnects_count += 1
|
||||
self._state = "reconnecting"
|
||||
self._publish_status_snapshot_unlocked()
|
||||
success = self.connect_adapter()
|
||||
|
||||
if not success:
|
||||
delay = BACKOFF_DELAYS[min(self._backoff_idx, len(BACKOFF_DELAYS) - 1)]
|
||||
self._backoff_idx += 1
|
||||
self._sleep(delay)
|
||||
|
||||
def maintain_loop(self) -> None:
|
||||
while True:
|
||||
time.sleep(2.0)
|
||||
if self.params.get_bool("BluetoothEnabled") and not self._user_disconnected:
|
||||
if self._state in ("idle", "reconnecting", "error"):
|
||||
self.reconnect_step()
|
||||
|
||||
def _execute_diagnostic(self, func, *args, **kwargs) -> Any:
|
||||
"""Serialize ELM transactions and record timing & error metrics."""
|
||||
with self._lock:
|
||||
if self._elm is None or self._state != "ready":
|
||||
# Try fast auto-connect
|
||||
if not self.connect_adapter():
|
||||
raise RuntimeError(self._last_error or "OBD adapter is not connected")
|
||||
|
||||
start_t = time.monotonic()
|
||||
self._requests_count += 1
|
||||
try:
|
||||
result = func(self._elm, *args, **kwargs)
|
||||
self._last_request_ms = int((time.monotonic() - start_t) * 1000)
|
||||
return result
|
||||
except ElmDisconnectedError:
|
||||
self._state = "reconnecting"
|
||||
self._errors_count += 1
|
||||
self._disconnect_transport()
|
||||
self._publish_status_snapshot_unlocked()
|
||||
raise
|
||||
except Exception as err:
|
||||
self._errors_count += 1
|
||||
self._last_error = str(err)
|
||||
self._publish_status_snapshot_unlocked()
|
||||
raise
|
||||
|
||||
def handle(self, request: dict[str, Any]) -> dict[str, Any]:
|
||||
cmd = str(request.get("command", ""))
|
||||
if cmd == "status":
|
||||
return {"ok": True, "status": self.status()}
|
||||
elif cmd == "connect":
|
||||
addr = str(request.get("address", ""))
|
||||
self._user_disconnected = False
|
||||
success = self.connect_adapter(addr)
|
||||
result: dict[str, Any] = {"ok": success, "status": self.status()}
|
||||
if not success:
|
||||
result["error"] = self._last_error or "OBD adapter is not connected"
|
||||
return result
|
||||
elif cmd == "disconnect":
|
||||
self.disconnect()
|
||||
return {"ok": True}
|
||||
elif cmd == "test_adapter":
|
||||
return {"ok": True, "result": self._execute_diagnostic(lambda elm: elm.command("ATI"))}
|
||||
elif cmd == "adapter_info":
|
||||
with self._lock:
|
||||
if self._elm and self._elm.adapter_info:
|
||||
info = self._elm.adapter_info
|
||||
return {"ok": True, "info": {
|
||||
"identity": info.identity,
|
||||
"version": info.reported_version,
|
||||
"voltage": info.voltage,
|
||||
"protocol": info.active_protocol,
|
||||
}}
|
||||
return {"ok": False, "error": "Adapter not connected"}
|
||||
elif cmd == "vehicle_info":
|
||||
vin = self._execute_diagnostic(read_vin)
|
||||
return {"ok": True, "vin": vin}
|
||||
elif cmd == "list_signals":
|
||||
profile = self._profile_manager.resolve_active_profile()
|
||||
sig_list = [
|
||||
{
|
||||
"id": s.id,
|
||||
"name": s.name,
|
||||
"path": s.path,
|
||||
"suggested_metric": s.suggested_metric,
|
||||
"unit": s.format.unit,
|
||||
}
|
||||
for s in profile.signals.values()
|
||||
]
|
||||
return {"ok": True, "signals": sig_list}
|
||||
elif cmd == "read_signal":
|
||||
sig_id = str(request.get("id", ""))
|
||||
profile = self._profile_manager.resolve_active_profile()
|
||||
sig_def = profile.signals.get(sig_id)
|
||||
if not sig_def:
|
||||
raise KeyError(f"Signal {sig_id} not found in profile {profile.id}")
|
||||
|
||||
groups = ProfileManager.group_signals_by_command(profile, [sig_id])
|
||||
if not groups:
|
||||
raise RuntimeError(f"No command mapped for signal {sig_id}")
|
||||
|
||||
diag_cmd, sigs = groups[0]
|
||||
self._require_profile_command_safety(
|
||||
diag_cmd.id,
|
||||
diag_cmd.service,
|
||||
diag_cmd.diagnostic_session_in,
|
||||
diag_cmd.diagnostic_session_out,
|
||||
)
|
||||
|
||||
def _query(elm: Elm327):
|
||||
if diag_cmd.diagnostic_session_in:
|
||||
uds_diagnostic_session_control(elm, diag_cmd.diagnostic_session_in, diag_cmd.context)
|
||||
try:
|
||||
full_payload = bytes([diag_cmd.service]) + diag_cmd.parameter
|
||||
res = elm.request(full_payload, diag_cmd.context, retry=True)
|
||||
val = decode_signal(res.payload, sig_def, strip_prefix=diag_cmd.expected_prefix)
|
||||
return val
|
||||
finally:
|
||||
if diag_cmd.diagnostic_session_out:
|
||||
try:
|
||||
uds_diagnostic_session_control(elm, diag_cmd.diagnostic_session_out, diag_cmd.context)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
val = self._execute_diagnostic(_query)
|
||||
return {"ok": True, "signal": {"id": sig_id, "value": val, "unit": sig_def.format.unit}}
|
||||
elif cmd == "read_signals":
|
||||
sig_ids = [str(i) for i in request.get("ids", [])]
|
||||
profile = self._profile_manager.resolve_active_profile()
|
||||
groups = ProfileManager.group_signals_by_command(profile, sig_ids)
|
||||
results: dict[str, Any] = {}
|
||||
|
||||
for diag_cmd, _sigs in groups:
|
||||
self._require_profile_command_safety(
|
||||
diag_cmd.id,
|
||||
diag_cmd.service,
|
||||
diag_cmd.diagnostic_session_in,
|
||||
diag_cmd.diagnostic_session_out,
|
||||
)
|
||||
|
||||
def _query_batch(elm: Elm327):
|
||||
for diag_cmd, sigs in groups:
|
||||
if diag_cmd.diagnostic_session_in:
|
||||
uds_diagnostic_session_control(elm, diag_cmd.diagnostic_session_in, diag_cmd.context)
|
||||
try:
|
||||
full_payload = bytes([diag_cmd.service]) + diag_cmd.parameter
|
||||
res = elm.request(full_payload, diag_cmd.context, retry=True)
|
||||
for s in sigs:
|
||||
results[s.id] = decode_signal(res.payload, s, strip_prefix=diag_cmd.expected_prefix)
|
||||
finally:
|
||||
if diag_cmd.diagnostic_session_out:
|
||||
try:
|
||||
uds_diagnostic_session_control(elm, diag_cmd.diagnostic_session_out, diag_cmd.context)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# Compute any requested synthetic signals
|
||||
for sid in sig_ids:
|
||||
if sid in profile.synthetic_signals:
|
||||
synth_def = profile.synthetic_signals[sid]
|
||||
results[sid] = calculate_synthetic_signal(synth_def, results)
|
||||
|
||||
self._execute_diagnostic(_query_batch)
|
||||
return {"ok": True, "signals": results}
|
||||
elif cmd == "read_dtcs":
|
||||
dtcs = self._execute_diagnostic(read_stored_dtcs)
|
||||
return {"ok": True, "dtcs": [d.to_dict() for d in dtcs]}
|
||||
elif cmd == "read_uds_dtcs":
|
||||
tx_addr = int(str(request.get("tx_addr", "7E0")), 16)
|
||||
rx_addr = int(str(request.get("rx_addr", "7E8")), 16)
|
||||
ctx = ElmContext(tx_header=tx_addr, rx_filter=rx_addr)
|
||||
dtcs = self._execute_diagnostic(lambda elm: uds_read_dtc_information(elm, context=ctx, ecu=f"{tx_addr:03X}"))
|
||||
return {"ok": True, "dtcs": [d.to_dict() for d in dtcs]}
|
||||
elif cmd == "clear_dtcs":
|
||||
self._require_offroad("clear_dtcs")
|
||||
self._execute_diagnostic(clear_dtcs)
|
||||
return {"ok": True}
|
||||
elif cmd == "clear_uds_dtcs":
|
||||
self._require_offroad("clear_uds_dtcs")
|
||||
tx_addr = int(str(request.get("tx_addr", "7E0")), 16)
|
||||
rx_addr = int(str(request.get("rx_addr", "7E8")), 16)
|
||||
ctx = ElmContext(tx_header=tx_addr, rx_filter=rx_addr)
|
||||
self._execute_diagnostic(lambda elm: uds_clear_diagnostic_information(elm, context=ctx))
|
||||
return {"ok": True}
|
||||
elif cmd == "uds_request":
|
||||
tx_addr = int(str(request.get("tx_addr", "7E0")), 16)
|
||||
rx_addr = int(str(request.get("rx_addr", "7E8")), 16)
|
||||
payload_str = str(request.get("payload", "")).replace(" ", "")
|
||||
payload_bytes = bytes.fromhex(payload_str)
|
||||
proto = request.get("protocol")
|
||||
|
||||
if not is_read_only_payload(payload_bytes):
|
||||
self._require_offroad("uds_request (mutating)")
|
||||
|
||||
ctx = ElmContext(protocol=str(proto) if proto else None, tx_header=tx_addr, rx_filter=rx_addr)
|
||||
res = self._execute_diagnostic(lambda elm: elm.request(payload_bytes, ctx, retry=is_read_only_payload(payload_bytes)))
|
||||
return {"ok": True, "response": res.payload.hex().upper(), "lines": list(res.lines)}
|
||||
elif cmd == "raw_request":
|
||||
tx_addr = int(str(request.get("tx_addr", "7E0")), 16)
|
||||
rx_addr = int(str(request.get("rx_addr", "7E8")), 16)
|
||||
payload_str = str(request.get("payload", "")).replace(" ", "")
|
||||
payload_bytes = bytes.fromhex(payload_str)
|
||||
proto = request.get("protocol")
|
||||
|
||||
if not is_read_only_payload(payload_bytes):
|
||||
self._require_offroad("raw_request (mutating)")
|
||||
|
||||
ctx = ElmContext(protocol=str(proto) if proto else None, tx_header=tx_addr, rx_filter=rx_addr)
|
||||
res = self._execute_diagnostic(lambda elm: elm.request(payload_bytes, ctx, retry=is_read_only_payload(payload_bytes)))
|
||||
return {"ok": True, "response": res.payload.hex().upper(), "lines": list(res.lines)}
|
||||
elif cmd == "debug_at_command":
|
||||
self._require_offroad("debug_at_command")
|
||||
at_cmd = str(request.get("command_str") or request.get("cmd") or request.get("at_command", ""))
|
||||
lines = self._execute_diagnostic(lambda elm: elm.command(at_cmd))
|
||||
return {"ok": True, "lines": lines}
|
||||
elif cmd == "profile_status":
|
||||
prof = self._profile_manager.resolve_active_profile()
|
||||
return {"ok": True, "profile": prof.id, "profiles": self._profile_manager.list_profiles()}
|
||||
elif cmd == "select_profile":
|
||||
prof_id = str(request.get("profile_id", ""))
|
||||
prof = self._profile_manager.resolve_active_profile(explicit_id=prof_id)
|
||||
return {"ok": True, "active_profile": prof.id}
|
||||
elif cmd == "remove_profile":
|
||||
self._require_offroad("remove_profile")
|
||||
prof_id = str(request.get("profile_id", ""))
|
||||
res = self._profile_manager.remove_profile(prof_id)
|
||||
return {"ok": res}
|
||||
else:
|
||||
raise RuntimeError(f"Unknown OBDyssey command: {cmd}")
|
||||
|
||||
|
||||
class OBDysseyServer(socketserver.ThreadingUnixStreamServer):
|
||||
daemon_threads = True
|
||||
|
||||
def __init__(self, socket_path: str, controller: OBDysseyController):
|
||||
if os.path.exists(socket_path):
|
||||
try:
|
||||
os.unlink(socket_path)
|
||||
except Exception:
|
||||
pass
|
||||
self.controller = controller
|
||||
super().__init__(socket_path, OBDysseyHandler)
|
||||
|
||||
|
||||
class OBDysseyHandler(socketserver.StreamRequestHandler):
|
||||
def handle(self) -> None:
|
||||
for line in self.rfile:
|
||||
if not line:
|
||||
break
|
||||
try:
|
||||
request_obj = json.loads(line.decode("utf-8"))
|
||||
res = self.server.controller.handle(request_obj)
|
||||
except Exception as err:
|
||||
err_type = type(err).__name__
|
||||
res = {"ok": False, "error": str(err), "error_type": err_type}
|
||||
|
||||
resp_bytes = json.dumps(res, separators=(",", ":")).encode("utf-8") + b"\n"
|
||||
self.wfile.write(resp_bytes)
|
||||
self.wfile.flush()
|
||||
|
||||
|
||||
def main():
|
||||
cloudlog.info("Starting OBDyssey daemon (obdysseyd)...")
|
||||
controller = OBDysseyController()
|
||||
threading.Thread(target=controller.maintain_loop, daemon=True).start()
|
||||
|
||||
server = OBDysseyServer(OBDYSSEY_SOCKET_PATH, controller)
|
||||
try:
|
||||
server.serve_forever()
|
||||
except (KeyboardInterrupt, SystemExit):
|
||||
pass
|
||||
finally:
|
||||
server.server_close()
|
||||
if os.path.exists(OBDYSSEY_SOCKET_PATH):
|
||||
try:
|
||||
os.unlink(OBDYSSEY_SOCKET_PATH)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,384 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from enum import IntEnum
|
||||
from typing import TYPE_CHECKING, Any
|
||||
|
||||
if TYPE_CHECKING:
|
||||
from openpilot.starpilot.system.obdyssey.elm327 import Elm327, ElmContext, DiagnosticResponse
|
||||
|
||||
|
||||
class SERVICE_TYPE(IntEnum):
|
||||
DIAGNOSTIC_SESSION_CONTROL = 0x10
|
||||
ECU_RESET = 0x11
|
||||
CLEAR_DIAGNOSTIC_INFORMATION = 0x14
|
||||
READ_DTC_INFORMATION = 0x19
|
||||
READ_DATA_BY_IDENTIFIER = 0x22
|
||||
READ_MEMORY_BY_ADDRESS = 0x23
|
||||
READ_SCALING_DATA_BY_IDENTIFIER = 0x24
|
||||
SECURITY_ACCESS = 0x27
|
||||
COMMUNICATION_CONTROL = 0x28
|
||||
WRITE_DATA_BY_IDENTIFIER = 0x2E
|
||||
INPUT_OUTPUT_CONTROL_BY_IDENTIFIER = 0x2F
|
||||
ROUTINE_CONTROL = 0x31
|
||||
REQUEST_DOWNLOAD = 0x34
|
||||
REQUEST_UPLOAD = 0x35
|
||||
TRANSFER_DATA = 0x36
|
||||
REQUEST_TRANSFER_EXIT = 0x37
|
||||
WRITE_MEMORY_BY_ADDRESS = 0x3D
|
||||
TESTER_PRESENT = 0x3E
|
||||
|
||||
|
||||
class SESSION_TYPE(IntEnum):
|
||||
DEFAULT = 1
|
||||
PROGRAMMING = 2
|
||||
EXTENDED_DIAGNOSTIC = 3
|
||||
SAFETY_SYSTEM_DIAGNOSTIC = 4
|
||||
|
||||
|
||||
class RESET_TYPE(IntEnum):
|
||||
HARD = 1
|
||||
KEY_OFF_ON = 2
|
||||
SOFT = 3
|
||||
ENABLE_RAPID_POWER_SHUTDOWN = 4
|
||||
DISABLE_RAPID_POWER_SHUTDOWN = 5
|
||||
|
||||
|
||||
class ACCESS_TYPE(IntEnum):
|
||||
REQUEST_SEED = 1
|
||||
SEND_KEY = 2
|
||||
|
||||
|
||||
class ROUTINE_CONTROL_TYPE(IntEnum):
|
||||
START = 1
|
||||
STOP = 2
|
||||
REQUEST_RESULTS = 3
|
||||
|
||||
|
||||
class DTC_REPORT_TYPE(IntEnum):
|
||||
NUMBER_OF_DTC_BY_STATUS_MASK = 0x01
|
||||
DTC_BY_STATUS_MASK = 0x02
|
||||
DTC_SNAPSHOT_IDENTIFICATION = 0x03
|
||||
DTC_SNAPSHOT_RECORD_BY_DTC_NUMBER = 0x04
|
||||
DTC_EXTENDED_DATA_RECORD_BY_DTC_NUMBER = 0x06
|
||||
SUPPORTED_DTC = 0x0A
|
||||
|
||||
|
||||
UDS_NRC_DESCRIPTIONS: dict[int, str] = {
|
||||
0x10: "General Reject",
|
||||
0x11: "Service Not Supported",
|
||||
0x12: "Sub-function Not Supported",
|
||||
0x13: "Incorrect Message Length Or Invalid Format",
|
||||
0x14: "Response Too Long",
|
||||
0x21: "Busy Repeat Request",
|
||||
0x22: "Conditions Not Correct",
|
||||
0x24: "Request Sequence Error",
|
||||
0x25: "No Response From Subnet Component",
|
||||
0x26: "Failure Prevents Execution Of Requested Action",
|
||||
0x31: "Request Out Of Range",
|
||||
0x33: "Security Access Denied",
|
||||
0x35: "Invalid Key",
|
||||
0x36: "Exceed Number Of Attempts",
|
||||
0x37: "Required Time Delay Not Expired",
|
||||
0x70: "Upload Download Not Accepted",
|
||||
0x71: "Transfer Data Suspended",
|
||||
0x72: "General Programming Failure",
|
||||
0x73: "Wrong Block Sequence Counter",
|
||||
0x78: "Response Pending",
|
||||
0x7E: "Sub-function Not Supported In Active Session",
|
||||
0x7F: "Service Not Supported In Active Session",
|
||||
}
|
||||
|
||||
|
||||
class UdsNegativeResponseError(Exception):
|
||||
def __init__(self, service_id: int, nrc: int):
|
||||
self.service_id = service_id
|
||||
self.nrc = nrc
|
||||
desc = UDS_NRC_DESCRIPTIONS.get(nrc, f"NRC 0x{nrc:02X}")
|
||||
super().__init__(f"UDS Negative Response: Service 0x{service_id:02X} failed with {desc} (0x{nrc:02X})")
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DiagnosticTroubleCode:
|
||||
code: str
|
||||
ecu: str | None = None
|
||||
status: int | None = None
|
||||
source: str = "OBD"
|
||||
description: str | None = None
|
||||
raw: bytes = b""
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return {
|
||||
"code": self.code,
|
||||
"ecu": self.ecu,
|
||||
"status": self.status,
|
||||
"source": self.source,
|
||||
"description": self.description,
|
||||
"raw": self.raw.hex().upper() if self.raw else "",
|
||||
}
|
||||
|
||||
|
||||
def parse_standard_dtcs(data: bytes, source: str = "OBD") -> list[DiagnosticTroubleCode]:
|
||||
"""Parse 2-byte standard SAE J1979 DTCs (e.g. from Mode 03, 07, 0A)."""
|
||||
dtcs: list[DiagnosticTroubleCode] = []
|
||||
prefix_map = {0: "P", 1: "C", 2: "B", 3: "U"}
|
||||
|
||||
# Payload starts after response service byte (e.g. 0x43, 0x47, 0x4A)
|
||||
payload = data[1:] if len(data) > 0 and data[0] in (0x43, 0x47, 0x4A) else data
|
||||
|
||||
for i in range(0, len(payload) - 1, 2):
|
||||
b0, b1 = payload[i], payload[i + 1]
|
||||
if b0 == 0 and b1 == 0:
|
||||
continue # padding / no DTC
|
||||
prefix = prefix_map.get((b0 >> 6) & 0x03, "P")
|
||||
d1 = (b0 >> 4) & 0x03
|
||||
d2 = b0 & 0x0F
|
||||
d3 = (b1 >> 4) & 0x0F
|
||||
d4 = b1 & 0x0F
|
||||
code = f"{prefix}{d1}{d2:X}{d3:X}{d4:X}"
|
||||
dtcs.append(DiagnosticTroubleCode(
|
||||
code=code,
|
||||
source=source,
|
||||
raw=bytes([b0, b1]),
|
||||
))
|
||||
return dtcs
|
||||
|
||||
|
||||
def parse_uds_dtcs(data: bytes, ecu: str | None = None) -> list[DiagnosticTroubleCode]:
|
||||
"""Parse UDS 3-byte DTCs (2 bytes DTC + 1 byte status mask) from Service 0x19 response."""
|
||||
dtcs: list[DiagnosticTroubleCode] = []
|
||||
prefix_map = {0: "P", 1: "C", 2: "B", 3: "U"}
|
||||
|
||||
# Positive response: 0x59 <ReportType> <StatusAvailabilityMask> <DTC1_B0> <DTC1_B1> <DTC1_Status> ...
|
||||
if len(data) < 3 or data[0] != 0x59:
|
||||
return dtcs
|
||||
|
||||
records = data[3:] # Skip 59, report_type, status_mask
|
||||
for i in range(0, len(records) - 2, 3):
|
||||
b0, b1, status = records[i], records[i + 1], records[i + 2]
|
||||
if b0 == 0 and b1 == 0:
|
||||
continue
|
||||
prefix = prefix_map.get((b0 >> 6) & 0x03, "P")
|
||||
d1 = (b0 >> 4) & 0x03
|
||||
d2 = b0 & 0x0F
|
||||
d3 = (b1 >> 4) & 0x0F
|
||||
d4 = b1 & 0x0F
|
||||
code = f"{prefix}{d1}{d2:X}{d3:X}{d4:X}"
|
||||
dtcs.append(DiagnosticTroubleCode(
|
||||
code=code,
|
||||
ecu=ecu,
|
||||
status=status,
|
||||
source="UDS",
|
||||
raw=bytes([b0, b1, status]),
|
||||
))
|
||||
return dtcs
|
||||
|
||||
|
||||
# Standard OBD-II Functions
|
||||
|
||||
def read_current_data(elm: Elm327, pid: int, context: ElmContext | None = None) -> DiagnosticResponse:
|
||||
"""Mode 01: Read current powertrain diagnostic data."""
|
||||
return elm.request(bytes([0x01, pid & 0xFF]), context, retry=True)
|
||||
|
||||
|
||||
def read_freeze_frame(elm: Elm327, pid: int, frame: int = 0, context: ElmContext | None = None) -> DiagnosticResponse:
|
||||
"""Mode 02: Read freeze frame data."""
|
||||
return elm.request(bytes([0x02, pid & 0xFF, frame & 0xFF]), context, retry=True)
|
||||
|
||||
|
||||
def read_stored_dtcs(elm: Elm327, context: ElmContext | None = None) -> list[DiagnosticTroubleCode]:
|
||||
"""Mode 03: Read confirmed/stored emission-related DTCs."""
|
||||
res = elm.request(bytes([0x03]), context, retry=True)
|
||||
return parse_standard_dtcs(res.payload, source="OBD_STORED")
|
||||
|
||||
|
||||
def read_pending_dtcs(elm: Elm327, context: ElmContext | None = None) -> list[DiagnosticTroubleCode]:
|
||||
"""Mode 07: Read pending DTCs detected during current/last drive cycle."""
|
||||
res = elm.request(bytes([0x07]), context, retry=True)
|
||||
return parse_standard_dtcs(res.payload, source="OBD_PENDING")
|
||||
|
||||
|
||||
def read_permanent_dtcs(elm: Elm327, context: ElmContext | None = None) -> list[DiagnosticTroubleCode]:
|
||||
"""Mode 0A: Read permanent DTCs."""
|
||||
res = elm.request(bytes([0x0A]), context, retry=True)
|
||||
return parse_standard_dtcs(res.payload, source="OBD_PERMANENT")
|
||||
|
||||
|
||||
def read_all_dtcs(elm: Elm327, context: ElmContext | None = None) -> list[DiagnosticTroubleCode]:
|
||||
"""Read confirmed (03), pending (07), and permanent (0A) DTCs."""
|
||||
results: list[DiagnosticTroubleCode] = []
|
||||
for func in (read_stored_dtcs, read_pending_dtcs, read_permanent_dtcs):
|
||||
try:
|
||||
results.extend(func(elm, context))
|
||||
except Exception:
|
||||
pass
|
||||
return results
|
||||
|
||||
|
||||
def clear_dtcs(elm: Elm327, context: ElmContext | None = None) -> DiagnosticResponse:
|
||||
"""Mode 04: Clear diagnostic trouble codes and reset MIL (Check Engine Light). Mutating!"""
|
||||
return elm.request(bytes([0x04]), context, retry=False)
|
||||
|
||||
|
||||
def read_vin(elm: Elm327, context: ElmContext | None = None) -> str:
|
||||
"""Mode 09 PID 02: Read Vehicle Identification Number (VIN)."""
|
||||
res = elm.request(bytes([0x09, 0x02]), context, retry=True)
|
||||
payload = res.payload
|
||||
|
||||
# Response format: 49 02 <data_items_or_line_nums> followed by ASCII VIN characters
|
||||
# Strip service 49 02 header if present
|
||||
if len(payload) >= 2 and payload[0] == 0x49 and payload[1] == 0x02:
|
||||
payload = payload[2:]
|
||||
|
||||
# Filter ascii alphanumeric printable characters
|
||||
ascii_chars = "".join(chr(b) for b in payload if 32 <= b <= 126)
|
||||
vin_match = re.search(r"([A-HJ-NPR-Z0-9]{17})", ascii_chars)
|
||||
if vin_match:
|
||||
return vin_match.group(1)
|
||||
return ascii_chars.strip()
|
||||
|
||||
|
||||
# UDS (ISO 14229) Service Functions
|
||||
|
||||
def uds_read_data_by_identifier(elm: Elm327, did: int, context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x22: ReadDataByIdentifier."""
|
||||
payload = bytes([SERVICE_TYPE.READ_DATA_BY_IDENTIFIER, (did >> 8) & 0xFF, did & 0xFF])
|
||||
res = elm.request(payload, context, retry=True)
|
||||
data = res.payload
|
||||
|
||||
# Positive response: 0x62 <DID_MSB> <DID_LSB> <DataBytes...>
|
||||
if len(data) >= 3 and data[0] == 0x62:
|
||||
resp_did = (data[1] << 8) | data[2]
|
||||
if resp_did == did:
|
||||
return data[3:]
|
||||
return data[1:]
|
||||
return data
|
||||
|
||||
|
||||
def uds_write_data_by_identifier(elm: Elm327, did: int, data: bytes, context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x2E: WriteDataByIdentifier. Mutating!"""
|
||||
payload = bytes([SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER, (did >> 8) & 0xFF, did & 0xFF]) + data
|
||||
res = elm.request(payload, context, retry=False)
|
||||
return res.payload
|
||||
|
||||
|
||||
def uds_diagnostic_session_control(elm: Elm327, session_type: int, context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x10: DiagnosticSessionControl."""
|
||||
payload = bytes([SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, session_type & 0xFF])
|
||||
res = elm.request(payload, context, retry=False)
|
||||
return res.payload
|
||||
|
||||
|
||||
def uds_ecu_reset(elm: Elm327, reset_type: int, context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x11: ECUReset. Mutating!"""
|
||||
payload = bytes([SERVICE_TYPE.ECU_RESET, reset_type & 0xFF])
|
||||
res = elm.request(payload, context, retry=False)
|
||||
return res.payload
|
||||
|
||||
|
||||
def uds_read_dtc_information(elm: Elm327, report_type: int = DTC_REPORT_TYPE.DTC_BY_STATUS_MASK,
|
||||
status_mask: int = 0xFF, context: ElmContext | None = None,
|
||||
ecu: str | None = None) -> list[DiagnosticTroubleCode]:
|
||||
"""UDS Service 0x19: ReadDTCInformation."""
|
||||
payload = bytes([SERVICE_TYPE.READ_DTC_INFORMATION, report_type & 0xFF, status_mask & 0xFF])
|
||||
res = elm.request(payload, context, retry=True)
|
||||
return parse_uds_dtcs(res.payload, ecu=ecu)
|
||||
|
||||
|
||||
def uds_clear_diagnostic_information(elm: Elm327, group: int = 0xFFFFFF, context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x14: ClearDiagnosticInformation. Mutating!"""
|
||||
payload = bytes([
|
||||
SERVICE_TYPE.CLEAR_DIAGNOSTIC_INFORMATION,
|
||||
(group >> 16) & 0xFF,
|
||||
(group >> 8) & 0xFF,
|
||||
group & 0xFF,
|
||||
])
|
||||
res = elm.request(payload, context, retry=False)
|
||||
return res.payload
|
||||
|
||||
|
||||
def uds_security_access(elm: Elm327, access_type: int, key_data: bytes = b"", context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x27: SecurityAccess (Request Seed or Send Key)."""
|
||||
payload = bytes([SERVICE_TYPE.SECURITY_ACCESS, access_type & 0xFF]) + key_data
|
||||
is_mutating = (access_type == ACCESS_TYPE.SEND_KEY)
|
||||
res = elm.request(payload, context, retry=not is_mutating)
|
||||
return res.payload
|
||||
|
||||
|
||||
def uds_routine_control(elm: Elm327, routine_type: int, routine_id: int, option_record: bytes = b"", context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x31: RoutineControl. Mutating!"""
|
||||
payload = bytes([
|
||||
SERVICE_TYPE.ROUTINE_CONTROL,
|
||||
routine_type & 0xFF,
|
||||
(routine_id >> 8) & 0xFF,
|
||||
routine_id & 0xFF,
|
||||
]) + option_record
|
||||
res = elm.request(payload, context, retry=False)
|
||||
return res.payload
|
||||
|
||||
|
||||
def uds_input_output_control(elm: Elm327, did: int, control_option: int, control_state: bytes = b"", context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x2F: InputOutputControlByIdentifier. Mutating!"""
|
||||
payload = bytes([
|
||||
SERVICE_TYPE.INPUT_OUTPUT_CONTROL_BY_IDENTIFIER,
|
||||
(did >> 8) & 0xFF,
|
||||
did & 0xFF,
|
||||
control_option & 0xFF,
|
||||
]) + control_state
|
||||
res = elm.request(payload, context, retry=False)
|
||||
return res.payload
|
||||
|
||||
|
||||
def uds_tester_present(elm: Elm327, subfunction: int = 0x00, context: ElmContext | None = None) -> bytes:
|
||||
"""UDS Service 0x3E: TesterPresent."""
|
||||
payload = bytes([SERVICE_TYPE.TESTER_PRESENT, subfunction & 0xFF])
|
||||
res = elm.request(payload, context, retry=True)
|
||||
return res.payload
|
||||
|
||||
|
||||
# Safety Classification Helpers
|
||||
|
||||
READ_ONLY_SERVICES = {
|
||||
0x01, 0x02, 0x03, 0x07, 0x09, 0x0A,
|
||||
SERVICE_TYPE.READ_DTC_INFORMATION,
|
||||
SERVICE_TYPE.READ_DATA_BY_IDENTIFIER,
|
||||
SERVICE_TYPE.READ_MEMORY_BY_ADDRESS,
|
||||
SERVICE_TYPE.READ_SCALING_DATA_BY_IDENTIFIER,
|
||||
SERVICE_TYPE.TESTER_PRESENT,
|
||||
}
|
||||
|
||||
MUTATING_SERVICES = {
|
||||
0x04,
|
||||
SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
|
||||
SERVICE_TYPE.ECU_RESET,
|
||||
SERVICE_TYPE.CLEAR_DIAGNOSTIC_INFORMATION,
|
||||
SERVICE_TYPE.SECURITY_ACCESS,
|
||||
SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER,
|
||||
SERVICE_TYPE.INPUT_OUTPUT_CONTROL_BY_IDENTIFIER,
|
||||
SERVICE_TYPE.ROUTINE_CONTROL,
|
||||
SERVICE_TYPE.REQUEST_DOWNLOAD,
|
||||
SERVICE_TYPE.REQUEST_UPLOAD,
|
||||
SERVICE_TYPE.TRANSFER_DATA,
|
||||
SERVICE_TYPE.REQUEST_TRANSFER_EXIT,
|
||||
SERVICE_TYPE.WRITE_MEMORY_BY_ADDRESS,
|
||||
}
|
||||
|
||||
|
||||
def is_read_only_service(service: int) -> bool:
|
||||
return service in READ_ONLY_SERVICES
|
||||
|
||||
|
||||
def is_mutating_service(service: int) -> bool:
|
||||
return service in MUTATING_SERVICES
|
||||
|
||||
|
||||
def is_read_only_payload(payload: bytes) -> bool:
|
||||
if not payload:
|
||||
return True
|
||||
sid = payload[0]
|
||||
if sid == SERVICE_TYPE.SECURITY_ACCESS:
|
||||
# Subfunction 0x01 (Request Seed) is read-only; Subfunction 0x02 (Send Key) is mutating
|
||||
subfn = payload[1] if len(payload) > 1 else 0
|
||||
return subfn % 2 == 1 # Odd = request seed
|
||||
return is_read_only_service(sid)
|
||||
@@ -0,0 +1,342 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
import time
|
||||
from dataclasses import dataclass, field
|
||||
from openpilot.starpilot.system.obdyssey.transport import ElmTransport
|
||||
|
||||
|
||||
class ElmError(Exception):
|
||||
"""Base exception for ELM327 operations."""
|
||||
|
||||
|
||||
class ElmTimeoutError(ElmError):
|
||||
"""ELM327 command timed out waiting for prompt."""
|
||||
|
||||
|
||||
class ElmDisconnectedError(ElmError):
|
||||
"""ELM327 transport connection lost."""
|
||||
|
||||
|
||||
class ElmCommandError(ElmError):
|
||||
"""ELM327 returned unrecognized command or syntax error."""
|
||||
|
||||
|
||||
class ElmNoDataError(ElmError):
|
||||
"""Vehicle or ECU returned NO DATA."""
|
||||
|
||||
|
||||
class ElmBusError(ElmError):
|
||||
"""CAN error, bus initialization error, or unable to connect."""
|
||||
|
||||
|
||||
class ElmUnsupportedError(ElmError):
|
||||
"""Requested protocol or feature is not supported by adapter."""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AdapterInfo:
|
||||
identity: str = ""
|
||||
reported_version: str = ""
|
||||
voltage: float | None = None
|
||||
active_protocol: str = ""
|
||||
capabilities: tuple[str, ...] = ()
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ElmContext:
|
||||
protocol: str | None = None
|
||||
tx_header: int | None = None
|
||||
rx_filter: int | None = None
|
||||
priority: int | None = None
|
||||
response_priority: int | None = None
|
||||
extended_address: int | None = None
|
||||
tester_address: int | None = None
|
||||
timeout: int | None = None
|
||||
flow_control: bool | None = None
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DiagnosticResponse:
|
||||
raw: bytes
|
||||
payload: bytes
|
||||
lines: tuple[str, ...] = ()
|
||||
service: int | None = None
|
||||
|
||||
|
||||
PROTOCOL_MAP: dict[str, str] = {
|
||||
"AUTO": "0",
|
||||
"0": "0",
|
||||
"SAE J1850 PWM": "1",
|
||||
"1": "1",
|
||||
"SAE J1850 VPW": "2",
|
||||
"2": "2",
|
||||
"ISO 9141-2": "3",
|
||||
"3": "3",
|
||||
"ISO 14230-4 (KWP 5BAUD)": "4",
|
||||
"4": "4",
|
||||
"ISO 14230-4 (KWP FAST)": "5",
|
||||
"5": "5",
|
||||
"ISO 15765-4 (CAN 11/500)": "6",
|
||||
"CAN 11/500": "6",
|
||||
"CAN_11_500": "6",
|
||||
"15765-4-11BIT-500K": "6",
|
||||
"6": "6",
|
||||
"ISO 15765-4 (CAN 29/500)": "7",
|
||||
"CAN 29/500": "7",
|
||||
"CAN_29_500": "7",
|
||||
"15765-4-29BIT-500K": "7",
|
||||
"7": "7",
|
||||
"ISO 15765-4 (CAN 11/250)": "8",
|
||||
"CAN 11/250": "8",
|
||||
"CAN_11_250": "8",
|
||||
"15765-4-11BIT-250K": "8",
|
||||
"8": "8",
|
||||
"ISO 15765-4 (CAN 29/250)": "9",
|
||||
"CAN 29/250": "9",
|
||||
"CAN_29_250": "9",
|
||||
"15765-4-29BIT-250K": "9",
|
||||
"9": "9",
|
||||
"SAE J1939 (CAN 29/250)": "A",
|
||||
"A": "A",
|
||||
}
|
||||
|
||||
IGNORE_PATTERNS = {
|
||||
"SEARCHING...",
|
||||
"BUS INIT...",
|
||||
"BUS INIT: OK",
|
||||
"STOPPED",
|
||||
"...",
|
||||
}
|
||||
|
||||
ERROR_RESPONSES = {
|
||||
"CAN ERROR": ElmBusError,
|
||||
"BUS ERROR": ElmBusError,
|
||||
"UNABLE TO CONNECT": ElmBusError,
|
||||
"BUS INIT: ERROR": ElmBusError,
|
||||
"FB ERROR": ElmBusError,
|
||||
"BUFFER FULL": ElmBusError,
|
||||
"?": ElmCommandError,
|
||||
"NO DATA": ElmNoDataError,
|
||||
}
|
||||
|
||||
|
||||
class Elm327:
|
||||
def __init__(self, transport: ElmTransport, default_timeout: float = 3.0):
|
||||
self.transport = transport
|
||||
self.default_timeout = default_timeout
|
||||
self.adapter_info: AdapterInfo | None = None
|
||||
self.active_context: ElmContext | None = None
|
||||
self._max_buffer_size = 4096
|
||||
|
||||
def initialize(self) -> AdapterInfo:
|
||||
self.reset_context()
|
||||
|
||||
# Base compatibility initialization sequence
|
||||
self.command("ATZ", timeout=3.0)
|
||||
time.sleep(0.05)
|
||||
self.command("ATE0", timeout=2.0) # Echo off
|
||||
self.command("ATL0", timeout=2.0) # Linefeeds off
|
||||
self.command("ATS0", timeout=2.0) # Spaces off
|
||||
self.command("ATR1", timeout=2.0) # Responses on
|
||||
|
||||
# Identify and get voltage
|
||||
identity_lines = self.command("ATI", timeout=2.0)
|
||||
identity = " ".join(identity_lines).strip() or "ELM327 compatible"
|
||||
|
||||
voltage_lines = self.command("ATRV", timeout=2.0)
|
||||
voltage_str = " ".join(voltage_lines).strip()
|
||||
voltage = None
|
||||
volt_match = re.search(r"(\d+(?:\.\d+)?)V?", voltage_str, re.IGNORECASE)
|
||||
if volt_match:
|
||||
try:
|
||||
voltage = float(volt_match.group(1))
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
# Discover active protocol
|
||||
protocol = ""
|
||||
try:
|
||||
proto_lines = self.command("ATDP", timeout=2.0)
|
||||
protocol = " ".join(proto_lines).strip()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
self.adapter_info = AdapterInfo(
|
||||
identity=identity,
|
||||
reported_version=identity,
|
||||
voltage=voltage,
|
||||
active_protocol=protocol,
|
||||
)
|
||||
return self.adapter_info
|
||||
|
||||
def reset_context(self) -> None:
|
||||
self.active_context = None
|
||||
|
||||
def command(self, cmd: str, timeout: float | None = None) -> list[str]:
|
||||
cmd_clean = cmd.strip()
|
||||
write_data = (cmd_clean + "\r").encode("ascii")
|
||||
try:
|
||||
self.transport.write(write_data)
|
||||
except ConnectionResetError as err:
|
||||
self.reset_context()
|
||||
raise ElmDisconnectedError(f"Connection lost writing {cmd_clean}: {err}") from err
|
||||
except TimeoutError as err:
|
||||
raise ElmTimeoutError(f"Timeout writing {cmd_clean}: {err}") from err
|
||||
except Exception as err:
|
||||
raise ElmError(f"Write failed for {cmd_clean}: {err}") from err
|
||||
|
||||
timeout_val = timeout if timeout is not None else self.default_timeout
|
||||
deadline = time.monotonic() + timeout_val
|
||||
buffer = bytearray()
|
||||
|
||||
while time.monotonic() < deadline:
|
||||
remaining = max(0.01, deadline - time.monotonic())
|
||||
try:
|
||||
chunk = self.transport.read(self._max_buffer_size)
|
||||
if chunk:
|
||||
buffer.extend(chunk)
|
||||
if len(buffer) > self._max_buffer_size:
|
||||
del buffer[:-self._max_buffer_size]
|
||||
if b">" in buffer:
|
||||
break
|
||||
except ConnectionResetError as err:
|
||||
self.reset_context()
|
||||
raise ElmDisconnectedError(f"Connection lost reading {cmd_clean}: {err}") from err
|
||||
except TimeoutError:
|
||||
if time.monotonic() >= deadline:
|
||||
break
|
||||
except Exception as err:
|
||||
raise ElmError(f"Read failed for {cmd_clean}: {err}") from err
|
||||
|
||||
if b">" not in buffer:
|
||||
raise ElmTimeoutError(f"Timeout waiting for ELM prompt for '{cmd_clean}' (received: {bytes(buffer)!r})")
|
||||
|
||||
# Remove the prompt '>' and split into lines
|
||||
raw_text = buffer.decode("ascii", errors="ignore")
|
||||
prompt_idx = raw_text.rfind(">")
|
||||
if prompt_idx != -1:
|
||||
raw_text = raw_text[:prompt_idx]
|
||||
|
||||
# Normalize newlines and filter lines
|
||||
raw_lines = [line.strip() for line in re.split(r"[\r\n]+", raw_text) if line.strip()]
|
||||
cleaned_lines: list[str] = []
|
||||
|
||||
for line in raw_lines:
|
||||
normalized = line.replace(" ", "").upper()
|
||||
cmd_norm = cmd_clean.replace(" ", "").upper()
|
||||
|
||||
# Strip command echo if clone still echoed it
|
||||
if normalized == cmd_norm or normalized.startswith(cmd_norm):
|
||||
continue
|
||||
|
||||
# Filter out noise messages
|
||||
if normalized in IGNORE_PATTERNS or any(normalized.startswith(p) for p in IGNORE_PATTERNS):
|
||||
continue
|
||||
|
||||
# Check for known errors
|
||||
for err_key, err_cls in ERROR_RESPONSES.items():
|
||||
if normalized == err_key.replace(" ", "").upper():
|
||||
raise err_cls(f"ELM returned {line} for command '{cmd_clean}'")
|
||||
|
||||
cleaned_lines.append(line)
|
||||
|
||||
return cleaned_lines
|
||||
|
||||
def apply_context(self, context: ElmContext) -> None:
|
||||
if self.active_context == context:
|
||||
return
|
||||
|
||||
prev = self.active_context or ElmContext()
|
||||
|
||||
# 1. Protocol change
|
||||
if context.protocol is not None and context.protocol != prev.protocol:
|
||||
proto_key = str(context.protocol).upper().strip()
|
||||
proto_code = PROTOCOL_MAP.get(proto_key, proto_key)
|
||||
self.command(f"ATSP{proto_code}")
|
||||
|
||||
# 2. Transmit Header / Priority change
|
||||
header_changed = (context.tx_header != prev.tx_header) or (context.priority != prev.priority)
|
||||
if header_changed and context.tx_header is not None:
|
||||
if context.priority is not None:
|
||||
# 29-bit CAN ID with priority override
|
||||
full_id = ((context.priority & 0xFF) << 24) | (context.tx_header & 0x00FFFFFF)
|
||||
self.command(f"ATSH{full_id:08X}")
|
||||
elif context.tx_header > 0x7FF:
|
||||
# Standard 29-bit header
|
||||
self.command(f"ATSH{context.tx_header:08X}")
|
||||
else:
|
||||
# Standard 11-bit header
|
||||
self.command(f"ATSH{context.tx_header:03X}")
|
||||
|
||||
# 3. Receive Filter change
|
||||
if context.rx_filter is not None and context.rx_filter != prev.rx_filter:
|
||||
if context.rx_filter > 0x7FF:
|
||||
self.command(f"ATCRA{context.rx_filter:08X}")
|
||||
else:
|
||||
self.command(f"ATCRA{context.rx_filter:03X}")
|
||||
|
||||
# 4. Extended Address change
|
||||
if context.extended_address != prev.extended_address:
|
||||
if context.extended_address is not None:
|
||||
self.command(f"ATCEA{context.extended_address:02X}")
|
||||
else:
|
||||
try:
|
||||
self.command("ATCEA")
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# 5. Flow Control change
|
||||
if context.flow_control is not None and context.flow_control != prev.flow_control:
|
||||
self.command("ATCAF1" if context.flow_control else "ATCAF0")
|
||||
|
||||
# 6. Timeout change (ATST in 4ms increments: 0-FF)
|
||||
if context.timeout is not None and context.timeout != prev.timeout:
|
||||
st_val = max(0, min(255, int(context.timeout)))
|
||||
self.command(f"ATST{st_val:02X}")
|
||||
|
||||
self.active_context = context
|
||||
|
||||
def request(self, payload: bytes, context: ElmContext | None = None, *, timeout: float | None = None, retry: bool = True) -> DiagnosticResponse:
|
||||
if context is not None:
|
||||
self.apply_context(context)
|
||||
|
||||
payload_hex = payload.hex().upper()
|
||||
try:
|
||||
lines = self.command(payload_hex, timeout=timeout)
|
||||
except ElmDisconnectedError:
|
||||
if retry:
|
||||
# Attempt single reconnect and retry for read operations
|
||||
self.transport.connect()
|
||||
self.initialize()
|
||||
if context is not None:
|
||||
self.apply_context(context)
|
||||
lines = self.command(payload_hex, timeout=timeout)
|
||||
else:
|
||||
raise
|
||||
|
||||
# Extract hex bytes from response lines
|
||||
extracted_bytes = bytearray()
|
||||
for line in lines:
|
||||
# Strip spaces and any ISO-TP frame indexing (e.g. '0: 49 02 01' or '1:')
|
||||
cleaned = re.sub(r"^[0-9A-Fa-f]+:\s*", "", line)
|
||||
cleaned = cleaned.replace(" ", "").strip()
|
||||
if re.fullmatch(r"[0-9A-Fa-f]+", cleaned) and len(cleaned) % 2 == 0:
|
||||
extracted_bytes.extend(bytes.fromhex(cleaned))
|
||||
|
||||
raw_bytes = bytes(extracted_bytes)
|
||||
service_id = raw_bytes[0] if raw_bytes else None
|
||||
|
||||
# Check for UDS Negative Response: 0x7F <RequestedSID> <NRC>
|
||||
if len(raw_bytes) >= 3 and raw_bytes[0] == 0x7F:
|
||||
from openpilot.starpilot.system.obdyssey.diagnostics import UdsNegativeResponseError
|
||||
req_sid = raw_bytes[1]
|
||||
nrc = raw_bytes[2]
|
||||
raise UdsNegativeResponseError(req_sid, nrc)
|
||||
|
||||
return DiagnosticResponse(
|
||||
raw=raw_bytes,
|
||||
payload=raw_bytes,
|
||||
lines=tuple(lines),
|
||||
service=service_id,
|
||||
)
|
||||
@@ -0,0 +1,317 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Any
|
||||
|
||||
from openpilot.starpilot.system.obdyssey.elm327 import ElmContext
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SignalFormat:
|
||||
bix: int = 0
|
||||
len: int = 8
|
||||
blsb: bool = False
|
||||
sign: bool = False
|
||||
mul: float = 1.0
|
||||
div: float = 1.0
|
||||
add: float = 0.0
|
||||
min: float | None = None
|
||||
max: float | None = None
|
||||
nullmin: float | None = None
|
||||
nullmax: float | None = None
|
||||
map: dict[str, str] | None = None
|
||||
unit: str = ""
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, data: dict[str, Any]) -> SignalFormat:
|
||||
return cls(
|
||||
bix=int(data.get("bix", 0)),
|
||||
len=int(data.get("len", 8)),
|
||||
blsb=bool(data.get("blsb", False)),
|
||||
sign=bool(data.get("sign", False)),
|
||||
mul=float(data.get("mul", 1.0)),
|
||||
div=float(data.get("div", 1.0)),
|
||||
add=float(data.get("add", 0.0)),
|
||||
min=float(data["min"]) if "min" in data and data["min"] is not None else None,
|
||||
max=float(data["max"]) if "max" in data and data["max"] is not None else None,
|
||||
nullmin=float(data["nullmin"]) if "nullmin" in data and data["nullmin"] is not None else None,
|
||||
nullmax=float(data["nullmax"]) if "nullmax" in data and data["nullmax"] is not None else None,
|
||||
map={str(k): str(v) for k, v in data["map"].items()} if "map" in data and isinstance(data["map"], dict) else None,
|
||||
unit=str(data.get("unit", "")),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SignalDefinition:
|
||||
id: str
|
||||
name: str
|
||||
path: str | None = None
|
||||
suggested_metric: str | None = None
|
||||
format: SignalFormat = field(default_factory=SignalFormat)
|
||||
synthetic: dict[str, Any] | None = None
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, data: dict[str, Any]) -> SignalDefinition:
|
||||
fmt_dict = data.get("fmt") or data.get("format") or {}
|
||||
return cls(
|
||||
id=str(data.get("id", "")),
|
||||
name=str(data.get("name", data.get("id", ""))),
|
||||
path=str(data.get("path")) if data.get("path") is not None else None,
|
||||
suggested_metric=str(data.get("suggested_metric")) if data.get("suggested_metric") is not None else None,
|
||||
format=SignalFormat.from_dict(fmt_dict) if fmt_dict else SignalFormat(),
|
||||
synthetic=data.get("synthetic"),
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DiagnosticCommand:
|
||||
id: str
|
||||
context: ElmContext
|
||||
service: int
|
||||
parameter: bytes
|
||||
frequency: float = 1.0
|
||||
diagnostic_session_in: int | None = None
|
||||
diagnostic_session_out: int | None = None
|
||||
signals: tuple[SignalDefinition, ...] = ()
|
||||
expected_prefix: bytes = b""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class VehicleProfile:
|
||||
id: str
|
||||
name: str
|
||||
provider: str = "obdb"
|
||||
revision: str = ""
|
||||
commands: tuple[DiagnosticCommand, ...] = ()
|
||||
signals: dict[str, SignalDefinition] = field(default_factory=dict)
|
||||
synthetic_signals: dict[str, SignalDefinition] = field(default_factory=dict)
|
||||
metadata: dict[str, Any] = field(default_factory=dict)
|
||||
|
||||
|
||||
def _parse_int(value: Any) -> int | None:
|
||||
if value is None or value == "":
|
||||
return None
|
||||
if isinstance(value, int):
|
||||
return value
|
||||
val_str = str(value).strip()
|
||||
if val_str.startswith(("0x", "0X")):
|
||||
return int(val_str, 16)
|
||||
if re.fullmatch(r"[0-9A-Fa-f]+", val_str) and not val_str.isdigit():
|
||||
return int(val_str, 16)
|
||||
return int(val_str)
|
||||
|
||||
|
||||
def _parse_bytes(value: Any) -> bytes:
|
||||
if value is None:
|
||||
return b""
|
||||
if isinstance(value, bytes):
|
||||
return value
|
||||
if isinstance(value, list):
|
||||
return bytes(value)
|
||||
val_str = str(value).replace(" ", "").strip()
|
||||
if len(val_str) % 2 != 0:
|
||||
val_str = "0" + val_str
|
||||
return bytes.fromhex(val_str)
|
||||
|
||||
|
||||
def extract_raw_value(data: bytes, bix: int, bit_len: int, blsb: bool = False, sign: bool = False) -> int:
|
||||
"""Extract bit-level integer with arbitrary bit offset, length, and endianness."""
|
||||
if not data or bit_len <= 0:
|
||||
return 0
|
||||
|
||||
start_byte = bix // 8
|
||||
end_byte = (bix + bit_len - 1) // 8
|
||||
|
||||
if end_byte >= len(data):
|
||||
# Zero-pad right if payload was truncated
|
||||
padded = data + b"\x00" * (end_byte - len(data) + 1)
|
||||
else:
|
||||
padded = data
|
||||
|
||||
raw_int = 0
|
||||
if blsb:
|
||||
for i in range(end_byte, start_byte - 1, -1):
|
||||
raw_int = (raw_int << 8) | padded[i]
|
||||
else:
|
||||
for i in range(start_byte, end_byte + 1):
|
||||
raw_int = (raw_int << 8) | padded[i]
|
||||
|
||||
bit_offset = (8 - ((bix + bit_len) % 8)) % 8
|
||||
extracted = (raw_int >> bit_offset) & ((1 << bit_len) - 1)
|
||||
|
||||
if sign and (extracted & (1 << (bit_len - 1))):
|
||||
extracted -= (1 << bit_len)
|
||||
|
||||
return extracted
|
||||
|
||||
|
||||
def decode_signal(payload: bytes, signal: SignalDefinition, strip_prefix: bytes | None = None) -> Any:
|
||||
"""Deterministic signal decoder applying structured math, null checks, min/max, and enum mappings."""
|
||||
data = payload
|
||||
if strip_prefix and data.startswith(strip_prefix):
|
||||
data = data[len(strip_prefix):]
|
||||
|
||||
fmt = signal.format
|
||||
raw_val = extract_raw_value(data, fmt.bix, fmt.len, fmt.blsb, fmt.sign)
|
||||
|
||||
# Linear scaling: (raw * mul / div) + add
|
||||
div = fmt.div if fmt.div != 0 else 1.0
|
||||
scaled_val = (raw_val * fmt.mul / div) + fmt.add
|
||||
|
||||
# Null range check
|
||||
if fmt.nullmin is not None and fmt.nullmax is not None:
|
||||
if fmt.nullmin <= scaled_val <= fmt.nullmax:
|
||||
return None
|
||||
|
||||
# Min / Max bounds
|
||||
if fmt.min is not None:
|
||||
scaled_val = max(fmt.min, scaled_val)
|
||||
if fmt.max is not None:
|
||||
scaled_val = min(fmt.max, scaled_val)
|
||||
|
||||
# Enum mapping
|
||||
if fmt.map:
|
||||
key_str = str(int(round(scaled_val))) if isinstance(scaled_val, (int, float)) else str(scaled_val)
|
||||
if key_str in fmt.map:
|
||||
return fmt.map[key_str]
|
||||
|
||||
# Format clean int if exact
|
||||
if isinstance(scaled_val, float) and scaled_val.is_integer() and fmt.div == 1.0 and fmt.mul.is_integer():
|
||||
return int(scaled_val)
|
||||
|
||||
return round(scaled_val, 4) if isinstance(scaled_val, float) else scaled_val
|
||||
|
||||
|
||||
def calculate_synthetic_signal(signal: SignalDefinition, available_signals: dict[str, Any]) -> Any:
|
||||
"""Calculate post-processed synthetic signal without issuing extra diagnostic traffic."""
|
||||
synth = signal.synthetic
|
||||
if not synth or not isinstance(synth, dict):
|
||||
return None
|
||||
|
||||
op = str(synth.get("operation", "")).lower()
|
||||
source_ids = synth.get("signals", [])
|
||||
values = [available_signals.get(sid) for sid in source_ids if sid in available_signals and available_signals[sid] is not None]
|
||||
|
||||
if not values or len(values) < len(source_ids):
|
||||
return None
|
||||
|
||||
try:
|
||||
if op in ("sum", "add"):
|
||||
res = sum(values)
|
||||
elif op in ("subtract", "diff") and len(values) >= 2:
|
||||
res = values[0] - sum(values[1:])
|
||||
elif op in ("multiply", "mul") and len(values) >= 2:
|
||||
res = 1.0
|
||||
for v in values:
|
||||
res *= v
|
||||
elif op in ("divide", "div") and len(values) >= 2 and values[1] != 0:
|
||||
res = values[0] / values[1]
|
||||
elif op == "average":
|
||||
res = sum(values) / len(values)
|
||||
elif op == "min":
|
||||
res = min(values)
|
||||
elif op == "max":
|
||||
res = max(values)
|
||||
else:
|
||||
return None
|
||||
|
||||
# Apply scaling if format specified
|
||||
fmt = signal.format
|
||||
if fmt.mul != 1.0 or fmt.div != 1.0 or fmt.add != 0.0:
|
||||
div = fmt.div if fmt.div != 0 else 1.0
|
||||
res = (res * fmt.mul / div) + fmt.add
|
||||
|
||||
return round(res, 4) if isinstance(res, float) else res
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def parse_obdb_profile(data: dict[str, Any], profile_id: str = "") -> VehicleProfile:
|
||||
"""Parse OBDb v3 JSON dictionary into VehicleProfile."""
|
||||
meta = data.get("metadata") or data.get("vehicle") or {}
|
||||
pid_id = profile_id or str(meta.get("id") or meta.get("name") or "vehicle_profile")
|
||||
name = str(meta.get("name") or pid_id)
|
||||
provider = str(meta.get("provider", "obdb"))
|
||||
revision = str(meta.get("revision") or data.get("revision") or "v3")
|
||||
|
||||
all_signals: dict[str, SignalDefinition] = {}
|
||||
synthetic_signals: dict[str, SignalDefinition] = {}
|
||||
commands_list: list[DiagnosticCommand] = []
|
||||
|
||||
raw_commands = data.get("commands") or data.get("pids") or []
|
||||
for cmd_idx, cmd_data in enumerate(raw_commands):
|
||||
cmd_id = str(cmd_data.get("id", f"cmd_{cmd_idx}"))
|
||||
|
||||
# Parse context
|
||||
hdr = _parse_int(cmd_data.get("hdr") or cmd_data.get("header") or cmd_data.get("tx_header"))
|
||||
rax = _parse_int(cmd_data.get("rax") or cmd_data.get("receive_filter") or cmd_data.get("rx_filter"))
|
||||
eax = _parse_int(cmd_data.get("eax") or cmd_data.get("extended_address"))
|
||||
pri = _parse_int(cmd_data.get("pri") or cmd_data.get("priority"))
|
||||
tst = _parse_int(cmd_data.get("tst") or cmd_data.get("tester_address"))
|
||||
tmo = _parse_int(cmd_data.get("tmo") or cmd_data.get("timeout"))
|
||||
fcm1 = bool(cmd_data.get("fcm1", False))
|
||||
proto = str(cmd_data.get("proto") or cmd_data.get("protocol") or "")
|
||||
if not proto and "protocol" in meta:
|
||||
proto = str(meta["protocol"])
|
||||
|
||||
context = ElmContext(
|
||||
protocol=proto if proto else None,
|
||||
tx_header=hdr,
|
||||
rx_filter=rax,
|
||||
priority=pri,
|
||||
extended_address=eax,
|
||||
tester_address=tst,
|
||||
timeout=tmo,
|
||||
flow_control=fcm1 if "fcm1" in cmd_data else None,
|
||||
)
|
||||
|
||||
service_int = _parse_int(cmd_data.get("service", 1)) or 1
|
||||
param_bytes = _parse_bytes(cmd_data.get("pid") or cmd_data.get("parameter") or b"")
|
||||
freq = float(cmd_data.get("freq", 1.0))
|
||||
din = _parse_int(cmd_data.get("din"))
|
||||
dout = _parse_int(cmd_data.get("dout"))
|
||||
|
||||
# Expected prefix (e.g. 0x62 <DID> or 0x41 <PID>)
|
||||
eax_prefix = _parse_bytes(cmd_data.get("eax_prefix") or b"")
|
||||
if not eax_prefix:
|
||||
resp_service = service_int + 0x40
|
||||
eax_prefix = bytes([resp_service]) + param_bytes
|
||||
|
||||
cmd_signals: list[SignalDefinition] = []
|
||||
for sig_data in cmd_data.get("signals", []):
|
||||
sig_def = SignalDefinition.from_dict(sig_data)
|
||||
cmd_signals.append(sig_def)
|
||||
all_signals[sig_def.id] = sig_def
|
||||
|
||||
diag_cmd = DiagnosticCommand(
|
||||
id=cmd_id,
|
||||
context=context,
|
||||
service=service_int,
|
||||
parameter=param_bytes,
|
||||
frequency=freq,
|
||||
diagnostic_session_in=din,
|
||||
diagnostic_session_out=dout,
|
||||
signals=tuple(cmd_signals),
|
||||
expected_prefix=eax_prefix,
|
||||
)
|
||||
commands_list.append(diag_cmd)
|
||||
|
||||
# Parse top-level signals or synthetics
|
||||
for sig_data in data.get("signals", []):
|
||||
sig_def = SignalDefinition.from_dict(sig_data)
|
||||
if sig_def.synthetic:
|
||||
synthetic_signals[sig_def.id] = sig_def
|
||||
else:
|
||||
all_signals[sig_def.id] = sig_def
|
||||
|
||||
return VehicleProfile(
|
||||
id=pid_id,
|
||||
name=name,
|
||||
provider=provider,
|
||||
revision=revision,
|
||||
commands=tuple(commands_list),
|
||||
signals=all_signals,
|
||||
synthetic_signals=synthetic_signals,
|
||||
metadata=meta,
|
||||
)
|
||||
@@ -0,0 +1,233 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import shutil
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.starpilot.system.obdyssey.obdb import (
|
||||
DiagnosticCommand,
|
||||
SignalDefinition,
|
||||
VehicleProfile,
|
||||
parse_obdb_profile,
|
||||
)
|
||||
|
||||
BUNDLED_SAEJ1979_PATH = Path(__file__).resolve().parents[3] / "third_party" / "obdb_saej1979" / "profile.json"
|
||||
CURATED_PROFILES_DIR = Path(__file__).resolve().parent / "curated_profiles"
|
||||
DEFAULT_DATA_DIR = Path("/data/obdyssey/profiles") if Path("/data").is_dir() else Path.home() / ".comma" / "obdyssey" / "profiles"
|
||||
|
||||
# Mapping from StarPilot / openpilot CarFingerprint / CarModel to known OBDb profiles
|
||||
VEHICLE_PROFILE_MAPPINGS: dict[str, str] = {
|
||||
"CHEVROLET BOLT EV": "Chevrolet-Bolt-EV",
|
||||
"CHEVROLET BOLT EUV": "Chevrolet-Bolt-EV",
|
||||
"HYUNDAI IONIQ 5": "Hyundai-Ioniq-5",
|
||||
"HYUNDAI IONIQ 6": "Hyundai-Ioniq-6",
|
||||
"KIA EV6": "Kia-EV6",
|
||||
"TOYOTA RAV4": "Toyota-RAV4",
|
||||
"TOYOTA COROLLA": "Toyota-Corolla",
|
||||
"TOYOTA PRIUS": "Toyota-Prius",
|
||||
}
|
||||
|
||||
|
||||
def load_profile_from_file(path: Path | str, profile_id: str = "") -> VehicleProfile:
|
||||
with open(path, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
return parse_obdb_profile(data, profile_id=profile_id)
|
||||
|
||||
|
||||
class ProfileManager:
|
||||
def __init__(self, data_dir: Path | str | None = None, params: Params | None = None):
|
||||
self.data_dir = Path(data_dir) if data_dir is not None else DEFAULT_DATA_DIR
|
||||
self.params = params or Params()
|
||||
self._cached_profiles: dict[str, VehicleProfile] = {}
|
||||
self._active_profile: VehicleProfile | None = None
|
||||
self._ensure_storage()
|
||||
|
||||
def _ensure_storage(self) -> None:
|
||||
try:
|
||||
self.data_dir.mkdir(parents=True, exist_ok=True)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def load_bundled_saej1979(self) -> VehicleProfile:
|
||||
if "saej1979" in self._cached_profiles:
|
||||
return self._cached_profiles["saej1979"]
|
||||
if BUNDLED_SAEJ1979_PATH.is_file():
|
||||
profile = load_profile_from_file(BUNDLED_SAEJ1979_PATH, profile_id="saej1979")
|
||||
self._cached_profiles["saej1979"] = profile
|
||||
return profile
|
||||
# Fallback minimal profile
|
||||
return parse_obdb_profile({"metadata": {"id": "saej1979", "name": "SAE J1979 Standard OBD-II"}}, "saej1979")
|
||||
|
||||
def list_profiles(self) -> list[dict[str, Any]]:
|
||||
profiles: list[dict[str, Any]] = []
|
||||
|
||||
# 1. Bundled standard
|
||||
sae = self.load_bundled_saej1979()
|
||||
profiles.append({
|
||||
"id": sae.id,
|
||||
"name": sae.name,
|
||||
"provider": sae.provider,
|
||||
"revision": sae.revision,
|
||||
"bundled": True,
|
||||
"signal_count": len(sae.signals),
|
||||
})
|
||||
|
||||
# 2. Curated profiles in repository
|
||||
if CURATED_PROFILES_DIR.is_dir():
|
||||
for file in sorted(CURATED_PROFILES_DIR.glob("*.json")):
|
||||
try:
|
||||
prof = load_profile_from_file(file, profile_id=file.stem)
|
||||
profiles.append({
|
||||
"id": prof.id,
|
||||
"name": prof.name,
|
||||
"provider": prof.provider,
|
||||
"revision": prof.revision,
|
||||
"bundled": True,
|
||||
"signal_count": len(prof.signals),
|
||||
})
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# 3. Installed profiles in persistent data dir
|
||||
if self.data_dir.is_dir():
|
||||
for subdir in sorted(self.data_dir.iterdir()):
|
||||
prof_file = subdir / "profile.json"
|
||||
if prof_file.is_file():
|
||||
try:
|
||||
prof = load_profile_from_file(prof_file, profile_id=subdir.name)
|
||||
profiles.append({
|
||||
"id": prof.id,
|
||||
"name": prof.name,
|
||||
"provider": prof.provider,
|
||||
"revision": prof.revision,
|
||||
"bundled": False,
|
||||
"signal_count": len(prof.signals),
|
||||
})
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return profiles
|
||||
|
||||
def get_profile(self, profile_id: str) -> VehicleProfile | None:
|
||||
if not profile_id:
|
||||
return None
|
||||
if profile_id in self._cached_profiles:
|
||||
return self._cached_profiles[profile_id]
|
||||
|
||||
# 1. Check bundled standard
|
||||
if profile_id == "saej1979":
|
||||
return self.load_bundled_saej1979()
|
||||
|
||||
# 2. Check curated profiles
|
||||
curated_path = CURATED_PROFILES_DIR / f"{profile_id}.json"
|
||||
if curated_path.is_file():
|
||||
try:
|
||||
prof = load_profile_from_file(curated_path, profile_id=profile_id)
|
||||
self._cached_profiles[profile_id] = prof
|
||||
return prof
|
||||
except Exception as err:
|
||||
cloudlog.error(f"Error loading curated profile {profile_id}: {err}")
|
||||
|
||||
# 3. Check persistent storage
|
||||
installed_path = self.data_dir / profile_id / "profile.json"
|
||||
if installed_path.is_file():
|
||||
try:
|
||||
prof = load_profile_from_file(installed_path, profile_id=profile_id)
|
||||
self._cached_profiles[profile_id] = prof
|
||||
return prof
|
||||
except Exception as err:
|
||||
cloudlog.error(f"Error loading installed profile {profile_id}: {err}")
|
||||
|
||||
return None
|
||||
|
||||
def install_profile_data(self, profile_id: str, data: dict[str, Any], metadata: dict[str, Any] | None = None) -> VehicleProfile:
|
||||
"""Validate, normalize, and atomically install a profile dictionary."""
|
||||
profile = parse_obdb_profile(data, profile_id=profile_id)
|
||||
self._ensure_storage()
|
||||
|
||||
target_dir = self.data_dir / profile_id
|
||||
temp_dir = Path(tempfile.mkdtemp(prefix="obd_prof_", dir=self.data_dir))
|
||||
try:
|
||||
with open(temp_dir / "profile.json", "w", encoding="utf-8") as f:
|
||||
json.dump(data, f, indent=2)
|
||||
if metadata:
|
||||
with open(temp_dir / "metadata.json", "w", encoding="utf-8") as f:
|
||||
json.dump(metadata, f, indent=2)
|
||||
|
||||
if target_dir.exists():
|
||||
shutil.rmtree(target_dir)
|
||||
temp_dir.rename(target_dir)
|
||||
self._cached_profiles[profile_id] = profile
|
||||
return profile
|
||||
except Exception:
|
||||
if temp_dir.exists():
|
||||
shutil.rmtree(temp_dir, ignore_errors=True)
|
||||
raise
|
||||
|
||||
def remove_profile(self, profile_id: str) -> bool:
|
||||
if profile_id == "saej1979":
|
||||
raise RuntimeError("Cannot remove bundled SAE J1979 profile")
|
||||
target_dir = self.data_dir / profile_id
|
||||
self._cached_profiles.pop(profile_id, None)
|
||||
if target_dir.exists():
|
||||
shutil.rmtree(target_dir, ignore_errors=True)
|
||||
return True
|
||||
return False
|
||||
|
||||
def resolve_active_profile(self, explicit_id: str | None = None) -> VehicleProfile:
|
||||
"""Resolve active vehicle profile from explicit ID, CarParams, or fallback to SAE J1979."""
|
||||
if explicit_id:
|
||||
prof = self.get_profile(explicit_id)
|
||||
if prof is not None:
|
||||
self._active_profile = prof
|
||||
return prof
|
||||
|
||||
# Check vehicle fingerprint / make / model
|
||||
car_model = str(self.params.get("CarModel", encoding="utf-8") or "").upper().strip()
|
||||
car_make = str(self.params.get("CarMake", encoding="utf-8") or "").upper().strip()
|
||||
|
||||
# Simple rule: anything with "BOLT" -> Chevrolet-Bolt-EV
|
||||
if "BOLT" in car_model or "BOLT" in car_make:
|
||||
prof = self.get_profile("Chevrolet-Bolt-EV")
|
||||
if prof is not None:
|
||||
self._active_profile = prof
|
||||
return prof
|
||||
|
||||
mapped_id = None
|
||||
if car_model and car_model in VEHICLE_PROFILE_MAPPINGS:
|
||||
mapped_id = VEHICLE_PROFILE_MAPPINGS[car_model]
|
||||
elif car_make and car_model:
|
||||
full_name = f"{car_make} {car_model}".upper()
|
||||
for key, pid in VEHICLE_PROFILE_MAPPINGS.items():
|
||||
if key in full_name or full_name in key:
|
||||
mapped_id = pid
|
||||
break
|
||||
|
||||
if mapped_id:
|
||||
prof = self.get_profile(mapped_id)
|
||||
if prof is not None:
|
||||
self._active_profile = prof
|
||||
return prof
|
||||
|
||||
# Fallback to standard SAE J1979
|
||||
prof = self.load_bundled_saej1979()
|
||||
self._active_profile = prof
|
||||
return prof
|
||||
|
||||
@staticmethod
|
||||
def group_signals_by_command(profile: VehicleProfile, signal_ids: list[str]) -> list[tuple[DiagnosticCommand, list[SignalDefinition]]]:
|
||||
"""Group requested signal IDs by their parent DiagnosticCommand to execute each command exactly ONCE."""
|
||||
requested_set = set(signal_ids)
|
||||
grouped: list[tuple[DiagnosticCommand, list[SignalDefinition]]] = []
|
||||
|
||||
for cmd in profile.commands:
|
||||
matching_signals = [sig for sig in cmd.signals if sig.id in requested_set]
|
||||
if matching_signals:
|
||||
grouped.append((cmd, matching_signals))
|
||||
|
||||
return grouped
|
||||
@@ -0,0 +1,201 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import os
|
||||
import socket
|
||||
from dataclasses import asdict, dataclass
|
||||
from typing import Any
|
||||
|
||||
OBDYSSEY_SOCKET_PATH = "/tmp/starpilot-obdyssey.sock"
|
||||
API_VERSION = 1
|
||||
|
||||
COMMAND_TIMEOUTS = {
|
||||
"status": 5.0,
|
||||
"connect": 35.0,
|
||||
"disconnect": 10.0,
|
||||
"test_adapter": 15.0,
|
||||
"adapter_info": 5.0,
|
||||
"vehicle_info": 10.0,
|
||||
"list_signals": 5.0,
|
||||
"read_signal": 10.0,
|
||||
"read_signals": 15.0,
|
||||
"read_dtcs": 15.0,
|
||||
"read_uds_dtcs": 15.0,
|
||||
"clear_dtcs": 15.0,
|
||||
"clear_uds_dtcs": 15.0,
|
||||
"uds_request": 15.0,
|
||||
"raw_request": 15.0,
|
||||
"debug_at_command": 10.0,
|
||||
"profile_status": 5.0,
|
||||
"install_profile": 30.0,
|
||||
"update_profile": 30.0,
|
||||
"select_profile": 10.0,
|
||||
"remove_profile": 10.0,
|
||||
}
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class OBDysseyStatus:
|
||||
api_version: int = API_VERSION
|
||||
state: str = "disabled"
|
||||
enabled: bool = False
|
||||
bluetooth_enabled: bool = False
|
||||
adapter_address: str = ""
|
||||
adapter_name: str = ""
|
||||
connected: bool = False
|
||||
elm_identity: str = ""
|
||||
adapter_voltage: float | None = None
|
||||
protocol: str = ""
|
||||
profile: str = "saej1979"
|
||||
signal_count: int = 0
|
||||
last_error: str = ""
|
||||
last_request_ms: int = 0
|
||||
requests: int = 0
|
||||
errors: int = 0
|
||||
reconnects: int = 0
|
||||
link_connected: bool = False
|
||||
diagnostic_ready: bool = False
|
||||
|
||||
@classmethod
|
||||
def from_dict(cls, data: dict[str, Any]) -> OBDysseyStatus:
|
||||
connected = bool(data.get("connected", False))
|
||||
return cls(
|
||||
api_version=int(data.get("api_version", API_VERSION)),
|
||||
state=str(data.get("state", "disabled")),
|
||||
enabled=bool(data.get("enabled", False)),
|
||||
bluetooth_enabled=bool(data.get("bluetooth_enabled", False)),
|
||||
adapter_address=str(data.get("adapter_address", "")),
|
||||
adapter_name=str(data.get("adapter_name", "")),
|
||||
connected=connected,
|
||||
link_connected=bool(data.get("link_connected", connected)),
|
||||
diagnostic_ready=bool(data.get("diagnostic_ready", connected)),
|
||||
elm_identity=str(data.get("elm_identity", "")),
|
||||
adapter_voltage=float(data["adapter_voltage"]) if data.get("adapter_voltage") is not None else None,
|
||||
protocol=str(data.get("protocol", "")),
|
||||
profile=str(data.get("profile", "saej1979")),
|
||||
signal_count=int(data.get("signal_count", 0)),
|
||||
last_error=str(data.get("last_error", "")),
|
||||
last_request_ms=int(data.get("last_request_ms", 0)),
|
||||
requests=int(data.get("requests", 0)),
|
||||
errors=int(data.get("errors", 0)),
|
||||
reconnects=int(data.get("reconnects", 0)),
|
||||
)
|
||||
|
||||
def to_dict(self) -> dict[str, Any]:
|
||||
return asdict(self)
|
||||
|
||||
|
||||
class OBDysseyClient:
|
||||
def __init__(self, socket_path: str = OBDYSSEY_SOCKET_PATH, timeout: float = 5.0):
|
||||
self.socket_path = socket_path
|
||||
self.timeout = timeout
|
||||
|
||||
def call(self, cmd: str, **payload: Any) -> dict[str, Any]:
|
||||
request_data = {"command": cmd, **payload}
|
||||
request_bytes = json.dumps(request_data, separators=(",", ":")).encode("utf-8") + b"\n"
|
||||
|
||||
cmd_timeout = max(self.timeout, COMMAND_TIMEOUTS.get(cmd, 5.0))
|
||||
with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
|
||||
sock.settimeout(cmd_timeout)
|
||||
sock.connect(self.socket_path)
|
||||
sock.sendall(request_bytes)
|
||||
|
||||
response_bytes = bytearray()
|
||||
while not response_bytes.endswith(b"\n"):
|
||||
chunk = sock.recv(65536)
|
||||
if not chunk:
|
||||
break
|
||||
response_bytes.extend(chunk)
|
||||
|
||||
if not response_bytes:
|
||||
raise RuntimeError(f"OBDyssey service returned no response for '{cmd}'")
|
||||
|
||||
result = json.loads(response_bytes.decode("utf-8"))
|
||||
if not result.get("ok", False):
|
||||
err_msg = result.get("error", "OBDyssey operation failed")
|
||||
err_type = result.get("error_type", "RuntimeError")
|
||||
raise RuntimeError(f"[{err_type}] {err_msg}")
|
||||
|
||||
return result
|
||||
|
||||
def status(self) -> OBDysseyStatus:
|
||||
if not os.path.exists(self.socket_path):
|
||||
return OBDysseyStatus(state="disabled", enabled=False)
|
||||
res = self.call("status")
|
||||
return OBDysseyStatus.from_dict(res.get("status", {}))
|
||||
|
||||
def connect(self) -> dict[str, Any]:
|
||||
return self.call("connect")
|
||||
|
||||
def disconnect(self) -> dict[str, Any]:
|
||||
return self.call("disconnect")
|
||||
|
||||
def test_adapter(self) -> dict[str, Any]:
|
||||
return self.call("test_adapter")
|
||||
|
||||
def adapter_info(self) -> dict[str, Any]:
|
||||
return self.call("adapter_info")
|
||||
|
||||
def vehicle_info(self) -> dict[str, Any]:
|
||||
return self.call("vehicle_info")
|
||||
|
||||
def list_signals(self) -> list[dict[str, Any]]:
|
||||
res = self.call("list_signals")
|
||||
return res.get("signals", [])
|
||||
|
||||
def read_signal(self, signal_id: str) -> dict[str, Any]:
|
||||
res = self.call("read_signal", id=signal_id)
|
||||
return res.get("signal", {})
|
||||
|
||||
def read_signals(self, signal_ids: list[str]) -> dict[str, Any]:
|
||||
res = self.call("read_signals", ids=signal_ids)
|
||||
return res.get("signals", {})
|
||||
|
||||
def read_dtcs(self) -> list[dict[str, Any]]:
|
||||
res = self.call("read_dtcs")
|
||||
return res.get("dtcs", [])
|
||||
|
||||
def read_uds_dtcs(self, tx_addr: str, rx_addr: str) -> list[dict[str, Any]]:
|
||||
res = self.call("read_uds_dtcs", tx_addr=tx_addr, rx_addr=rx_addr)
|
||||
return res.get("dtcs", [])
|
||||
|
||||
def clear_dtcs(self) -> dict[str, Any]:
|
||||
return self.call("clear_dtcs")
|
||||
|
||||
def clear_uds_dtcs(self, tx_addr: str, rx_addr: str) -> dict[str, Any]:
|
||||
return self.call("clear_uds_dtcs", tx_addr=tx_addr, rx_addr=rx_addr)
|
||||
|
||||
def uds_request(self, tx_addr: str, rx_addr: str, payload: str, protocol: str | None = None, timeout: float | None = None) -> dict[str, Any]:
|
||||
kwargs: dict[str, Any] = {"tx_addr": tx_addr, "rx_addr": rx_addr, "payload": payload}
|
||||
if protocol is not None:
|
||||
kwargs["protocol"] = protocol
|
||||
if timeout is not None:
|
||||
kwargs["timeout"] = timeout
|
||||
return self.call("uds_request", **kwargs)
|
||||
|
||||
def raw_request(self, tx_addr: str, rx_addr: str, payload: str, protocol: str | None = None) -> dict[str, Any]:
|
||||
kwargs: dict[str, Any] = {"tx_addr": tx_addr, "rx_addr": rx_addr, "payload": payload}
|
||||
if protocol is not None:
|
||||
kwargs["protocol"] = protocol
|
||||
return self.call("raw_request", **kwargs)
|
||||
|
||||
def debug_at_command(self, command: str) -> dict[str, Any]:
|
||||
return self.call("debug_at_command", command_str=command)
|
||||
|
||||
def profile_status(self) -> dict[str, Any]:
|
||||
return self.call("profile_status")
|
||||
|
||||
def install_profile(self, provider: str, repository: str) -> dict[str, Any]:
|
||||
return self.call("install_profile", provider=provider, repository=repository)
|
||||
|
||||
def update_profile(self) -> dict[str, Any]:
|
||||
return self.call("update_profile")
|
||||
|
||||
def select_profile(self, profile_id: str, model_year: int | None = None) -> dict[str, Any]:
|
||||
kwargs: dict[str, Any] = {"profile_id": profile_id}
|
||||
if model_year is not None:
|
||||
kwargs["model_year"] = model_year
|
||||
return self.call("select_profile", **kwargs)
|
||||
|
||||
def remove_profile(self, profile_id: str) -> dict[str, Any]:
|
||||
return self.call("remove_profile", profile_id=profile_id)
|
||||
@@ -0,0 +1 @@
|
||||
# OBDyssey test suite
|
||||
@@ -0,0 +1,241 @@
|
||||
import threading
|
||||
import time
|
||||
import pytest
|
||||
|
||||
from openpilot.starpilot.system.bluetooth.protocol import BluetoothDevice, BluetoothStatus
|
||||
from openpilot.starpilot.system.bluetooth.tests.test_bluetooth import FakeParams
|
||||
from openpilot.starpilot.system.obdyssey.daemon import OBDysseyController, OBDysseyServer
|
||||
from openpilot.starpilot.system.obdyssey.profiles import ProfileManager
|
||||
from openpilot.starpilot.system.obdyssey.protocol import OBDysseyClient
|
||||
from openpilot.starpilot.system.obdyssey.transport import FakeElmTransport
|
||||
|
||||
|
||||
class FakeBluetoothClient:
|
||||
def __init__(self, devices: list[BluetoothDevice] | None = None, enabled: bool = True):
|
||||
self._enabled = enabled
|
||||
self._devices = devices or [
|
||||
BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDLink MX+", paired=True, trusted=True, serial=True),
|
||||
]
|
||||
|
||||
def status(self) -> BluetoothStatus:
|
||||
return BluetoothStatus(
|
||||
available=True,
|
||||
enabled=self._enabled,
|
||||
powered=self._enabled,
|
||||
devices=tuple(self._devices),
|
||||
)
|
||||
|
||||
|
||||
class BlockingTransport(FakeElmTransport):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.connect_started = threading.Event()
|
||||
self.allow_connect = threading.Event()
|
||||
|
||||
def connect(self) -> None:
|
||||
self.connect_started.set()
|
||||
self.allow_connect.wait(timeout=2.0)
|
||||
super().connect()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def obdyssey_service(tmp_path):
|
||||
socket_path = str(tmp_path / "obdyssey.sock")
|
||||
params = FakeParams(BluetoothEnabled=True, IsOffroad=True)
|
||||
prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params)
|
||||
bt_client = FakeBluetoothClient()
|
||||
|
||||
# Create fake transport with realistic mock responses
|
||||
fake_transport = FakeElmTransport(default_responses={
|
||||
"010C": "41 0C 1F 40\r\n>", # 2000 rpm
|
||||
"010D": "41 0D 41\r\n>", # 65 km/h
|
||||
"0902": "49 02 01 31 47 31 46 58 36 53 30 35 48 34 31 30 30 30 30 30\r\n>",
|
||||
"03": "43 01 33 03 00\r\n>",
|
||||
"04": "44\r\n>",
|
||||
"22F190": "62 F1 90 31 47 31 46\r\n>",
|
||||
"ATRV": "13.9V\r\n>",
|
||||
})
|
||||
|
||||
controller = OBDysseyController(
|
||||
params=params,
|
||||
transport_factory=lambda _addr: fake_transport,
|
||||
profile_manager=prof_mgr,
|
||||
bluetooth_client=bt_client,
|
||||
sleep=lambda _delay: None,
|
||||
)
|
||||
|
||||
# Start Unix socket server in background
|
||||
server = OBDysseyServer(socket_path, controller)
|
||||
server_thread = threading.Thread(target=server.serve_forever, daemon=True)
|
||||
server_thread.start()
|
||||
|
||||
client = OBDysseyClient(socket_path=socket_path)
|
||||
|
||||
# Wait for server socket to be active
|
||||
deadline = time.monotonic() + 1.0
|
||||
while time.monotonic() < deadline:
|
||||
try:
|
||||
client.status()
|
||||
break
|
||||
except Exception:
|
||||
time.sleep(0.02)
|
||||
|
||||
yield client, controller, params, fake_transport
|
||||
|
||||
server.shutdown()
|
||||
server.server_close()
|
||||
|
||||
|
||||
def test_daemon_status_and_auto_connect(obdyssey_service):
|
||||
client, controller, params, transport = obdyssey_service
|
||||
|
||||
# First connect
|
||||
client.connect()
|
||||
status = client.status()
|
||||
|
||||
assert status.connected is True
|
||||
assert status.link_connected is True
|
||||
assert status.diagnostic_ready is True
|
||||
assert status.state == "ready"
|
||||
assert status.adapter_address == "AA:BB:CC:DD:EE:FF"
|
||||
assert status.adapter_name == "OBDLink MX+"
|
||||
assert status.adapter_voltage == 13.9
|
||||
assert status.elm_identity == "ELM327 v1.5"
|
||||
|
||||
|
||||
def test_daemon_read_signals_batch(obdyssey_service):
|
||||
client, controller, params, transport = obdyssey_service
|
||||
client.connect()
|
||||
|
||||
# Read batch of standard signals
|
||||
res = client.read_signals(["SAE_ENGINE_RPM", "SAE_VEHICLE_SPEED"])
|
||||
|
||||
assert res["SAE_ENGINE_RPM"] == 2000
|
||||
assert res["SAE_VEHICLE_SPEED"] == 65
|
||||
|
||||
|
||||
def test_daemon_read_dtcs_and_vin(obdyssey_service):
|
||||
client, controller, params, transport = obdyssey_service
|
||||
client.connect()
|
||||
|
||||
# Vehicle info / VIN
|
||||
vin_res = client.vehicle_info()
|
||||
assert vin_res["vin"].startswith("1G1FX6")
|
||||
|
||||
# Read DTCs
|
||||
dtcs = client.read_dtcs()
|
||||
codes = [d["code"] for d in dtcs]
|
||||
assert "P0133" in codes
|
||||
assert "P0300" in codes
|
||||
|
||||
|
||||
def test_daemon_offroad_safety_enforcement(obdyssey_service):
|
||||
client, controller, params, transport = obdyssey_service
|
||||
client.connect()
|
||||
|
||||
# When ONROAD: mutating operations must be rejected!
|
||||
params.values["IsOffroad"] = False
|
||||
|
||||
with pytest.raises(RuntimeError, match="offroad"):
|
||||
client.clear_dtcs()
|
||||
|
||||
with pytest.raises(RuntimeError, match="offroad"):
|
||||
client.debug_at_command("ATZ")
|
||||
|
||||
with pytest.raises(RuntimeError, match="offroad"):
|
||||
client.uds_request("7E0", "7E8", "2EF19001") # Write DID is mutating
|
||||
|
||||
# Read operations are still allowed while onroad!
|
||||
rpm_res = client.read_signal("SAE_ENGINE_RPM")
|
||||
assert rpm_res["value"] == 2000
|
||||
|
||||
# When OFFROAD: mutating operations succeed
|
||||
params.values["IsOffroad"] = True
|
||||
clear_res = client.clear_dtcs()
|
||||
assert clear_res["ok"] is True
|
||||
|
||||
at_res = client.debug_at_command("ATI")
|
||||
assert at_res["ok"] is True
|
||||
|
||||
|
||||
def test_daemon_uds_and_raw_requests(obdyssey_service):
|
||||
client, controller, params, transport = obdyssey_service
|
||||
client.connect()
|
||||
|
||||
uds_res = client.uds_request("7E0", "7E8", "22F190")
|
||||
assert uds_res["ok"] is True
|
||||
assert uds_res["response"] == "62F19031473146"
|
||||
|
||||
|
||||
def test_daemon_list_and_select_profile(obdyssey_service):
|
||||
client, controller, params, transport = obdyssey_service
|
||||
|
||||
prof_status = client.profile_status()
|
||||
assert prof_status["ok"] is True
|
||||
|
||||
select_res = client.select_profile("Chevrolet-Bolt-EV")
|
||||
assert select_res["ok"] is True
|
||||
assert select_res["active_profile"] == "Chevrolet-Bolt-EV"
|
||||
|
||||
|
||||
def test_daemon_status_remains_observable_during_slow_connection(tmp_path):
|
||||
params = FakeParams(BluetoothEnabled=True, IsOffroad=True)
|
||||
prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params)
|
||||
bt_client = FakeBluetoothClient()
|
||||
transport = BlockingTransport()
|
||||
controller = OBDysseyController(
|
||||
params=params,
|
||||
transport_factory=lambda _addr: transport,
|
||||
profile_manager=prof_mgr,
|
||||
bluetooth_client=bt_client,
|
||||
)
|
||||
|
||||
worker = threading.Thread(target=lambda: controller.connect_adapter("AA:BB:CC:DD:EE:FF"))
|
||||
worker.start()
|
||||
assert transport.connect_started.wait(timeout=1.0)
|
||||
|
||||
status = controller.status()
|
||||
assert status["state"] == "connecting"
|
||||
assert status["connected"] is False
|
||||
assert status["diagnostic_ready"] is False
|
||||
|
||||
transport.allow_connect.set()
|
||||
worker.join(timeout=2.0)
|
||||
assert not worker.is_alive()
|
||||
assert controller.status()["state"] == "ready"
|
||||
|
||||
|
||||
def test_profile_session_control_is_rejected_onroad(tmp_path):
|
||||
params = FakeParams(BluetoothEnabled=True, IsOffroad=True)
|
||||
class FixedProfileManager(ProfileManager):
|
||||
def resolve_active_profile(self, explicit_id=None):
|
||||
return self.get_profile("session_profile") or super().resolve_active_profile(explicit_id)
|
||||
|
||||
prof_mgr = FixedProfileManager(data_dir=tmp_path / "profiles", params=params)
|
||||
prof_mgr.install_profile_data("session_profile", {
|
||||
"metadata": {"name": "Session Profile"},
|
||||
"commands": [{
|
||||
"id": "SESSION_SIGNAL",
|
||||
"service": 34,
|
||||
"pid": "1234",
|
||||
"din": 3,
|
||||
"dout": 1,
|
||||
"signals": [{"id": "SESSION_VALUE", "name": "Session value"}],
|
||||
}],
|
||||
})
|
||||
transport = FakeElmTransport()
|
||||
controller = OBDysseyController(
|
||||
params=params,
|
||||
transport_factory=lambda _addr: transport,
|
||||
profile_manager=prof_mgr,
|
||||
bluetooth_client=FakeBluetoothClient(),
|
||||
)
|
||||
|
||||
controller.connect_adapter("AA:BB:CC:DD:EE:FF")
|
||||
writes_before = len(transport.writes)
|
||||
params.values["IsOffroad"] = False
|
||||
|
||||
with pytest.raises(RuntimeError, match="offroad"):
|
||||
controller.handle({"command": "read_signal", "id": "SESSION_VALUE"})
|
||||
|
||||
assert len(transport.writes) == writes_before
|
||||
@@ -0,0 +1,180 @@
|
||||
import pytest
|
||||
from openpilot.starpilot.system.obdyssey.diagnostics import (
|
||||
SERVICE_TYPE,
|
||||
SESSION_TYPE,
|
||||
clear_dtcs,
|
||||
is_mutating_service,
|
||||
is_read_only_payload,
|
||||
is_read_only_service,
|
||||
parse_standard_dtcs,
|
||||
parse_uds_dtcs,
|
||||
read_current_data,
|
||||
read_freeze_frame,
|
||||
read_pending_dtcs,
|
||||
read_permanent_dtcs,
|
||||
read_stored_dtcs,
|
||||
read_vin,
|
||||
uds_clear_diagnostic_information,
|
||||
uds_diagnostic_session_control,
|
||||
uds_ecu_reset,
|
||||
uds_input_output_control,
|
||||
uds_read_data_by_identifier,
|
||||
uds_read_dtc_information,
|
||||
uds_routine_control,
|
||||
uds_security_access,
|
||||
uds_tester_present,
|
||||
uds_write_data_by_identifier,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.elm327 import Elm327
|
||||
from openpilot.starpilot.system.obdyssey.transport import FakeElmTransport
|
||||
|
||||
|
||||
def test_standard_dtc_parsing():
|
||||
# Raw bytes: 43 (Mode 03 resp) + 01 33 (P0133) + 40 35 (C0035) + 80 01 (B0001) + C1 00 (U0100) + 00 00 (padding)
|
||||
raw = bytes([0x43, 0x01, 0x33, 0x40, 0x35, 0x80, 0x01, 0xC1, 0x00, 0x00, 0x00])
|
||||
dtcs = parse_standard_dtcs(raw, source="OBD_STORED")
|
||||
|
||||
codes = [d.code for d in dtcs]
|
||||
assert codes == ["P0133", "C0035", "B0001", "U0100"]
|
||||
assert dtcs[0].source == "OBD_STORED"
|
||||
assert dtcs[0].raw == bytes([0x01, 0x33])
|
||||
|
||||
|
||||
def test_uds_dtc_parsing():
|
||||
# Positive response 0x59 02 FF followed by 3-byte records: 01 33 24, C1 00 2F
|
||||
raw = bytes([0x59, 0x02, 0xFF, 0x01, 0x33, 0x24, 0xC1, 0x00, 0x2F])
|
||||
dtcs = parse_uds_dtcs(raw, ecu="7E0")
|
||||
|
||||
assert len(dtcs) == 2
|
||||
assert dtcs[0].code == "P0133"
|
||||
assert dtcs[0].status == 0x24
|
||||
assert dtcs[0].ecu == "7E0"
|
||||
assert dtcs[1].code == "U0100"
|
||||
assert dtcs[1].status == 0x2F
|
||||
|
||||
|
||||
def test_standard_obd_services():
|
||||
transport = FakeElmTransport(default_responses={
|
||||
"010C": "41 0C 1F 40\r\n>",
|
||||
"020C00": "42 0C 00 1F 40\r\n>",
|
||||
"03": "43 01 33 03 00\r\n>",
|
||||
"07": "47 01 71\r\n>",
|
||||
"0A": "4A 04 20\r\n>",
|
||||
"04": "44\r\n>",
|
||||
"0902": "49 02 01 31 47 31 46 58 36 53 30 35 48 34 31 30 30 30 30 30\r\n>",
|
||||
})
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
|
||||
# Mode 01
|
||||
res_01 = read_current_data(elm, 0x0C)
|
||||
assert res_01.payload == bytes([0x41, 0x0C, 0x1F, 0x40])
|
||||
|
||||
# Mode 02
|
||||
res_02 = read_freeze_frame(elm, 0x0C, 0)
|
||||
assert res_02.payload == bytes([0x42, 0x0C, 0x00, 0x1F, 0x40])
|
||||
|
||||
# Mode 03 / 07 / 0A
|
||||
dtcs_stored = read_stored_dtcs(elm)
|
||||
assert [d.code for d in dtcs_stored] == ["P0133", "P0300"]
|
||||
|
||||
dtcs_pending = read_pending_dtcs(elm)
|
||||
assert [d.code for d in dtcs_pending] == ["P0171"]
|
||||
|
||||
dtcs_perm = read_permanent_dtcs(elm)
|
||||
assert [d.code for d in dtcs_perm] == ["P0420"]
|
||||
|
||||
# Mode 04
|
||||
res_04 = clear_dtcs(elm)
|
||||
assert res_04.payload == bytes([0x44])
|
||||
|
||||
# Mode 09 VIN
|
||||
vin = read_vin(elm)
|
||||
assert len(vin) == 17
|
||||
assert vin.startswith("1G1FX6")
|
||||
|
||||
|
||||
def test_uds_services():
|
||||
transport = FakeElmTransport(default_responses={
|
||||
"22F190": "62 F1 90 31 47 31 46 58 36\r\n>",
|
||||
"2EF19001": "6E F1 90\r\n>",
|
||||
"1003": "50 03 00 32 01 F4\r\n>",
|
||||
"1101": "51 01\r\n>",
|
||||
"1902FF": "59 02 FF 01 33 24\r\n>",
|
||||
"14FFFFFF": "54\r\n>",
|
||||
"2701": "67 01 11 22 33 44\r\n>",
|
||||
"3101FF00": "71 01 FF 00\r\n>",
|
||||
"2FF19000": "6F F1 90 00\r\n>",
|
||||
"3E00": "7E 00\r\n>",
|
||||
})
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
|
||||
# Read DID
|
||||
did_data = uds_read_data_by_identifier(elm, 0xF190)
|
||||
assert did_data == bytes.fromhex("314731465836")
|
||||
|
||||
# Write DID
|
||||
write_res = uds_write_data_by_identifier(elm, 0xF190, bytes([0x01]))
|
||||
assert write_res == bytes([0x6E, 0xF1, 0x90])
|
||||
|
||||
# Session Control
|
||||
sess_res = uds_diagnostic_session_control(elm, SESSION_TYPE.EXTENDED_DIAGNOSTIC)
|
||||
assert sess_res.startswith(bytes([0x50, 0x03]))
|
||||
|
||||
# ECU Reset
|
||||
reset_res = uds_ecu_reset(elm, 1)
|
||||
assert reset_res == bytes([0x51, 0x01])
|
||||
|
||||
# Read DTC Info
|
||||
dtcs = uds_read_dtc_information(elm, ecu="7E0")
|
||||
assert len(dtcs) == 1 and dtcs[0].code == "P0133"
|
||||
|
||||
# Clear Diagnostic Info
|
||||
clear_res = uds_clear_diagnostic_information(elm, 0xFFFFFF)
|
||||
assert clear_res == bytes([0x54])
|
||||
|
||||
# Security Access
|
||||
sec_res = uds_security_access(elm, 1)
|
||||
assert sec_res.startswith(bytes([0x67, 0x01]))
|
||||
|
||||
# Routine Control
|
||||
rc_res = uds_routine_control(elm, 1, 0xFF00)
|
||||
assert rc_res == bytes([0x71, 0x01, 0xFF, 0x00])
|
||||
|
||||
# IO Control
|
||||
io_res = uds_input_output_control(elm, 0xF190, 0)
|
||||
assert io_res == bytes([0x6F, 0xF1, 0x90, 0x00])
|
||||
|
||||
# Tester Present
|
||||
tp_res = uds_tester_present(elm, 0x00)
|
||||
assert tp_res == bytes([0x7E, 0x00])
|
||||
|
||||
|
||||
def test_safety_classification():
|
||||
# Read-only
|
||||
assert is_read_only_service(0x01)
|
||||
assert is_read_only_service(0x02)
|
||||
assert is_read_only_service(0x03)
|
||||
assert is_read_only_service(0x07)
|
||||
assert is_read_only_service(0x09)
|
||||
assert is_read_only_service(0x0A)
|
||||
assert is_read_only_service(SERVICE_TYPE.READ_DATA_BY_IDENTIFIER)
|
||||
assert is_read_only_service(SERVICE_TYPE.READ_DTC_INFORMATION)
|
||||
assert is_read_only_service(SERVICE_TYPE.TESTER_PRESENT)
|
||||
|
||||
# Mutating
|
||||
assert is_mutating_service(0x04)
|
||||
assert is_mutating_service(SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER)
|
||||
assert is_mutating_service(SERVICE_TYPE.CLEAR_DIAGNOSTIC_INFORMATION)
|
||||
assert is_mutating_service(SERVICE_TYPE.ECU_RESET)
|
||||
assert is_mutating_service(SERVICE_TYPE.ROUTINE_CONTROL)
|
||||
assert is_mutating_service(SERVICE_TYPE.INPUT_OUTPUT_CONTROL_BY_IDENTIFIER)
|
||||
|
||||
# Payload helper
|
||||
assert is_read_only_payload(bytes([0x01, 0x0C]))
|
||||
assert is_read_only_payload(bytes([0x22, 0xF1, 0x90]))
|
||||
assert is_read_only_payload(bytes([0x27, 0x01])) # Seed request is read-only
|
||||
assert not is_read_only_payload(bytes([0x04]))
|
||||
assert not is_read_only_payload(bytes([0x2E, 0xF1, 0x90, 0x00]))
|
||||
assert not is_read_only_payload(bytes([0x27, 0x02, 0x11, 0x22])) # Send key is mutating
|
||||
@@ -0,0 +1,160 @@
|
||||
import pytest
|
||||
from openpilot.starpilot.system.obdyssey.elm327 import (
|
||||
Elm327,
|
||||
ElmBusError,
|
||||
ElmCommandError,
|
||||
ElmContext,
|
||||
ElmDisconnectedError,
|
||||
ElmNoDataError,
|
||||
ElmTimeoutError,
|
||||
)
|
||||
from openpilot.starpilot.system.obdyssey.diagnostics import UdsNegativeResponseError
|
||||
from openpilot.starpilot.system.obdyssey.transport import FakeElmTransport
|
||||
|
||||
|
||||
def test_elm327_initialize():
|
||||
transport = FakeElmTransport()
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
|
||||
info = elm.initialize()
|
||||
assert info.identity == "ELM327 v1.5"
|
||||
assert info.voltage == 13.8
|
||||
assert elm.active_context is None
|
||||
|
||||
# Check that base init commands were sent
|
||||
writes_str = [w.decode("ascii").strip() for w in transport.writes]
|
||||
assert "ATZ" in writes_str
|
||||
assert "ATE0" in writes_str
|
||||
assert "ATL0" in writes_str
|
||||
assert "ATS0" in writes_str
|
||||
assert "ATR1" in writes_str
|
||||
assert "ATI" in writes_str
|
||||
assert "ATRV" in writes_str
|
||||
|
||||
|
||||
def test_elm327_command_strips_echo_and_noise():
|
||||
responses = {
|
||||
"0100": "0100\r\nSEARCHING...\r\n41 00 BE 3E B8 11\r\n>",
|
||||
}
|
||||
transport = FakeElmTransport(default_responses=responses)
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
|
||||
lines = elm.command("0100")
|
||||
assert lines == ["41 00 BE 3E B8 11"]
|
||||
|
||||
|
||||
def test_elm327_error_detection():
|
||||
transport = FakeElmTransport(default_responses={
|
||||
"0199": "NO DATA\r\n>",
|
||||
"ATINVALID": "?\r\n>",
|
||||
"0101": "CAN ERROR\r\n>",
|
||||
"0102": "BUS ERROR\r\n>",
|
||||
"0103": "UNABLE TO CONNECT\r\n>",
|
||||
})
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
|
||||
with pytest.raises(ElmNoDataError):
|
||||
elm.command("0199")
|
||||
|
||||
with pytest.raises(ElmCommandError):
|
||||
elm.command("ATINVALID")
|
||||
|
||||
with pytest.raises(ElmBusError):
|
||||
elm.command("0101")
|
||||
|
||||
with pytest.raises(ElmBusError):
|
||||
elm.command("0102")
|
||||
|
||||
with pytest.raises(ElmBusError):
|
||||
elm.command("0103")
|
||||
|
||||
|
||||
def test_elm327_timeout():
|
||||
transport = FakeElmTransport(default_responses={"SLOW": "NO_PROMPT_HERE\r\n"})
|
||||
transport.connect()
|
||||
elm = Elm327(transport, default_timeout=0.1)
|
||||
|
||||
with pytest.raises(ElmTimeoutError):
|
||||
elm.command("SLOW")
|
||||
|
||||
|
||||
def test_elm327_context_diffing():
|
||||
transport = FakeElmTransport()
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
elm.initialize()
|
||||
|
||||
# Clear recorded writes from initialize
|
||||
transport.writes.clear()
|
||||
|
||||
# Apply Context 1: 11-bit header 7E4, filter 7EC, protocol 6
|
||||
ctx1 = ElmContext(protocol="6", tx_header=0x7E4, rx_filter=0x7EC)
|
||||
elm.apply_context(ctx1)
|
||||
|
||||
writes1 = [w.decode("ascii").strip() for w in transport.writes]
|
||||
assert "ATSP6" in writes1
|
||||
assert "ATSH7E4" in writes1
|
||||
assert "ATCRA7EC" in writes1
|
||||
|
||||
# Apply same Context 1 again -> Should send ZERO AT commands
|
||||
transport.writes.clear()
|
||||
elm.apply_context(ctx1)
|
||||
assert len(transport.writes) == 0
|
||||
|
||||
# Apply Context 2: only header changes to 7E0, filter to 7E8
|
||||
ctx2 = ElmContext(protocol="6", tx_header=0x7E0, rx_filter=0x7E8)
|
||||
elm.apply_context(ctx2)
|
||||
|
||||
writes2 = [w.decode("ascii").strip() for w in transport.writes]
|
||||
assert "ATSP6" not in writes2 # Protocol unchanged
|
||||
assert "ATSH7E0" in writes2
|
||||
assert "ATCRA7E8" in writes2
|
||||
|
||||
|
||||
def test_elm327_29bit_priority_header():
|
||||
transport = FakeElmTransport()
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
|
||||
ctx_29bit = ElmContext(tx_header=0x18DB33F1, rx_filter=0x18DAF133)
|
||||
elm.apply_context(ctx_29bit)
|
||||
|
||||
writes = [w.decode("ascii").strip() for w in transport.writes]
|
||||
assert "ATSH18DB33F1" in writes
|
||||
assert "ATCRA18DAF133" in writes
|
||||
|
||||
# Priority override
|
||||
transport.writes.clear()
|
||||
ctx_prio = ElmContext(tx_header=0x00DB33F1, priority=0x1D)
|
||||
elm.apply_context(ctx_prio)
|
||||
writes_prio = [w.decode("ascii").strip() for w in transport.writes]
|
||||
assert "ATSH1DDB33F1" in writes_prio
|
||||
|
||||
|
||||
def test_elm327_diagnostic_request_parsing():
|
||||
transport = FakeElmTransport(default_responses={
|
||||
"010C": "41 0C 1F 40\r\n>",
|
||||
"228334": "62 83 34 00 11 22 33\r\n>",
|
||||
"22F190": "7F 22 31\r\n>", # UDS Negative response: Request Out of Range
|
||||
})
|
||||
transport.connect()
|
||||
elm = Elm327(transport)
|
||||
|
||||
# Mode 01 PID 0C
|
||||
res1 = elm.request(bytes([0x01, 0x0C]))
|
||||
assert res1.payload == bytes([0x41, 0x0C, 0x1F, 0x40])
|
||||
assert res1.service == 0x41
|
||||
|
||||
# Mode 22 DID 8334
|
||||
res2 = elm.request(bytes([0x22, 0x83, 0x34]))
|
||||
assert res2.payload == bytes([0x62, 0x83, 0x34, 0x00, 0x11, 0x22, 0x33])
|
||||
assert res2.service == 0x62
|
||||
|
||||
# Negative response throws UdsNegativeResponseError
|
||||
with pytest.raises(UdsNegativeResponseError) as exc_info:
|
||||
elm.request(bytes([0x22, 0xF1, 0x90]))
|
||||
assert exc_info.value.service_id == 0x22
|
||||
assert exc_info.value.nrc == 0x31
|
||||
@@ -0,0 +1,148 @@
|
||||
import pytest
|
||||
from openpilot.starpilot.system.obdyssey.obdb import (
|
||||
SignalDefinition,
|
||||
SignalFormat,
|
||||
calculate_synthetic_signal,
|
||||
decode_signal,
|
||||
extract_raw_value,
|
||||
parse_obdb_profile,
|
||||
)
|
||||
|
||||
|
||||
def test_extract_raw_value_bits():
|
||||
# Byte 0xAB, 0xCD -> Binary: 10101011 11001101
|
||||
data = bytes([0xAB, 0xCD])
|
||||
|
||||
# Extract 8 bits at offset 0 -> 0xAB (171)
|
||||
assert extract_raw_value(data, bix=0, bit_len=8) == 0xAB
|
||||
|
||||
# Extract 8 bits at offset 8 -> 0xCD (205)
|
||||
assert extract_raw_value(data, bix=8, bit_len=8) == 0xCD
|
||||
|
||||
# Extract 16 bits at offset 0 (Big-Endian) -> 0xABCD (43981)
|
||||
assert extract_raw_value(data, bix=0, bit_len=16, blsb=False) == 0xABCD
|
||||
|
||||
# Extract 16 bits at offset 0 (Little-Endian) -> 0xCDAB (52651)
|
||||
assert extract_raw_value(data, bix=0, bit_len=16, blsb=True) == 0xCDAB
|
||||
|
||||
# Extract 4 bits at offset 4 (lower nibble of byte 0) -> 1011 -> 0x0B (11)
|
||||
assert extract_raw_value(data, bix=4, bit_len=4) == 0x0B
|
||||
|
||||
# Signed extraction: 8-bit 0xFE (-2)
|
||||
data_signed = bytes([0xFE])
|
||||
assert extract_raw_value(data_signed, bix=0, bit_len=8, sign=True) == -2
|
||||
assert extract_raw_value(data_signed, bix=0, bit_len=8, sign=False) == 254
|
||||
|
||||
|
||||
def test_decode_signal_scaling_and_bounds():
|
||||
# Engine RPM: 2 bytes at offset 0, mul=1, div=4 -> (0x1F40 = 8000) / 4 = 2000 rpm
|
||||
sig_rpm = SignalDefinition(
|
||||
id="RPM",
|
||||
name="RPM",
|
||||
format=SignalFormat(bix=0, len=16, mul=1.0, div=4.0, unit="rpm"),
|
||||
)
|
||||
payload = bytes([0x1F, 0x40])
|
||||
assert decode_signal(payload, sig_rpm) == 2000
|
||||
|
||||
# Coolant Temp: 1 byte at offset 0, add=-40 -> 0x82 (130) - 40 = 90 C
|
||||
sig_temp = SignalDefinition(
|
||||
id="TEMP",
|
||||
name="Coolant Temp",
|
||||
format=SignalFormat(bix=0, len=8, add=-40.0, unit="°C"),
|
||||
)
|
||||
assert decode_signal(bytes([0x82]), sig_temp) == 90
|
||||
|
||||
# Null range filter: null between -40 and -40
|
||||
sig_null = SignalDefinition(
|
||||
id="SENSOR",
|
||||
name="Sensor",
|
||||
format=SignalFormat(bix=0, len=8, add=-40.0, nullmin=-40.0, nullmax=-40.0),
|
||||
)
|
||||
assert decode_signal(bytes([0x00]), sig_null) is None # 0 - 40 = -40 -> None
|
||||
assert decode_signal(bytes([0x50]), sig_null) == 40
|
||||
|
||||
# Enum mapping
|
||||
sig_map = SignalDefinition(
|
||||
id="GEAR",
|
||||
name="Gear",
|
||||
format=SignalFormat(bix=0, len=8, map={"0": "P", "1": "R", "2": "N", "3": "D"}),
|
||||
)
|
||||
assert decode_signal(bytes([0x00]), sig_map) == "P"
|
||||
assert decode_signal(bytes([0x03]), sig_map) == "D"
|
||||
|
||||
|
||||
def test_synthetic_signal_calculation():
|
||||
# Power (kW) = Voltage (V) * Current (A) * 0.001
|
||||
sig_power = SignalDefinition(
|
||||
id="HVBAT_POWER",
|
||||
name="Battery Power",
|
||||
synthetic={"operation": "multiply", "signals": ["HVBAT_VOLTAGE", "HVBAT_CURRENT"]},
|
||||
format=SignalFormat(mul=0.001, unit="kW"),
|
||||
)
|
||||
|
||||
signals = {"HVBAT_VOLTAGE": 380.0, "HVBAT_CURRENT": 50.0}
|
||||
power_kw = calculate_synthetic_signal(sig_power, signals)
|
||||
assert power_kw == 19.0 # 380 * 50 * 0.001 = 19 kW
|
||||
|
||||
# Missing input returns None
|
||||
assert calculate_synthetic_signal(sig_power, {"HVBAT_VOLTAGE": 380.0}) is None
|
||||
|
||||
|
||||
def test_parse_obdb_profile_json():
|
||||
obdb_data = {
|
||||
"metadata": {
|
||||
"id": "Test-Car",
|
||||
"name": "Test Vehicle",
|
||||
"provider": "OBDb",
|
||||
"revision": "v3.0.0",
|
||||
"protocol": "ISO 15765-4 (CAN 11/500)",
|
||||
},
|
||||
"commands": [
|
||||
{
|
||||
"id": "CMD_BMS",
|
||||
"hdr": "7E4",
|
||||
"rax": "7EC",
|
||||
"service": 34,
|
||||
"pid": "8334",
|
||||
"freq": 2.0,
|
||||
"din": 3,
|
||||
"dout": 1,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SOC",
|
||||
"name": "State of Charge",
|
||||
"fmt": {"bix": 0, "len": 8, "mul": 0.5, "unit": "%"}
|
||||
},
|
||||
{
|
||||
"id": "VOLT",
|
||||
"name": "Pack Voltage",
|
||||
"fmt": {"bix": 16, "len": 16, "mul": 0.05, "unit": "V"}
|
||||
}
|
||||
]
|
||||
}
|
||||
],
|
||||
"signals": [
|
||||
{
|
||||
"id": "SYNTH_POWER",
|
||||
"name": "Total Power",
|
||||
"synthetic": {"operation": "multiply", "signals": ["VOLT", "CURR"]}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
profile = parse_obdb_profile(obdb_data, "test_car")
|
||||
assert profile.id == "test_car"
|
||||
assert len(profile.commands) == 1
|
||||
|
||||
cmd = profile.commands[0]
|
||||
assert cmd.context.tx_header == 0x7E4
|
||||
assert cmd.context.rx_filter == 0x7EC
|
||||
assert cmd.context.protocol == "ISO 15765-4 (CAN 11/500)"
|
||||
assert cmd.service == 34
|
||||
assert cmd.parameter == bytes.fromhex("8334")
|
||||
assert cmd.diagnostic_session_in == 3
|
||||
assert cmd.diagnostic_session_out == 1
|
||||
assert len(cmd.signals) == 2
|
||||
assert "SOC" in profile.signals
|
||||
assert "VOLT" in profile.signals
|
||||
assert "SYNTH_POWER" in profile.synthetic_signals
|
||||
@@ -0,0 +1,86 @@
|
||||
import pytest
|
||||
from openpilot.starpilot.system.bluetooth.tests.test_bluetooth import FakeParams
|
||||
from openpilot.starpilot.system.obdyssey.profiles import ProfileManager
|
||||
|
||||
|
||||
def test_load_bundled_saej1979(tmp_path):
|
||||
manager = ProfileManager(data_dir=tmp_path)
|
||||
profile = manager.load_bundled_saej1979()
|
||||
|
||||
assert profile.id == "saej1979"
|
||||
assert len(profile.commands) > 0
|
||||
assert "SAE_ENGINE_RPM" in profile.signals
|
||||
assert "SAE_VEHICLE_SPEED" in profile.signals
|
||||
assert "SAE_ENGINE_COOLANT_TEMP" in profile.signals
|
||||
|
||||
|
||||
def test_list_and_get_curated_profiles(tmp_path):
|
||||
manager = ProfileManager(data_dir=tmp_path)
|
||||
profiles = manager.list_profiles()
|
||||
|
||||
prof_ids = [p["id"] for p in profiles]
|
||||
assert "saej1979" in prof_ids
|
||||
assert "Chevrolet-Bolt-EV" in prof_ids
|
||||
|
||||
bolt = manager.get_profile("Chevrolet-Bolt-EV")
|
||||
assert bolt is not None
|
||||
assert "BOLT_HVBAT_SOC" in bolt.signals
|
||||
assert "BOLT_HVBAT_VOLTAGE" in bolt.signals
|
||||
assert "BOLT_HVBAT_POWER" in bolt.synthetic_signals
|
||||
|
||||
|
||||
def test_resolve_active_profile_from_params(tmp_path):
|
||||
# 1. Detected Chevy Bolt EV -> Resolves Chevrolet-Bolt-EV
|
||||
params = FakeParams(CarMake="Chevrolet", CarModel="BOLT EV")
|
||||
manager = ProfileManager(data_dir=tmp_path, params=params)
|
||||
active = manager.resolve_active_profile()
|
||||
assert active.id == "Chevrolet-Bolt-EV"
|
||||
|
||||
# 2. Unmapped car -> Falls back to SAE J1979
|
||||
params_other = FakeParams(CarMake="Subaru", CarModel="Forester")
|
||||
manager_other = ProfileManager(data_dir=tmp_path, params=params_other)
|
||||
active_other = manager_other.resolve_active_profile()
|
||||
assert active_other.id == "saej1979"
|
||||
|
||||
# 3. Explicit override
|
||||
active_explicit = manager_other.resolve_active_profile(explicit_id="Chevrolet-Bolt-EV")
|
||||
assert active_explicit.id == "Chevrolet-Bolt-EV"
|
||||
|
||||
|
||||
def test_group_signals_by_command(tmp_path):
|
||||
manager = ProfileManager(data_dir=tmp_path)
|
||||
bolt = manager.get_profile("Chevrolet-Bolt-EV")
|
||||
|
||||
# Request 3 signals that belong to the SAME BMS command (CMD_BOLT_BMS_MAIN)
|
||||
requested = ["BOLT_HVBAT_SOC", "BOLT_HVBAT_VOLTAGE", "BOLT_HVBAT_CURRENT"]
|
||||
grouped = ProfileManager.group_signals_by_command(bolt, requested)
|
||||
|
||||
# Must produce exactly ONE command group containing all 3 signals!
|
||||
assert len(grouped) == 1
|
||||
cmd, signals = grouped[0]
|
||||
assert cmd.id == "CMD_BOLT_BMS_MAIN"
|
||||
assert len(signals) == 3
|
||||
assert [s.id for s in signals] == requested
|
||||
|
||||
|
||||
def test_install_and_remove_custom_profile(tmp_path):
|
||||
manager = ProfileManager(data_dir=tmp_path)
|
||||
custom_data = {
|
||||
"metadata": {"name": "Custom Vehicle", "provider": "User"},
|
||||
"commands": [
|
||||
{
|
||||
"id": "CMD_1",
|
||||
"service": 1,
|
||||
"pid": "0C",
|
||||
"signals": [{"id": "CUSTOM_RPM", "name": "RPM"}]
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
installed = manager.install_profile_data("custom_car", custom_data)
|
||||
assert installed.id == "custom_car"
|
||||
assert manager.get_profile("custom_car") is not None
|
||||
|
||||
removed = manager.remove_profile("custom_car")
|
||||
assert removed
|
||||
assert manager.get_profile("custom_car") is None
|
||||
@@ -0,0 +1,30 @@
|
||||
from openpilot.starpilot.system.obdyssey.protocol import OBDysseyStatus
|
||||
|
||||
|
||||
def test_obdyssey_status_serialization():
|
||||
status = OBDysseyStatus(
|
||||
state="ready",
|
||||
enabled=True,
|
||||
bluetooth_enabled=True,
|
||||
adapter_address="00:11:22:33:44:55",
|
||||
adapter_name="OBDLink MX+",
|
||||
connected=True,
|
||||
elm_identity="ELM327 v1.5",
|
||||
adapter_voltage=13.9,
|
||||
protocol="ISO 15765-4 CAN 11/500",
|
||||
profile="Chevrolet-Bolt-EV",
|
||||
signal_count=10,
|
||||
last_error="",
|
||||
last_request_ms=42,
|
||||
requests=100,
|
||||
errors=1,
|
||||
reconnects=2,
|
||||
)
|
||||
|
||||
data = status.to_dict()
|
||||
assert data["state"] == "ready"
|
||||
assert data["connected"] is True
|
||||
assert data["adapter_voltage"] == 13.9
|
||||
|
||||
restored = OBDysseyStatus.from_dict(data)
|
||||
assert restored == status
|
||||
@@ -0,0 +1,101 @@
|
||||
import socket
|
||||
import threading
|
||||
|
||||
import pytest
|
||||
from openpilot.starpilot.system.obdyssey.transport import BluezSppTransport, FakeElmTransport
|
||||
|
||||
|
||||
class _FakeProfileClient:
|
||||
def __init__(self, sock: socket.socket | None = None, error: Exception | None = None):
|
||||
self.sock = sock
|
||||
self.error = error
|
||||
self.closed = False
|
||||
|
||||
def connect_profile(self, _address: str, timeout: float):
|
||||
assert timeout > 0
|
||||
if self.error is not None:
|
||||
raise self.error
|
||||
assert self.sock is not None
|
||||
return self.sock
|
||||
|
||||
def close(self):
|
||||
self.closed = True
|
||||
|
||||
|
||||
def test_bluez_transport_connect_does_not_deadlock_when_replacing_connection():
|
||||
peer, sock = socket.socketpair()
|
||||
profile = _FakeProfileClient(sock=sock)
|
||||
transport = BluezSppTransport("AA:BB:CC:DD:EE:FF", profile_factory=lambda: profile)
|
||||
|
||||
worker = threading.Thread(target=transport.connect)
|
||||
worker.start()
|
||||
worker.join(timeout=1.0)
|
||||
|
||||
assert not worker.is_alive()
|
||||
assert transport._sock is sock
|
||||
transport.close()
|
||||
peer.close()
|
||||
|
||||
|
||||
def test_bluez_transport_failed_connect_closes_profile_client_and_socket():
|
||||
profile = _FakeProfileClient(error=RuntimeError("profile failed"))
|
||||
transport = BluezSppTransport("AA:BB:CC:DD:EE:FF", profile_factory=lambda: profile)
|
||||
|
||||
with pytest.raises(RuntimeError, match="profile failed"):
|
||||
transport.connect()
|
||||
|
||||
assert profile.closed
|
||||
assert transport._profile_client is None
|
||||
assert transport._sock is None
|
||||
|
||||
|
||||
def test_fake_transport_connect_close():
|
||||
transport = FakeElmTransport()
|
||||
assert not transport.connected
|
||||
|
||||
transport.connect()
|
||||
assert transport.connected
|
||||
|
||||
transport.write(b"ATI\r")
|
||||
assert b"ATI\r" in transport.writes
|
||||
data = transport.read(4096)
|
||||
assert b"ELM327" in data
|
||||
|
||||
transport.close()
|
||||
assert not transport.connected
|
||||
with pytest.raises(ConnectionResetError):
|
||||
transport.read(4096)
|
||||
|
||||
|
||||
def test_fake_transport_disconnect_on_write():
|
||||
transport = FakeElmTransport()
|
||||
transport.connect()
|
||||
transport.disconnect_on_write = True
|
||||
|
||||
with pytest.raises(ConnectionResetError, match="disconnect on write"):
|
||||
transport.write(b"ATZ\r")
|
||||
|
||||
|
||||
def test_fake_transport_timeout_on_read():
|
||||
transport = FakeElmTransport()
|
||||
transport.connect()
|
||||
transport.timeout_on_read = True
|
||||
|
||||
with pytest.raises(TimeoutError, match="timeout"):
|
||||
transport.read(4096)
|
||||
|
||||
|
||||
def test_fake_transport_custom_handler():
|
||||
def custom_handler(data: bytes) -> bytes:
|
||||
if b"CUSTOM" in data:
|
||||
return b"CUSTOM_REPLY\r\n>"
|
||||
return b"DEFAULT\r\n>"
|
||||
|
||||
transport = FakeElmTransport(handler=custom_handler)
|
||||
transport.connect()
|
||||
|
||||
transport.write(b"CUSTOM_CMD\r")
|
||||
assert transport.read(4096) == b"CUSTOM_REPLY\r\n>"
|
||||
|
||||
transport.write(b"OTHER\r")
|
||||
assert transport.read(4096) == b"DEFAULT\r\n>"
|
||||
@@ -0,0 +1,225 @@
|
||||
import socket
|
||||
import threading
|
||||
import time
|
||||
from collections.abc import Callable
|
||||
from typing import Protocol
|
||||
|
||||
|
||||
class ElmTransport(Protocol):
|
||||
def connect(self) -> None:
|
||||
...
|
||||
|
||||
def close(self) -> None:
|
||||
...
|
||||
|
||||
def read(self, size: int = 4096) -> bytes:
|
||||
...
|
||||
|
||||
def write(self, data: bytes) -> None:
|
||||
...
|
||||
|
||||
|
||||
class BluezSppTransport:
|
||||
def __init__(self, address: str, profile_factory=None, timeout: float = 10.0):
|
||||
self.address = address
|
||||
self.timeout = timeout
|
||||
self._profile_factory = profile_factory
|
||||
self._profile_client = None
|
||||
self._sock: socket.socket | None = None
|
||||
# connect() tears down an existing profile before replacing it. Keep the
|
||||
# lock re-entrant so that close() can safely be used from that path.
|
||||
self._lock = threading.RLock()
|
||||
|
||||
def connect(self) -> None:
|
||||
with self._lock:
|
||||
self.close()
|
||||
if self._profile_factory is None:
|
||||
from openpilot.starpilot.system.bluetooth.bluez import BlueZProfileClient
|
||||
self._profile_factory = BlueZProfileClient
|
||||
|
||||
profile_client = self._profile_factory()
|
||||
self._profile_client = profile_client
|
||||
sock = None
|
||||
try:
|
||||
sock = profile_client.connect_profile(self.address, timeout=self.timeout)
|
||||
sock.settimeout(self.timeout)
|
||||
self._sock = sock
|
||||
except Exception:
|
||||
if sock is not None:
|
||||
try:
|
||||
sock.close()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
profile_client.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._profile_client = None
|
||||
self._sock = None
|
||||
raise
|
||||
|
||||
def close(self) -> None:
|
||||
with self._lock:
|
||||
if self._sock is not None:
|
||||
try:
|
||||
self._sock.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._sock = None
|
||||
if self._profile_client is not None:
|
||||
try:
|
||||
self._profile_client.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._profile_client = None
|
||||
|
||||
def read(self, size: int = 4096) -> bytes:
|
||||
with self._lock:
|
||||
sock = self._sock
|
||||
if sock is None:
|
||||
raise ConnectionResetError("Transport is not connected")
|
||||
try:
|
||||
chunk = sock.recv(size)
|
||||
if not chunk:
|
||||
raise ConnectionResetError("Connection closed by peer")
|
||||
return chunk
|
||||
except TimeoutError as err:
|
||||
raise TimeoutError("Socket read timed out") from err
|
||||
except (OSError, ConnectionResetError) as err:
|
||||
raise ConnectionResetError(f"Socket read error: {err}") from err
|
||||
|
||||
def write(self, data: bytes) -> None:
|
||||
with self._lock:
|
||||
sock = self._sock
|
||||
if sock is None:
|
||||
raise ConnectionResetError("Transport is not connected")
|
||||
try:
|
||||
sock.sendall(data)
|
||||
except TimeoutError as err:
|
||||
raise TimeoutError("Socket write timed out") from err
|
||||
except (OSError, ConnectionResetError) as err:
|
||||
raise ConnectionResetError(f"Socket write error: {err}") from err
|
||||
|
||||
|
||||
class FakeElmTransport:
|
||||
"""In-memory transport simulator for testing ELM327 communications without hardware."""
|
||||
|
||||
def __init__(self, handler: Callable[[bytes], bytes | list[bytes]] | None = None, default_responses: dict[str, str] | None = None):
|
||||
self.handler = handler
|
||||
self.default_responses: dict[str, str] = {
|
||||
"ATZ": "ELM327 v1.5\r\n>",
|
||||
"ATE0": "OK\r\n>",
|
||||
"ATL0": "OK\r\n>",
|
||||
"ATS0": "OK\r\n>",
|
||||
"ATR1": "OK\r\n>",
|
||||
"ATI": "ELM327 v1.5\r\n>",
|
||||
"ATRV": "13.8V\r\n>",
|
||||
"ATDP": "ISO 15765-4 (CAN 11/500)\r\n>",
|
||||
"ATDPN": "6\r\n>",
|
||||
"ATSP0": "OK\r\n>",
|
||||
"ATSP6": "OK\r\n>",
|
||||
"ATSP7": "OK\r\n>",
|
||||
"ATSH": "OK\r\n>",
|
||||
"ATCRA": "OK\r\n>",
|
||||
"ATCAF1": "OK\r\n>",
|
||||
"ATCAF0": "OK\r\n>",
|
||||
"ATST": "OK\r\n>",
|
||||
"ATCEA": "OK\r\n>",
|
||||
}
|
||||
if default_responses:
|
||||
self.default_responses.update(default_responses)
|
||||
|
||||
self.connected = False
|
||||
self.writes: list[bytes] = []
|
||||
self._read_buffer = bytearray()
|
||||
self._lock = threading.Lock()
|
||||
self._write_event = threading.Event()
|
||||
self.disconnect_on_write = False
|
||||
self.timeout_on_read = False
|
||||
|
||||
def connect(self) -> None:
|
||||
with self._lock:
|
||||
self.connected = True
|
||||
|
||||
def close(self) -> None:
|
||||
with self._lock:
|
||||
self.connected = False
|
||||
self._read_buffer.clear()
|
||||
|
||||
def read(self, size: int = 4096) -> bytes:
|
||||
with self._lock:
|
||||
if not self.connected:
|
||||
raise ConnectionResetError("Transport is not connected")
|
||||
if self.timeout_on_read:
|
||||
raise TimeoutError("Simulated read timeout")
|
||||
if not self._read_buffer:
|
||||
# If nothing buffered, simulate timeout or wait
|
||||
time.sleep(0.005)
|
||||
if not self._read_buffer:
|
||||
raise TimeoutError("Read buffer is empty")
|
||||
chunk = bytes(self._read_buffer[:size])
|
||||
del self._read_buffer[:size]
|
||||
return chunk
|
||||
|
||||
def write(self, data: bytes) -> None:
|
||||
with self._lock:
|
||||
if not self.connected:
|
||||
raise ConnectionResetError("Transport is not connected")
|
||||
if self.disconnect_on_write:
|
||||
self.connected = False
|
||||
raise ConnectionResetError("Simulated disconnect on write")
|
||||
self.writes.append(data)
|
||||
|
||||
# Generate response
|
||||
response_bytes: bytes = b""
|
||||
if self.handler is not None:
|
||||
res = self.handler(data)
|
||||
if isinstance(res, list):
|
||||
response_bytes = b"".join(res)
|
||||
elif isinstance(res, bytes):
|
||||
response_bytes = res
|
||||
else:
|
||||
cmd_str = data.decode("ascii", errors="ignore").strip().upper()
|
||||
matched = False
|
||||
# 1. Exact match first
|
||||
for key, reply in self.default_responses.items():
|
||||
if cmd_str == key.upper():
|
||||
response_bytes = reply.encode("ascii")
|
||||
matched = True
|
||||
break
|
||||
# 2. Prefix match for AT commands (e.g. ATSH 7E4 -> ATSH match)
|
||||
if not matched:
|
||||
for key, reply in self.default_responses.items():
|
||||
if key.startswith("AT") and len(key) > 2 and cmd_str.startswith(key.upper()):
|
||||
response_bytes = reply.encode("ascii")
|
||||
matched = True
|
||||
break
|
||||
if not matched:
|
||||
# Default OBD mock reply
|
||||
if cmd_str.startswith("010C"): # RPM: 2000 rpm ((0x1F * 256 + 0x40) / 4)
|
||||
response_bytes = b"41 0C 1F 40\r\n>"
|
||||
elif cmd_str.startswith("010D"): # Speed: 65 km/h (0x41)
|
||||
response_bytes = b"41 0D 41\r\n>"
|
||||
elif cmd_str.startswith("0105"): # Coolant: 90 C (0x82 - 40 = 90)
|
||||
response_bytes = b"41 05 82\r\n>"
|
||||
elif cmd_str.startswith("0902"): # VIN: 1G1FX6S05H4100000
|
||||
response_bytes = b"49 02 01 31 47 31 46\r\n49 02 02 58 36 53 30 35\r\n49 02 03 48 34 31 30 30 30 30 30\r\n>"
|
||||
elif cmd_str.startswith("03"): # Stored DTCs: P0133, P0300
|
||||
response_bytes = b"43 01 33 03 00 00 00\r\n>"
|
||||
elif cmd_str.startswith("07"): # Pending DTCs: None
|
||||
response_bytes = b"47 00 00 00 00 00 00\r\n>"
|
||||
elif cmd_str.startswith("0A"): # Permanent DTCs: None
|
||||
response_bytes = b"4A 00 00 00 00 00 00\r\n>"
|
||||
elif cmd_str.startswith("04"): # Clear DTCs: OK
|
||||
response_bytes = b"44\r\n>"
|
||||
elif cmd_str.startswith("22"): # UDS ReadDID
|
||||
response_bytes = b"62 " + cmd_str[2:].encode("ascii") + b" 00 11 22 33\r\n>"
|
||||
else:
|
||||
response_bytes = b"NO DATA\r\n>"
|
||||
|
||||
self._read_buffer.extend(response_bytes)
|
||||
self._write_event.set()
|
||||
|
||||
def queue_response(self, data: bytes) -> None:
|
||||
with self._lock:
|
||||
self._read_buffer.extend(data)
|
||||
@@ -202,6 +202,7 @@ procs = [
|
||||
# StarPilot variables
|
||||
procs += [
|
||||
PythonProcess("bluetooth_managerd", "starpilot.system.bluetooth.daemon", bluetooth_enabled, enabled=TICI),
|
||||
PythonProcess("obdysseyd", "starpilot.system.obdyssey.daemon", always_run, enabled=TICI),
|
||||
PythonProcess("wheel_controlsd", "starpilot.system.wheel_controls.wheel_controlsd", wheel_controls_enabled, enabled=TICI, nice=19),
|
||||
PythonProcess("the_galaxy", "starpilot.system.the_galaxy.the_galaxy", always_run, nice=10),
|
||||
PythonProcess("galaxy", "starpilot.system.galaxy.galaxy", always_run, nice=10),
|
||||
|
||||
@@ -5,7 +5,7 @@ from functools import partial
|
||||
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.starpilot.system.bluetooth.protocol import BluetoothDevice, BluetoothStatus
|
||||
from openpilot.starpilot.system.bluetooth.protocol import BluetoothDevice, BluetoothStatus, looks_like_obd_device
|
||||
from openpilot.system.ui.lib.application import FontWeight, MousePos, gui_app
|
||||
from openpilot.system.ui.lib.bluetooth_manager import BluetoothManager
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
@@ -23,6 +23,7 @@ HEADER_HEIGHT = 180
|
||||
ITEM_HEIGHT = 160
|
||||
HEADER_PADDING = 40
|
||||
SCAN_BUTTON_WIDTH = 260
|
||||
OBDYSSEY_BUTTON_WIDTH = 260
|
||||
FORGET_BUTTON_WIDTH = 180
|
||||
ACTION_GAP = 35
|
||||
|
||||
@@ -35,6 +36,10 @@ TEXT_DISABLED = rl.Color(150, 150, 150, 255)
|
||||
TEXT_CONNECTED = rl.Color(113, 209, 135, 255)
|
||||
|
||||
|
||||
def is_obd_device(device: BluetoothDevice) -> bool:
|
||||
return (device.serial or looks_like_obd_device(device.name)) and not (device.audio or device.controller)
|
||||
|
||||
|
||||
def device_status_text(device: BluetoothDevice, operation: str, selected_audio: str) -> str:
|
||||
"""Return the concise, state-first label shown below a Bluetooth device name."""
|
||||
if operation:
|
||||
@@ -45,11 +50,15 @@ def device_status_text(device: BluetoothDevice, operation: str, selected_audio:
|
||||
capabilities.append(tr("audio output") if selected_audio.upper() == device.address.upper() else tr("audio"))
|
||||
if device.controller:
|
||||
capabilities.append(tr("controller"))
|
||||
if device.serial or looks_like_obd_device(device.name):
|
||||
capabilities.append(tr("OBD-II"))
|
||||
capability_text = " / ".join(capabilities)
|
||||
|
||||
if device.connected:
|
||||
return tr("Connected") + (f" / {capability_text}" if capability_text else "")
|
||||
if device.paired:
|
||||
if is_obd_device(device):
|
||||
return tr("Paired - tap to open OBDyssey")
|
||||
return tr("Paired - tap to connect")
|
||||
return tr("Tap to pair") + (f" / {capability_text}" if capability_text else "")
|
||||
|
||||
@@ -183,6 +192,7 @@ class BluetoothManagerUI(Widget):
|
||||
self._scroll_panel = GuiScrollPanel()
|
||||
self._power_toggle = Toggle(initial_state=False, callback=self._toggle_power)
|
||||
self._scan_button = Button(tr("Scan"), self._scan, button_style=ButtonStyle.NORMAL, font_size=42)
|
||||
self._obdyssey_button = Button(tr("OBDyssey"), self._open_obdyssey, button_style=ButtonStyle.PRIMARY, font_size=42)
|
||||
self._device_rows: dict[str, BluetoothDeviceRow] = {}
|
||||
self._pending_power: bool | None = None
|
||||
self._scan_pending = False
|
||||
@@ -192,6 +202,10 @@ class BluetoothManagerUI(Widget):
|
||||
self._last_operation_error = ""
|
||||
self._keyboard = Keyboard(max_text_size=32, min_text_size=1, password_mode=False)
|
||||
|
||||
def _open_obdyssey(self):
|
||||
from openpilot.system.ui.widgets.obdyssey import OBDysseyScreen
|
||||
gui_app.push_widget(OBDysseyScreen())
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._manager.set_active(True)
|
||||
@@ -264,6 +278,8 @@ class BluetoothManagerUI(Widget):
|
||||
return
|
||||
if not device.paired:
|
||||
self._manager.pair(device.address)
|
||||
elif is_obd_device(device):
|
||||
self._open_obdyssey()
|
||||
elif not device.connected:
|
||||
self._manager.connect(device.address)
|
||||
else:
|
||||
@@ -406,11 +422,17 @@ class BluetoothManagerUI(Widget):
|
||||
subtitle += " - " + tr("Limited while driving")
|
||||
gui_label(rl.Rectangle(rect.x + HEADER_PADDING, rect.y + 104, 650, 44), subtitle, font_size=40, color=TEXT_SECONDARY)
|
||||
|
||||
has_obd = any((device.connected or device.paired) and (device.serial or looks_like_obd_device(device.name)) for device in status.devices)
|
||||
|
||||
toggle_rect = rl.Rectangle(rect.x + rect.width - HEADER_PADDING - 160, rect.y + (rect.height - 80) / 2, 160, 80)
|
||||
self._power_toggle.render(toggle_rect)
|
||||
scan_rect = rl.Rectangle(toggle_rect.x - ACTION_GAP - SCAN_BUTTON_WIDTH, rect.y + (rect.height - 100) / 2, SCAN_BUTTON_WIDTH, 100)
|
||||
self._scan_button.render(scan_rect)
|
||||
|
||||
if has_obd:
|
||||
obdyssey_rect = rl.Rectangle(scan_rect.x - ACTION_GAP - OBDYSSEY_BUTTON_WIDTH, rect.y + (rect.height - 100) / 2, OBDYSSEY_BUTTON_WIDTH, 100)
|
||||
self._obdyssey_button.render(obdyssey_rect)
|
||||
|
||||
def _render_device_list(self, rect: rl.Rectangle, rows: list[BluetoothDeviceRow]):
|
||||
content_rect = rl.Rectangle(rect.x, rect.y, rect.width, len(rows) * ITEM_HEIGHT)
|
||||
offset = self._scroll_panel.update(rect, content_rect)
|
||||
|
||||
@@ -0,0 +1,419 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import pyray as rl
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.starpilot.system.obdyssey.protocol import OBDysseyClient, OBDysseyStatus
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app
|
||||
from openpilot.system.ui.lib.multilang import tr
|
||||
from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel
|
||||
from openpilot.system.ui.widgets import DialogResult, Widget
|
||||
from openpilot.system.ui.widgets.button import Button, ButtonStyle
|
||||
from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog, alert_dialog
|
||||
from openpilot.system.ui.widgets.label import gui_label
|
||||
|
||||
HEADER_HEIGHT = 180
|
||||
HEADER_PADDING = 40
|
||||
BUTTON_GAP = 25
|
||||
BUTTON_HEIGHT = 100
|
||||
|
||||
PANEL_BACKGROUND = rl.BLACK
|
||||
CARD_BACKGROUND = rl.Color(27, 27, 27, 255)
|
||||
ROW_BORDER = rl.LIGHTGRAY
|
||||
TEXT_SECONDARY = rl.Color(170, 170, 170, 255)
|
||||
TEXT_CONNECTED = rl.Color(113, 209, 135, 255)
|
||||
TEXT_DANGER = rl.Color(255, 100, 100, 255)
|
||||
TEXT_WARNING = rl.Color(255, 185, 45, 255)
|
||||
|
||||
DTC_STATE_UNAVAILABLE = "unavailable"
|
||||
DTC_STATE_IN_PROGRESS = "in_progress"
|
||||
DTC_STATE_CLEAN = "clean"
|
||||
DTC_STATE_FAULTS = "faults"
|
||||
|
||||
COMMON_SIGNAL_NAMES = {
|
||||
"SAE_ENGINE_RPM": ("Engine RPM", "rpm"),
|
||||
"SAE_VEHICLE_SPEED": ("Speed", "km/h"),
|
||||
"SAE_ENGINE_COOLANT_TEMP": ("Coolant Temp", "°C"),
|
||||
"SAE_CALCULATED_ENGINE_LOAD": ("Engine Load", "%"),
|
||||
"SAE_THROTTLE_POSITION": ("Throttle Position", "%"),
|
||||
"SAE_INTAKE_AIR_TEMP": ("Intake Air Temp", "°C"),
|
||||
"SAE_MAF_AIR_FLOW": ("MAF Air Flow", "g/s"),
|
||||
"SAE_FUEL_TANK_LEVEL": ("Fuel Level", "%"),
|
||||
"SAE_CONTROL_MODULE_VOLTAGE": ("Module Voltage", "V"),
|
||||
"SAE_AMBIENT_AIR_TEMP": ("Ambient Temp", "°C"),
|
||||
"SAE_HYBRID_EV_BATTERY_REMAINING": ("EV Battery SOC", "%"),
|
||||
"BOLT_HVBAT_SOC": ("Battery SOC", "%"),
|
||||
"BOLT_HVBAT_VOLTAGE": ("Pack Voltage", "V"),
|
||||
"BOLT_HVBAT_CURRENT": ("Pack Current", "A"),
|
||||
"BOLT_HVBAT_TEMP": ("Battery Temp", "°C"),
|
||||
"BOLT_HVBAT_POWER": ("Battery Power", "kW"),
|
||||
"BOLT_MOTOR_RPM": ("Motor RPM", "rpm"),
|
||||
"BOLT_MOTOR_TEMP": ("Motor Temp", "°C"),
|
||||
}
|
||||
|
||||
|
||||
class OBDysseyScreen(Widget):
|
||||
"""Full on-device Car Scanner and Diagnostics UI panel backed by obdysseyd."""
|
||||
def __init__(self, client: OBDysseyClient | None = None, params: Params | None = None):
|
||||
super().__init__()
|
||||
self._client = client or OBDysseyClient()
|
||||
self.params = params or Params()
|
||||
|
||||
self._scroll_panel = GuiScrollPanel()
|
||||
self._back_button = Button(tr("Back"), self._go_back, button_style=ButtonStyle.NORMAL, font_size=42)
|
||||
self._retry_button = Button(tr("Retry"), self._retry_connection, button_style=ButtonStyle.NORMAL, font_size=42)
|
||||
self._dtc_button = Button(tr("Scan DTCs"), self._trigger_dtc_scan, button_style=ButtonStyle.NORMAL, font_size=42)
|
||||
self._clear_button = Button(tr("Clear Codes"), self._confirm_clear_dtcs, button_style=ButtonStyle.DANGER, font_size=42)
|
||||
|
||||
self._stop_event = threading.Event()
|
||||
self._poller_thread: threading.Thread | None = None
|
||||
|
||||
# Cached state updated by poller thread
|
||||
self._status: OBDysseyStatus | None = None
|
||||
self._available_signals: list[dict[str, Any]] = []
|
||||
self._live_telemetry: dict[str, Any] = {}
|
||||
self._dtcs: list[dict[str, Any]] = []
|
||||
self._dtc_state = DTC_STATE_UNAVAILABLE
|
||||
self._dtc_scan_in_progress = False
|
||||
self._clear_in_progress = False
|
||||
self._retry_in_progress = False
|
||||
self._last_error = ""
|
||||
|
||||
def _go_back(self):
|
||||
gui_app.pop_widget()
|
||||
|
||||
def _diagnostic_ready(self) -> bool:
|
||||
status = self._status
|
||||
return bool(status and (getattr(status, "diagnostic_ready", False) or getattr(status, "connected", False) or status.state == "ready"))
|
||||
|
||||
def _retry_connection(self):
|
||||
if self._retry_in_progress:
|
||||
return
|
||||
self._retry_in_progress = True
|
||||
self._last_error = ""
|
||||
self._dtc_state = DTC_STATE_UNAVAILABLE
|
||||
|
||||
def _task():
|
||||
try:
|
||||
self._client.connect()
|
||||
except Exception as err:
|
||||
self._last_error = str(err)
|
||||
cloudlog.warning(f"OBDysseyScreen error retrying connection: {err}")
|
||||
finally:
|
||||
self._retry_in_progress = False
|
||||
|
||||
threading.Thread(target=_task, daemon=True).start()
|
||||
|
||||
def show_event(self):
|
||||
super().show_event()
|
||||
self._stop_event.clear()
|
||||
self._poller_thread = threading.Thread(target=self._worker_loop, daemon=True)
|
||||
self._poller_thread.start()
|
||||
|
||||
def hide_event(self):
|
||||
self._stop_event.set()
|
||||
if self._poller_thread is not None and self._poller_thread.is_alive():
|
||||
self._poller_thread.join(timeout=0.2)
|
||||
self._poller_thread = None
|
||||
super().hide_event()
|
||||
|
||||
def _trigger_dtc_scan(self):
|
||||
if not self._diagnostic_ready():
|
||||
return
|
||||
if not self._dtc_scan_in_progress:
|
||||
self._dtc_scan_in_progress = True
|
||||
self._dtc_state = DTC_STATE_IN_PROGRESS
|
||||
self._last_error = ""
|
||||
threading.Thread(target=self._scan_dtcs_worker, daemon=True).start()
|
||||
|
||||
def _scan_dtcs_worker(self):
|
||||
try:
|
||||
dtcs = self._client.read_dtcs()
|
||||
self._dtcs = dtcs
|
||||
self._dtc_state = DTC_STATE_FAULTS if dtcs else DTC_STATE_CLEAN
|
||||
self._last_error = ""
|
||||
except Exception as err:
|
||||
self._dtcs = []
|
||||
self._dtc_state = DTC_STATE_UNAVAILABLE
|
||||
self._last_error = str(err)
|
||||
cloudlog.warning(f"OBDysseyScreen error scanning DTCs: {err}")
|
||||
finally:
|
||||
self._dtc_scan_in_progress = False
|
||||
|
||||
def _confirm_clear_dtcs(self):
|
||||
if not self.params.get_bool("IsOffroad"):
|
||||
gui_app.push_widget(alert_dialog(tr("Clearing trouble codes is only permitted when vehicle is parked / offroad.")))
|
||||
return
|
||||
|
||||
def apply(result: DialogResult):
|
||||
if result == DialogResult.CONFIRM:
|
||||
self._clear_dtcs_worker()
|
||||
|
||||
gui_app.push_widget(ConfirmDialog(
|
||||
tr("Clear all diagnostic trouble codes and reset Check Engine Light?"),
|
||||
tr("Clear Codes"),
|
||||
callback=apply,
|
||||
))
|
||||
|
||||
def _clear_dtcs_worker(self):
|
||||
self._clear_in_progress = True
|
||||
self._dtc_state = DTC_STATE_IN_PROGRESS
|
||||
|
||||
def _task():
|
||||
try:
|
||||
self._client.clear_dtcs()
|
||||
time.sleep(0.5)
|
||||
# Rescan after clearing
|
||||
dtcs = self._client.read_dtcs()
|
||||
self._dtcs = dtcs
|
||||
self._dtc_state = DTC_STATE_FAULTS if dtcs else DTC_STATE_CLEAN
|
||||
self._last_error = ""
|
||||
gui_app.push_widget(alert_dialog(tr("Diagnostic trouble codes cleared successfully.")))
|
||||
except Exception as err:
|
||||
self._dtc_state = DTC_STATE_UNAVAILABLE
|
||||
self._last_error = str(err)
|
||||
gui_app.push_widget(alert_dialog(tr("Failed to clear trouble codes: {}").format(err)))
|
||||
finally:
|
||||
self._clear_in_progress = False
|
||||
|
||||
threading.Thread(target=_task, daemon=True).start()
|
||||
|
||||
def _worker_loop(self):
|
||||
# 1. Fetch status and available signals once upon entry. DTC scans are
|
||||
# explicitly user-triggered so an empty, unscanned result is never shown
|
||||
# as a clean vehicle.
|
||||
try:
|
||||
self._status = self._client.status()
|
||||
self._available_signals = self._client.list_signals()
|
||||
except Exception as err:
|
||||
self._last_error = str(err)
|
||||
|
||||
# 2. Main telemetry polling loop (~500ms cycle)
|
||||
while not self._stop_event.is_set():
|
||||
try:
|
||||
was_diagnostic_ready = self._diagnostic_ready()
|
||||
self._status = self._client.status()
|
||||
if self._status.last_error:
|
||||
self._last_error = self._status.last_error
|
||||
elif self._diagnostic_ready() and not was_diagnostic_ready:
|
||||
# Clear a connection failure once the daemon has recovered, while
|
||||
# preserving errors from a diagnostic operation made in ready state.
|
||||
self._last_error = ""
|
||||
|
||||
if self._diagnostic_ready():
|
||||
if not self._available_signals:
|
||||
self._available_signals = self._client.list_signals()
|
||||
sig_ids = [s["id"] for s in self._available_signals]
|
||||
if sig_ids:
|
||||
# Query in batches
|
||||
readings = self._client.read_signals(sig_ids)
|
||||
self._live_telemetry.update(readings)
|
||||
else:
|
||||
# Never present stale readings as live while the link is only
|
||||
# connecting/initializing or after it has failed.
|
||||
self._live_telemetry.clear()
|
||||
self._dtcs.clear()
|
||||
self._dtc_state = DTC_STATE_UNAVAILABLE
|
||||
except Exception as err:
|
||||
self._last_error = str(err)
|
||||
|
||||
self._stop_event.wait(0.5)
|
||||
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
header_rect = rl.Rectangle(rect.x, rect.y, rect.width, HEADER_HEIGHT)
|
||||
self._render_header(header_rect)
|
||||
|
||||
content_rect = rl.Rectangle(rect.x, rect.y + HEADER_HEIGHT, rect.width, rect.height - HEADER_HEIGHT)
|
||||
self._render_content(content_rect)
|
||||
|
||||
def _render_header(self, rect: rl.Rectangle):
|
||||
rl.draw_rectangle_rec(rect, PANEL_BACKGROUND)
|
||||
line_y = int(rect.y + rect.height - 1)
|
||||
rl.draw_line(int(rect.x), line_y, int(rect.x + rect.width), line_y, ROW_BORDER)
|
||||
|
||||
# Title & Adapter status subtitle
|
||||
gui_label(rl.Rectangle(rect.x + HEADER_PADDING, rect.y + 26, 600, 68), tr("OBDyssey Diagnostics"), font_size=64, font_weight=FontWeight.BOLD)
|
||||
|
||||
status = self._status
|
||||
if status and (getattr(status, "diagnostic_ready", False) or getattr(status, "connected", False) or status.state == "ready"):
|
||||
volt_str = f"{status.adapter_voltage:.1f}V" if status.adapter_voltage is not None else ""
|
||||
sub_parts = [status.adapter_name or status.elm_identity or "OBDII Adapter"]
|
||||
if volt_str:
|
||||
sub_parts.append(volt_str)
|
||||
if status.profile:
|
||||
sub_parts.append(f"Profile: {status.profile}")
|
||||
subtitle = " • ".join(sub_parts)
|
||||
sub_color = TEXT_CONNECTED
|
||||
elif status and (getattr(status, "link_connected", False) or status.state == "initializing"):
|
||||
subtitle = tr("Adapter connected • Preparing diagnostics...")
|
||||
sub_color = TEXT_WARNING
|
||||
elif status and status.state in ("connecting", "reconnecting"):
|
||||
subtitle = tr("Connecting to adapter...")
|
||||
sub_color = TEXT_WARNING
|
||||
elif status and status.state == "error":
|
||||
subtitle = tr("Connection failed")
|
||||
sub_color = TEXT_DANGER
|
||||
else:
|
||||
subtitle = tr("Adapter disconnected")
|
||||
sub_color = TEXT_SECONDARY
|
||||
|
||||
gui_label(rl.Rectangle(rect.x + HEADER_PADDING, rect.y + 104, 750, 44), subtitle, font_size=38, color=sub_color)
|
||||
|
||||
error_text = (status.last_error if status else "") or self._last_error
|
||||
if error_text:
|
||||
# BlueZ errors can include a long diagnostic detail. Keep the status
|
||||
# readable on the fixed-width header while retaining the full value in
|
||||
# the daemon status/API and logs.
|
||||
error_text = error_text.replace("\n", " ")
|
||||
if len(error_text) > 105:
|
||||
error_text = error_text[:102] + "..."
|
||||
gui_label(rl.Rectangle(rect.x + HEADER_PADDING, rect.y + 150, 1000, 25), f"{tr('Error')}: {error_text}", font_size=24, color=TEXT_DANGER)
|
||||
|
||||
# Action buttons on right
|
||||
btn_y = rect.y + (rect.height - BUTTON_HEIGHT) / 2
|
||||
cur_x = rect.x + rect.width - HEADER_PADDING
|
||||
|
||||
# Back button
|
||||
cur_x -= 180
|
||||
self._back_button.render(rl.Rectangle(cur_x, btn_y, 180, BUTTON_HEIGHT))
|
||||
|
||||
# Retry is the only UI-initiated connection action. The daemon remains
|
||||
# responsible for its background reconnect policy.
|
||||
show_retry = bool(not status or status.state in ("idle", "error", "disabled"))
|
||||
if show_retry:
|
||||
cur_x -= (BUTTON_GAP + 180)
|
||||
self._retry_button.set_enabled(not self._retry_in_progress)
|
||||
self._retry_button.set_text(tr("Retrying...") if self._retry_in_progress else tr("Retry"))
|
||||
self._retry_button.render(rl.Rectangle(cur_x, btn_y, 180, BUTTON_HEIGHT))
|
||||
|
||||
# Clear DTCs button
|
||||
cur_x -= (BUTTON_GAP + 260)
|
||||
self._clear_button.set_enabled(not self._clear_in_progress and self._diagnostic_ready())
|
||||
self._clear_button.set_text(tr("Clearing...") if self._clear_in_progress else tr("Clear Codes"))
|
||||
self._clear_button.render(rl.Rectangle(cur_x, btn_y, 260, BUTTON_HEIGHT))
|
||||
|
||||
# Scan DTCs button
|
||||
cur_x -= (BUTTON_GAP + 240)
|
||||
self._dtc_button.set_enabled(not self._dtc_scan_in_progress and self._diagnostic_ready())
|
||||
self._dtc_button.set_text(tr("Scanning...") if self._dtc_scan_in_progress else tr("Scan DTCs"))
|
||||
self._dtc_button.render(rl.Rectangle(cur_x, btn_y, 240, BUTTON_HEIGHT))
|
||||
|
||||
def _render_content(self, rect: rl.Rectangle):
|
||||
# Total content height calculation
|
||||
telemetry_items = list(self._live_telemetry.items())
|
||||
num_cards = len(telemetry_items)
|
||||
cols = 3
|
||||
card_margin = 25
|
||||
card_width = (rect.width - 2 * HEADER_PADDING - (cols - 1) * card_margin) / cols
|
||||
card_height = 150
|
||||
rows_count = (num_cards + cols - 1) // cols
|
||||
|
||||
telemetry_height = rows_count * (card_height + card_margin) + 70 if num_cards > 0 else 0
|
||||
dtc_height = 120 + max(1, len(self._dtcs)) * 140
|
||||
total_height = max(rect.height + 10, telemetry_height + dtc_height + 100)
|
||||
|
||||
content_rect = rl.Rectangle(rect.x, rect.y, rect.width, total_height)
|
||||
offset = self._scroll_panel.update(rect, content_rect)
|
||||
|
||||
rl.begin_scissor_mode(int(rect.x), int(rect.y), int(rect.width), int(rect.height))
|
||||
cur_y = rect.y + HEADER_PADDING + offset
|
||||
|
||||
# Section 1: Live Telemetry Grid
|
||||
if telemetry_items:
|
||||
gui_label(rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 50),
|
||||
tr("Live Sensor Telemetry"), font_size=46, font_weight=FontWeight.BOLD)
|
||||
cur_y += 65
|
||||
|
||||
for idx, (sig_id, val) in enumerate(telemetry_items):
|
||||
col = idx % cols
|
||||
row = idx // cols
|
||||
card_x = rect.x + HEADER_PADDING + col * (card_width + card_margin)
|
||||
card_y = cur_y + row * (card_height + card_margin)
|
||||
card_rect = rl.Rectangle(card_x, card_y, card_width, card_height)
|
||||
|
||||
if rl.check_collision_recs(card_rect, rect):
|
||||
rl.draw_rectangle_rounded(card_rect, 0.12, 16, CARD_BACKGROUND)
|
||||
|
||||
# Name & Unit
|
||||
name, default_unit = COMMON_SIGNAL_NAMES.get(sig_id, (sig_id.replace("SAE_", "").replace("BOLT_", "").replace("_", " ").title(), ""))
|
||||
gui_label(rl.Rectangle(card_x + 20, card_y + 15, card_width - 40, 36), name, font_size=32, color=TEXT_SECONDARY)
|
||||
|
||||
# Value
|
||||
if val is None:
|
||||
val_str = "--"
|
||||
val_color = TEXT_SECONDARY
|
||||
elif isinstance(val, (int, float)):
|
||||
val_str = f"{val:,.1f}" if isinstance(val, float) and not val.is_integer() else f"{int(val):,}"
|
||||
if default_unit:
|
||||
val_str += f" {default_unit}"
|
||||
val_color = TEXT_CONNECTED
|
||||
else:
|
||||
val_str = str(val)
|
||||
val_color = rl.WHITE
|
||||
|
||||
gui_label(rl.Rectangle(card_x + 20, card_y + 60, card_width - 40, 65), val_str, font_size=52, font_weight=FontWeight.BOLD, color=val_color)
|
||||
|
||||
cur_y += rows_count * (card_height + card_margin) + 30
|
||||
|
||||
# Section 2: DTC Fault Codes
|
||||
gui_label(rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 50),
|
||||
tr("Diagnostic Trouble Codes (DTC)"), font_size=46, font_weight=FontWeight.BOLD)
|
||||
cur_y += 65
|
||||
|
||||
dtc_state = self._dtc_state
|
||||
if self._dtcs:
|
||||
dtc_state = DTC_STATE_FAULTS
|
||||
|
||||
if dtc_state == DTC_STATE_IN_PROGRESS:
|
||||
state_rect = rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 120)
|
||||
if rl.check_collision_recs(state_rect, rect):
|
||||
rl.draw_rectangle_rounded(state_rect, 0.12, 16, CARD_BACKGROUND)
|
||||
gui_label(rl.Rectangle(state_rect.x + 30, state_rect.y + 35, state_rect.width - 60, 50),
|
||||
tr("Scanning diagnostic trouble codes..."), font_size=42, color=TEXT_WARNING)
|
||||
cur_y += 140
|
||||
elif dtc_state == DTC_STATE_UNAVAILABLE:
|
||||
state_rect = rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 120)
|
||||
if rl.check_collision_recs(state_rect, rect):
|
||||
rl.draw_rectangle_rounded(state_rect, 0.12, 16, CARD_BACKGROUND)
|
||||
gui_label(rl.Rectangle(state_rect.x + 30, state_rect.y + 25, state_rect.width - 60, 40),
|
||||
tr("DTC status unavailable"), font_size=42, color=TEXT_SECONDARY)
|
||||
gui_label(rl.Rectangle(state_rect.x + 30, state_rect.y + 72, state_rect.width - 60, 30),
|
||||
tr("Connect to the adapter and scan to check vehicle codes."), font_size=26, color=TEXT_SECONDARY)
|
||||
cur_y += 140
|
||||
elif dtc_state == DTC_STATE_CLEAN and not self._dtcs:
|
||||
# Clean state card, only after a successful scan.
|
||||
clean_rect = rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 120)
|
||||
if rl.check_collision_recs(clean_rect, rect):
|
||||
rl.draw_rectangle_rounded(clean_rect, 0.12, 16, CARD_BACKGROUND)
|
||||
gui_label(rl.Rectangle(clean_rect.x + 30, clean_rect.y + 35, clean_rect.width - 60, 50),
|
||||
tr("✓ No diagnostic trouble codes detected. System normal."), font_size=42, color=TEXT_CONNECTED)
|
||||
cur_y += 140
|
||||
else:
|
||||
for dtc in self._dtcs:
|
||||
dtc_rect = rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 120)
|
||||
if rl.check_collision_recs(dtc_rect, rect):
|
||||
rl.draw_rectangle_rounded(dtc_rect, 0.12, 16, CARD_BACKGROUND)
|
||||
|
||||
code = dtc.get("code", "DTC")
|
||||
source = dtc.get("source", "OBD")
|
||||
ecu = dtc.get("ecu")
|
||||
desc = dtc.get("description") or tr("Diagnostic trouble code reported by vehicle ECU")
|
||||
|
||||
# Code badge
|
||||
badge_rect = rl.Rectangle(dtc_rect.x + 25, dtc_rect.y + 25, 180, 70)
|
||||
rl.draw_rectangle_rounded(badge_rect, 0.2, 12, rl.Color(80, 20, 20, 255))
|
||||
gui_label(badge_rect, code, font_size=44, font_weight=FontWeight.BOLD, color=TEXT_DANGER, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
|
||||
|
||||
# Description & ECU info
|
||||
gui_label(rl.Rectangle(dtc_rect.x + 230, dtc_rect.y + 20, dtc_rect.width - 250, 42), desc, font_size=38, font_weight=FontWeight.BOLD)
|
||||
info_str = f"Status: Confirmed • Source: {source}" + (f" • ECU: {ecu}" if ecu else "")
|
||||
gui_label(rl.Rectangle(dtc_rect.x + 230, dtc_rect.y + 65, dtc_rect.width - 250, 36), info_str, font_size=32, color=TEXT_SECONDARY)
|
||||
|
||||
cur_y += 140
|
||||
|
||||
rl.end_scissor_mode()
|
||||
Vendored
+13
@@ -0,0 +1,13 @@
|
||||
Creative Commons Attribution-ShareAlike 4.0 International (CC BY-SA 4.0)
|
||||
|
||||
This profile is sourced from the Open On-Board Diagnostics Database (OBDb)
|
||||
Repository: https://github.com/OBDb/SAEJ1979
|
||||
License: CC BY-SA 4.0 (https://creativecommons.org/licenses/by-sa/4.0/)
|
||||
|
||||
You are free to:
|
||||
- Share — copy and redistribute the material in any medium or format
|
||||
- Adapt — remix, transform, and build upon the material for any purpose, even commercially.
|
||||
|
||||
Under the following terms:
|
||||
- Attribution — You must give appropriate credit, provide a link to the license, and indicate if changes were made.
|
||||
- ShareAlike — If you remix, transform, or build upon the material, you must distribute your contributions under the same license as the original.
|
||||
+305
@@ -0,0 +1,305 @@
|
||||
{
|
||||
"metadata": {
|
||||
"id": "saej1979",
|
||||
"name": "SAE J1979 Standard OBD-II",
|
||||
"provider": "OBDb",
|
||||
"revision": "v3.0.0",
|
||||
"license": "CC-BY-SA-4.0",
|
||||
"description": "Standard OBD-II Mode 01 diagnostic definitions from OBDb SAEJ1979 repository"
|
||||
},
|
||||
"commands": [
|
||||
{
|
||||
"id": "CMD_SAE_RPM",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "0C",
|
||||
"freq": 5.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_ENGINE_RPM",
|
||||
"name": "Engine RPM",
|
||||
"path": "Engine",
|
||||
"suggested_metric": "engineRpm",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 16,
|
||||
"mul": 1.0,
|
||||
"div": 4.0,
|
||||
"unit": "rpm"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_SPEED",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "0D",
|
||||
"freq": 5.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_VEHICLE_SPEED",
|
||||
"name": "Vehicle Speed",
|
||||
"path": "Vehicle",
|
||||
"suggested_metric": "vEgo",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"unit": "km/h"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_ECT",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "05",
|
||||
"freq": 1.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_ENGINE_COOLANT_TEMP",
|
||||
"name": "Engine Coolant Temperature",
|
||||
"path": "Engine",
|
||||
"suggested_metric": "coolantTemp",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"add": -40.0,
|
||||
"unit": "°C"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_LOAD",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "04",
|
||||
"freq": 2.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_CALCULATED_ENGINE_LOAD",
|
||||
"name": "Calculated Engine Load",
|
||||
"path": "Engine",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 100.0,
|
||||
"div": 255.0,
|
||||
"unit": "%"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_THROTTLE",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "11",
|
||||
"freq": 5.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_THROTTLE_POSITION",
|
||||
"name": "Throttle Position",
|
||||
"path": "Engine",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 100.0,
|
||||
"div": 255.0,
|
||||
"unit": "%"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_IAT",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "0F",
|
||||
"freq": 1.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_INTAKE_AIR_TEMP",
|
||||
"name": "Intake Air Temperature",
|
||||
"path": "Engine",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"add": -40.0,
|
||||
"unit": "°C"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_MAF",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "10",
|
||||
"freq": 2.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_MAF_AIR_FLOW",
|
||||
"name": "MAF Air Flow Rate",
|
||||
"path": "Engine",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 16,
|
||||
"mul": 1.0,
|
||||
"div": 100.0,
|
||||
"unit": "g/s"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_FUEL_LEVEL",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "2F",
|
||||
"freq": 0.5,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_FUEL_TANK_LEVEL",
|
||||
"name": "Fuel Tank Level Input",
|
||||
"path": "Fuel",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 100.0,
|
||||
"div": 255.0,
|
||||
"unit": "%"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_BARO",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "33",
|
||||
"freq": 0.2,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_BAROMETRIC_PRESSURE",
|
||||
"name": "Absolute Barometric Pressure",
|
||||
"path": "Engine",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"unit": "kPa"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_VOLTAGE",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "42",
|
||||
"freq": 1.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_CONTROL_MODULE_VOLTAGE",
|
||||
"name": "Control Module Voltage",
|
||||
"path": "Electrical",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 16,
|
||||
"mul": 1.0,
|
||||
"div": 1000.0,
|
||||
"unit": "V"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_AMBIENT_TEMP",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "46",
|
||||
"freq": 0.5,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_AMBIENT_AIR_TEMP",
|
||||
"name": "Ambient Air Temperature",
|
||||
"path": "Environment",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"add": -40.0,
|
||||
"unit": "°C"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_EV_BATTERY",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "5B",
|
||||
"freq": 1.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_HYBRID_EV_BATTERY_REMAINING",
|
||||
"name": "Hybrid/EV Battery Pack Remaining Life",
|
||||
"path": "Battery",
|
||||
"suggested_metric": "stateOfCharge",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 8,
|
||||
"mul": 100.0,
|
||||
"div": 255.0,
|
||||
"unit": "%"
|
||||
}
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"id": "CMD_SAE_RUNTIME",
|
||||
"hdr": "7DF",
|
||||
"rax": "7E8",
|
||||
"service": 1,
|
||||
"pid": "1F",
|
||||
"freq": 1.0,
|
||||
"signals": [
|
||||
{
|
||||
"id": "SAE_RUN_TIME_SINCE_ENGINE_START",
|
||||
"name": "Run Time Since Engine Start",
|
||||
"path": "Engine",
|
||||
"fmt": {
|
||||
"bix": 0,
|
||||
"len": 16,
|
||||
"mul": 1.0,
|
||||
"div": 1.0,
|
||||
"unit": "s"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user