diff --git a/selfdrive/ui/tests/test_bluetooth_big_ui.py b/selfdrive/ui/tests/test_bluetooth_big_ui.py index ce2fbea05..e71c21a3b 100644 --- a/selfdrive/ui/tests/test_bluetooth_big_ui.py +++ b/selfdrive/ui/tests/test_bluetooth_big_ui.py @@ -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) diff --git a/selfdrive/ui/tests/test_obdyssey_ui.py b/selfdrive/ui/tests/test_obdyssey_ui.py new file mode 100644 index 000000000..36aae2d70 --- /dev/null +++ b/selfdrive/ui/tests/test_obdyssey_ui.py @@ -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 diff --git a/starpilot/system/bluetooth/bluez.py b/starpilot/system/bluetooth/bluez.py index f897beb09..b3a6b93a1 100644 --- a/starpilot/system/bluetooth/bluez.py +++ b/starpilot/system/bluetooth/bluez.py @@ -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) diff --git a/starpilot/system/bluetooth/protocol.py b/starpilot/system/bluetooth/protocol.py index e2641691c..b8580309b 100644 --- a/starpilot/system/bluetooth/protocol.py +++ b/starpilot/system/bluetooth/protocol.py @@ -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() diff --git a/starpilot/system/bluetooth/tests/test_bluetooth.py b/starpilot/system/bluetooth/tests/test_bluetooth.py index 1628a50cb..87589f843 100644 --- a/starpilot/system/bluetooth/tests/test_bluetooth.py +++ b/starpilot/system/bluetooth/tests/test_bluetooth.py @@ -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() diff --git a/starpilot/system/obdyssey/__init__.py b/starpilot/system/obdyssey/__init__.py new file mode 100644 index 000000000..f8133cf50 --- /dev/null +++ b/starpilot/system/obdyssey/__init__.py @@ -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", +] diff --git a/starpilot/system/obdyssey/curated_profiles/Chevrolet-Bolt-EV.json b/starpilot/system/obdyssey/curated_profiles/Chevrolet-Bolt-EV.json new file mode 100644 index 000000000..3598e805c --- /dev/null +++ b/starpilot/system/obdyssey/curated_profiles/Chevrolet-Bolt-EV.json @@ -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" + } + } + ] +} diff --git a/starpilot/system/obdyssey/daemon.py b/starpilot/system/obdyssey/daemon.py new file mode 100644 index 000000000..517237b54 --- /dev/null +++ b/starpilot/system/obdyssey/daemon.py @@ -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() diff --git a/starpilot/system/obdyssey/diagnostics.py b/starpilot/system/obdyssey/diagnostics.py new file mode 100644 index 000000000..fd85ea6ba --- /dev/null +++ b/starpilot/system/obdyssey/diagnostics.py @@ -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 ... + 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 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 + 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) diff --git a/starpilot/system/obdyssey/elm327.py b/starpilot/system/obdyssey/elm327.py new file mode 100644 index 000000000..6edc93f42 --- /dev/null +++ b/starpilot/system/obdyssey/elm327.py @@ -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 + 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, + ) diff --git a/starpilot/system/obdyssey/obdb.py b/starpilot/system/obdyssey/obdb.py new file mode 100644 index 000000000..75a412a9d --- /dev/null +++ b/starpilot/system/obdyssey/obdb.py @@ -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 or 0x41 ) + 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, + ) diff --git a/starpilot/system/obdyssey/profiles.py b/starpilot/system/obdyssey/profiles.py new file mode 100644 index 000000000..8739abba7 --- /dev/null +++ b/starpilot/system/obdyssey/profiles.py @@ -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 diff --git a/starpilot/system/obdyssey/protocol.py b/starpilot/system/obdyssey/protocol.py new file mode 100644 index 000000000..11c393afa --- /dev/null +++ b/starpilot/system/obdyssey/protocol.py @@ -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) diff --git a/starpilot/system/obdyssey/tests/__init__.py b/starpilot/system/obdyssey/tests/__init__.py new file mode 100644 index 000000000..2e0152a48 --- /dev/null +++ b/starpilot/system/obdyssey/tests/__init__.py @@ -0,0 +1 @@ +# OBDyssey test suite diff --git a/starpilot/system/obdyssey/tests/test_daemon.py b/starpilot/system/obdyssey/tests/test_daemon.py new file mode 100644 index 000000000..2c8c8e338 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_daemon.py @@ -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 diff --git a/starpilot/system/obdyssey/tests/test_diagnostics.py b/starpilot/system/obdyssey/tests/test_diagnostics.py new file mode 100644 index 000000000..79d32a09c --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_diagnostics.py @@ -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 diff --git a/starpilot/system/obdyssey/tests/test_elm327.py b/starpilot/system/obdyssey/tests/test_elm327.py new file mode 100644 index 000000000..dc57aa0a2 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_elm327.py @@ -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 diff --git a/starpilot/system/obdyssey/tests/test_obdb.py b/starpilot/system/obdyssey/tests/test_obdb.py new file mode 100644 index 000000000..f5299900c --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_obdb.py @@ -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 diff --git a/starpilot/system/obdyssey/tests/test_profiles.py b/starpilot/system/obdyssey/tests/test_profiles.py new file mode 100644 index 000000000..db472641d --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_profiles.py @@ -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 diff --git a/starpilot/system/obdyssey/tests/test_protocol.py b/starpilot/system/obdyssey/tests/test_protocol.py new file mode 100644 index 000000000..1df841579 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_protocol.py @@ -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 diff --git a/starpilot/system/obdyssey/tests/test_transport.py b/starpilot/system/obdyssey/tests/test_transport.py new file mode 100644 index 000000000..22f917716 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_transport.py @@ -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>" diff --git a/starpilot/system/obdyssey/transport.py b/starpilot/system/obdyssey/transport.py new file mode 100644 index 000000000..941168b00 --- /dev/null +++ b/starpilot/system/obdyssey/transport.py @@ -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) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 9d94384ba..cce7f91f3 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -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), diff --git a/system/ui/widgets/bluetooth.py b/system/ui/widgets/bluetooth.py index def4d6182..eb955ab4c 100644 --- a/system/ui/widgets/bluetooth.py +++ b/system/ui/widgets/bluetooth.py @@ -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) diff --git a/system/ui/widgets/obdyssey.py b/system/ui/widgets/obdyssey.py new file mode 100644 index 000000000..e8cb86530 --- /dev/null +++ b/system/ui/widgets/obdyssey.py @@ -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() diff --git a/third_party/obdb_saej1979/LICENSE b/third_party/obdb_saej1979/LICENSE new file mode 100644 index 000000000..cce96c704 --- /dev/null +++ b/third_party/obdb_saej1979/LICENSE @@ -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. diff --git a/third_party/obdb_saej1979/profile.json b/third_party/obdb_saej1979/profile.json new file mode 100644 index 000000000..5cf785d29 --- /dev/null +++ b/third_party/obdb_saej1979/profile.json @@ -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" + } + } + ] + } + ] +}