From e3a82ba5c8427c765f953d82cc640d7c99a5e02f Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Sun, 30 Aug 2026 04:28:08 -0400 Subject: [PATCH] OBDyssey --- selfdrive/ui/tests/test_bluetooth_big_ui.py | 13 + selfdrive/ui/tests/test_obdyssey_ui.py | 224 ++++++ starpilot/system/bluetooth/bluez.py | 366 +++++++-- starpilot/system/bluetooth/daemon.py | 1 + starpilot/system/bluetooth/protocol.py | 22 +- .../system/bluetooth/tests/test_bluetooth.py | 89 ++- starpilot/system/obdyssey/__init__.py | 47 ++ .../curated_profiles/Chevrolet-Bolt-EV.json | 173 +++++ starpilot/system/obdyssey/daemon.py | 708 ++++++++++++++++++ starpilot/system/obdyssey/diagnostics.py | 456 +++++++++++ starpilot/system/obdyssey/elm327.py | 590 +++++++++++++++ starpilot/system/obdyssey/obdb.py | 339 +++++++++ starpilot/system/obdyssey/profiles.py | 413 ++++++++++ starpilot/system/obdyssey/protocol.py | 228 ++++++ starpilot/system/obdyssey/tests/__init__.py | 1 + .../system/obdyssey/tests/test_daemon.py | 546 ++++++++++++++ .../system/obdyssey/tests/test_diagnostics.py | 246 ++++++ .../system/obdyssey/tests/test_elm327.py | 381 ++++++++++ starpilot/system/obdyssey/tests/test_obdb.py | 151 ++++ .../system/obdyssey/tests/test_profiles.py | 220 ++++++ .../system/obdyssey/tests/test_protocol.py | 30 + .../system/obdyssey/tests/test_transport.py | 101 +++ starpilot/system/obdyssey/transport.py | 228 ++++++ system/manager/process_config.py | 1 + system/ui/widgets/bluetooth.py | 24 +- system/ui/widgets/obdyssey.py | 464 ++++++++++++ third_party/obdb_saej1979/LICENSE | 13 + third_party/obdb_saej1979/profile.json | 292 ++++++++ 28 files changed, 6280 insertions(+), 87 deletions(-) create mode 100644 selfdrive/ui/tests/test_obdyssey_ui.py create mode 100644 starpilot/system/obdyssey/__init__.py create mode 100644 starpilot/system/obdyssey/curated_profiles/Chevrolet-Bolt-EV.json create mode 100644 starpilot/system/obdyssey/daemon.py create mode 100644 starpilot/system/obdyssey/diagnostics.py create mode 100644 starpilot/system/obdyssey/elm327.py create mode 100644 starpilot/system/obdyssey/obdb.py create mode 100644 starpilot/system/obdyssey/profiles.py create mode 100644 starpilot/system/obdyssey/protocol.py create mode 100644 starpilot/system/obdyssey/tests/__init__.py create mode 100644 starpilot/system/obdyssey/tests/test_daemon.py create mode 100644 starpilot/system/obdyssey/tests/test_diagnostics.py create mode 100644 starpilot/system/obdyssey/tests/test_elm327.py create mode 100644 starpilot/system/obdyssey/tests/test_obdb.py create mode 100644 starpilot/system/obdyssey/tests/test_profiles.py create mode 100644 starpilot/system/obdyssey/tests/test_protocol.py create mode 100644 starpilot/system/obdyssey/tests/test_transport.py create mode 100644 starpilot/system/obdyssey/transport.py create mode 100644 system/ui/widgets/obdyssey.py create mode 100644 third_party/obdb_saej1979/LICENSE create mode 100644 third_party/obdb_saej1979/profile.json diff --git a/selfdrive/ui/tests/test_bluetooth_big_ui.py b/selfdrive/ui/tests/test_bluetooth_big_ui.py index ce2fbea05..b10317bae 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 address: opened.append(address) + ui._select_device("AA:BB:CC:DD:EE:FF") + + assert opened == ["AA:BB:CC:DD:EE:FF"] + 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..236a8930e --- /dev/null +++ b/selfdrive/ui/tests/test_obdyssey_ui.py @@ -0,0 +1,224 @@ +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._adapter_address = "" + 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/daemon.py b/starpilot/system/bluetooth/daemon.py index 6ea8d5ae6..d5970133c 100644 --- a/starpilot/system/bluetooth/daemon.py +++ b/starpilot/system/bluetooth/daemon.py @@ -167,6 +167,7 @@ class BluetoothController: finally: self._reset_client() self._radio.stop() + self.params.remove("BluetoothAudioAddress") self.params.put_bool("BluetoothEnabled", False) self._scan_deadline = 0.0 elif command == "start_scan": diff --git a/starpilot/system/bluetooth/protocol.py b/starpilot/system/bluetooth/protocol.py index a7ce359f9..d7be7301f 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: @@ -205,8 +215,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 0e61937e7..a68b6bdbb 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): @@ -235,6 +254,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() @@ -267,16 +324,6 @@ def test_power_pair_audio_and_offroad_enforcement(): assert not params.get_bool("BluetoothEnabled") and radio.stops == 1 and clients[0].closed -def test_power_off_preserves_saved_audio_selection(): - params = FakeParams(IsOffroad=True, BluetoothEnabled=False, BluetoothAudioAddress="00:11:22:33:44:55") - controller = BluetoothController(params, FakeBlueZ, FakeRadio()) - - controller.handle({"command": "set_power", "enabled": True}) - controller.handle({"command": "set_power", "enabled": False}) - - assert params.get("BluetoothAudioAddress") == "00:11:22:33:44:55" - - def test_status_does_not_restart_radio_during_disable(): params = FakeParams(IsOffroad=True, BluetoothEnabled=True) radio = BlockingStopRadio() 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..b66764df9 --- /dev/null +++ b/starpilot/system/obdyssey/curated_profiles/Chevrolet-Bolt-EV.json @@ -0,0 +1,173 @@ +{ + "metadata": { + "id": "Chevrolet-Bolt-EV", + "name": "Chevrolet Bolt EV / EUV", + "provider": "OBDb", + "revision": "v3.2.0", + "protocol": "ISO 15765-4 (CAN 11/500)", + "description": "OEM Mode 22 High-Voltage Battery & Inverter definitions for Chevrolet Bolt EV", + "override": { + "reason": "Keep validated Bolt signals on their confirmed individual request PIDs instead of the earlier grouped request assumption.", + "source": "StarPilot OBDyssey hardware validation", + "validated_vehicle_year": "Chevrolet Bolt EV/EUV, 2017-2023", + "upstream_obdb_revision": "v3.2.0", + "replaces": "Upstream or prototype grouped Bolt Mode 22 definitions where the request PID was not independently confirmed." + } + }, + "commands": [ + { + "id": "CMD_BOLT_SOC", + "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": 100.0, + "div": 255.0, + "unit": "%" + } + } + ] + }, + { + "id": "CMD_BOLT_VOLTAGE", + "hdr": "7E4", + "rax": "7EC", + "service": 34, + "pid": "41A3", + "freq": 2.0, + "signals": [ + { + "id": "BOLT_HVBAT_VOLTAGE", + "name": "HV Battery Pack Voltage", + "path": "Battery", + "suggested_metric": "batteryVoltage", + "fmt": { + "bix": 0, + "len": 16, + "mul": 0.01, + "div": 1.0, + "unit": "V" + } + } + ] + }, + { + "id": "CMD_BOLT_CURRENT", + "hdr": "7E4", + "rax": "7EC", + "service": 34, + "pid": "2409", + "freq": 2.0, + "signals": [ + { + "id": "BOLT_HVBAT_CURRENT", + "name": "HV Battery Pack Current", + "path": "Battery", + "suggested_metric": "batteryCurrent", + "fmt": { + "bix": 0, + "len": 16, + "sign": true, + "mul": 0.05, + "div": 1.0, + "unit": "A" + } + } + ] + }, + { + "id": "CMD_BOLT_TEMP", + "hdr": "7E4", + "rax": "7EC", + "service": 34, + "pid": "41A6", + "freq": 1.0, + "signals": [ + { + "id": "BOLT_HVBAT_TEMP", + "name": "HV Battery Average Temperature", + "path": "Battery", + "fmt": { + "bix": 0, + "len": 8, + "mul": 1.0, + "div": 1.0, + "add": -40.0, + "unit": "°C" + } + } + ] + }, + { + "id": "CMD_BOLT_MOTOR_SPEED", + "hdr": "7E2", + "rax": "7EA", + "service": 34, + "pid": "0038", + "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": "CMD_BOLT_MOTOR_TEMP", + "hdr": "7E2", + "rax": "7EA", + "service": 34, + "pid": "4084", + "freq": 2.0, + "signals": [ + { + "id": "BOLT_MOTOR_TEMP", + "name": "Motor Temperature", + "path": "Motor", + "fmt": { + "bix": 0, + "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..0799a0bae --- /dev/null +++ b/starpilot/system/obdyssey/daemon.py @@ -0,0 +1,708 @@ +from __future__ import annotations + +import json +import os +import re +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_all_dtcs, + read_freeze_frame, + read_pending_dtcs, + read_permanent_dtcs, + read_stored_dtcs, + read_vin, + uds_clear_diagnostic_information, + uds_diagnostic_session_control, + uds_read_dtc_information, + UdsResponsePendingError, +) +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 + + + +def _parse_hex_value(value: Any, *, name: str, default: int | None = None, + maximum: int = 0x1FFFFFFF) -> int: + """Parse an API hexadecimal value and reject malformed CAN addresses.""" + if value is None or (isinstance(value, str) and not value.strip()): + if default is not None: + return default + raise ValueError(f"{name} is required") + if isinstance(value, bool): + raise ValueError(f"{name} must be hexadecimal") + + if isinstance(value, int): + parsed = value + else: + text = str(value).strip() + if text.lower().startswith("0x"): + text = text[2:] + if not text or re.fullmatch(r"[0-9A-Fa-f]+", text) is None: + raise ValueError(f"{name} must be hexadecimal") + parsed = int(text, 16) + + if not 0 <= parsed <= maximum: + raise ValueError(f"{name} is outside the supported CAN address range") + return parsed + + +def _parse_hex_payload(value: Any, *, name: str = "payload") -> bytes: + if isinstance(value, (bytes, bytearray)): + payload = bytes(value) + else: + try: + payload = bytes.fromhex(str(value or "")) + except ValueError as err: + raise ValueError(f"{name} must contain an even number of hexadecimal digits") from err + if not payload: + raise ValueError(f"{name} must not be empty") + return payload + + +def _parse_request_timeout(value: Any) -> float | None: + if value is None: + return None + try: + timeout = float(value) + except (TypeError, ValueError) as err: + raise ValueError("timeout must be a positive number of seconds") from err + if not 0 < timeout <= 120: + raise ValueError("timeout must be between 0 and 120 seconds") + return timeout + + +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" + # Adapter selection is deliberately process-local. The Bluetooth panel + # supplies an address when the user selects a device; this value only + # permits a read-only retry during the current daemon session. + self._session_adapter_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._status_snapshot: dict[str, Any] = OBDysseyStatus( + api_version=API_VERSION, + state=self._state, + 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: + if self._state == "disabled": + self._state = "idle" + if not self.params.get_bool("BluetoothEnabled") and (self._transport is not None or self._state != "idle"): + self._disconnect_transport() + self._state = "idle" + self._active_address = "" + self._active_name = "" + 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._session_adapter_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: + target_address = str(target_address or "").strip() + target_name = str(target_name or "").strip() + if not self.params.get_bool("BluetoothEnabled"): + self._disconnect_transport() + self._active_address = "" + self._active_name = "" + self._state = "idle" + self._last_error = "Bluetooth is disabled" + self._publish_status_snapshot_unlocked() + return False + explicit_target = bool(target_address) + self._state = "connecting" + self._publish_status_snapshot_unlocked() + self._disconnect_transport() + + if not target_address: + target_address = self._session_adapter_address + + if not target_address: + candidates = self.find_candidate_devices() + if len(candidates) != 1: + self._state = "error" + self._active_address = "" + self._active_name = "" + if not candidates: + self._last_error = "No paired OBD adapter is available" + else: + self._last_error = "Select an OBD adapter from Bluetooth settings" + self._publish_status_snapshot_unlocked() + return False + target_address = str(candidates[0]["address"]) + target_name = str(candidates[0]["name"]) + elif not target_name: + target_name = next((candidate["name"] for candidate in self.find_candidate_devices() + if candidate["address"].upper() == target_address.upper()), "") + + # A failed explicit selection must not leave the previous adapter as an + # implicit reconnect target. The target is restored only after ELM + # initialization validates the newly selected device. + if explicit_target and self._session_adapter_address.upper() != target_address.upper(): + self._session_adapter_address = "" + + 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._session_adapter_address = target_address + self._state = "ready" + self._last_error = "" + 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._disconnect_transport() + self._session_adapter_address = "" + self._state = "idle" + self._active_address = "" + self._active_name = "" + self._publish_status_snapshot_unlocked() + + def reconnect_step(self) -> None: + """Perform passive Bluetooth cleanup; adapter selection is user-driven.""" + if not self.params.get_bool("BluetoothEnabled"): + with self._lock: + if self._transport is not None or self._state != "idle": + self._disconnect_transport() + self._state = "idle" + self._active_address = "" + self._active_name = "" + self._publish_status_snapshot_unlocked() + + def maintain_loop(self) -> None: + while True: + time.sleep(2.0) + self.reconnect_step() + + def _execute_diagnostic(self, func, *args, read_only: bool = True, **kwargs) -> Any: + """Run one serialized transaction, retrying reads after one reconnect.""" + with self._lock: + if self._elm is None or self._state != "ready": + # Requests never probe arbitrary paired serial devices. Discovery is + # an explicit connect action; a demand-driven retry uses only the + # address validated earlier in this daemon session. + target_address = self._session_adapter_address + if not target_address: + raise RuntimeError("No active OBD adapter session; select an adapter from Bluetooth settings") + if not self.connect_adapter(target_address): + raise RuntimeError(self._last_error or "OBD adapter is not connected") + + start_t = time.monotonic() + self._requests_count += 1 + reconnect_attempted = False + pending_attempts = 0 + while True: + try: + result = func(self._elm, *args, **kwargs) + self._last_request_ms = int((time.monotonic() - start_t) * 1000) + return result + except ElmDisconnectedError as err: + self._errors_count += 1 + self._last_error = str(err) + self._state = "reconnecting" + self._disconnect_transport() + self._publish_status_snapshot_unlocked() + + if read_only and not reconnect_attempted: + reconnect_attempted = True + self._reconnects_count += 1 + target_address = self._session_adapter_address + if target_address and self.connect_adapter(target_address): + continue + # A mutating request may have reached the vehicle before the link + # dropped. Report the indeterminate failure and never replay it. + raise + except UdsResponsePendingError as err: + # A read may be safely retried after the ECU's bounded P2/P2* wait; + # a mutating request must never be replayed after response pending. + if read_only and pending_attempts < 3: + pending_attempts += 1 + self._sleep(0.25) + continue + self._errors_count += 1 + self._last_error = str(err) + 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 diagnostic_request(self, payload: bytes, context: ElmContext | None = None, *, + read_only: bool = True, timeout: float | None = None): + """Shared request primitive for raw and structured diagnostic operations.""" + return self._execute_diagnostic( + lambda elm: self._diagnostic_request_unlocked(elm, payload, context, timeout=timeout), + read_only=read_only, + ) + + @staticmethod + def _diagnostic_request_unlocked(elm: Elm327, payload: bytes, context: ElmContext | None = None, + *, timeout: float | None = None): + """Issue one ELM diagnostic request while the controller lock is held.""" + response = elm.request(payload, context, timeout=timeout) + if response.pending: + raise UdsResponsePendingError(payload[0]) + return response + + @staticmethod + def _profile_response(response, expected_prefix: bytes) -> bytes: + """Select the response matching a profile command's declared prefix.""" + if expected_prefix: + for payload in response.payloads: + if payload.startswith(expected_prefix): + return payload + elif response.payload: + return response.payload + expected = expected_prefix.hex().upper() if expected_prefix else "a diagnostic response" + raise ValueError(f"Profile command returned no response beginning with {expected}") + + @staticmethod + def _request_context(request: dict[str, Any], *, require_explicit_addresses: bool = False) -> ElmContext: + if require_explicit_addresses: + for name in ("tx_addr", "rx_addr"): + if request.get(name) is None or not str(request.get(name)).strip(): + raise ValueError(f"{name} is required for this operation") + tx_addr = _parse_hex_value(request.get("tx_addr"), name="tx_addr", default=0x7E0) + rx_addr = _parse_hex_value(request.get("rx_addr"), name="rx_addr", default=0x7E8) + proto = request.get("protocol") + return ElmContext( + protocol=str(proto) if proto else None, + tx_header=tx_addr, + rx_filter=rx_addr, + flow_control=True, + can_auto_format=True, + ) + + 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", "")) + 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 is not None: + uds_diagnostic_session_control(elm, diag_cmd.diagnostic_session_in, diag_cmd.context) + try: + full_payload = bytes([diag_cmd.service]) + diag_cmd.parameter + res = self._diagnostic_request_unlocked(elm, full_payload, diag_cmd.context) + payload = self._profile_response(res, diag_cmd.expected_prefix) + val = decode_signal(payload, sig_def, strip_prefix=diag_cmd.expected_prefix) + return val + finally: + if diag_cmd.diagnostic_session_out is not None: + try: + uds_diagnostic_session_control(elm, diag_cmd.diagnostic_session_out, diag_cmd.context) + except Exception: + pass + + command_read_only = diag_cmd.diagnostic_session_in is None and diag_cmd.diagnostic_session_out is None and not is_mutating_service(diag_cmd.service) + val = self._execute_diagnostic(_query, read_only=command_read_only) + 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 is not None: + uds_diagnostic_session_control(elm, diag_cmd.diagnostic_session_in, diag_cmd.context) + try: + full_payload = bytes([diag_cmd.service]) + diag_cmd.parameter + res = self._diagnostic_request_unlocked(elm, full_payload, diag_cmd.context) + payload = self._profile_response(res, diag_cmd.expected_prefix) + for s in sigs: + results[s.id] = decode_signal(payload, s, strip_prefix=diag_cmd.expected_prefix) + finally: + if diag_cmd.diagnostic_session_out is not None: + 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) + + batch_read_only = all( + diag_cmd.diagnostic_session_in is None and diag_cmd.diagnostic_session_out is None and not is_mutating_service(diag_cmd.service) + for diag_cmd, _sigs in groups + ) + self._execute_diagnostic(_query_batch, read_only=batch_read_only) + 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_pending_dtcs": + dtcs = self._execute_diagnostic(read_pending_dtcs) + return {"ok": True, "dtcs": [d.to_dict() for d in dtcs]} + elif cmd == "read_permanent_dtcs": + dtcs = self._execute_diagnostic(read_permanent_dtcs) + return {"ok": True, "dtcs": [d.to_dict() for d in dtcs]} + elif cmd == "read_all_dtcs": + dtcs = self._execute_diagnostic(read_all_dtcs) + return {"ok": True, "dtcs": [d.to_dict() for d in dtcs]} + elif cmd == "read_freeze_frame": + pid = _parse_hex_value(request.get("pid"), name="pid", default=0, maximum=0xFF) + frame = _parse_hex_value(request.get("frame"), name="frame", default=0, maximum=0xFF) + res = self._execute_diagnostic(lambda elm: read_freeze_frame(elm, pid, frame)) + return {"ok": True, "response": res.payload.hex().upper(), "lines": list(res.lines)} + elif cmd == "read_uds_dtcs": + ctx = self._request_context(request) + tx_addr = ctx.tx_header if ctx.tx_header is not None else 0x7E0 + 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, read_only=False) + return {"ok": True} + elif cmd == "clear_uds_dtcs": + self._require_offroad("clear_uds_dtcs") + ctx = self._request_context(request, require_explicit_addresses=True) + self._execute_diagnostic(lambda elm: uds_clear_diagnostic_information(elm, context=ctx), read_only=False) + return {"ok": True} + elif cmd == "uds_request": + payload_bytes = _parse_hex_payload(request.get("payload")) + ctx = self._request_context(request) + timeout = _parse_request_timeout(request.get("timeout")) + + if not is_read_only_payload(payload_bytes): + self._require_offroad("uds_request (mutating)") + + read_only = is_read_only_payload(payload_bytes) + res = self.diagnostic_request(payload_bytes, ctx, read_only=read_only, timeout=timeout) + return {"ok": True, "response": res.payload.hex().upper(), "lines": list(res.lines)} + elif cmd == "raw_request": + payload_bytes = _parse_hex_payload(request.get("payload")) + ctx = self._request_context(request) + + if not is_read_only_payload(payload_bytes): + self._require_offroad("raw_request (mutating)") + + read_only = is_read_only_payload(payload_bytes) + res = self.diagnostic_request(payload_bytes, ctx, read_only=read_only) + 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", "")) + # An arbitrary AT command may change adapter state; never replay it + # after an ambiguous disconnect. + lines = self._execute_diagnostic(lambda elm: elm.command(at_cmd), read_only=False) + 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 == "install_profile": + provider = str(request.get("provider", "")) + profile_id = str(request.get("profile_id", "")) + metadata = request.get("metadata") + if metadata is not None and not isinstance(metadata, dict): + raise ValueError("profile metadata must be an object") + + data = request.get("data") + if isinstance(data, dict): + raw_meta = data.get("metadata") + if not profile_id and isinstance(raw_meta, dict): + profile_id = str(raw_meta.get("id", "")) + if not profile_id: + raise ValueError("profile_id is required when profile data has no metadata id") + prof = self._profile_manager.install_profile_data(profile_id, data, metadata) + else: + source = str(request.get("repository") or request.get("source") or "") + prof = self._profile_manager.install_profile_source( + source, profile_id=profile_id, provider=provider, metadata=metadata, + ) + return {"ok": True, "profile": prof.id, "revision": prof.revision} + elif cmd == "update_profile": + profile_id = str(request.get("profile_id") or self._profile_manager.resolve_active_profile().id) + if not profile_id: + raise RuntimeError("No active profile is available") + prof = self._profile_manager.update_profile(profile_id) + return {"ok": True, "profile": prof.id, "revision": prof.revision} + elif cmd == "select_profile": + prof_id = str(request.get("profile_id", "")) + prof = self._profile_manager.select_profile(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..4a8e13175 --- /dev/null +++ b/starpilot/system/obdyssey/diagnostics.py @@ -0,0 +1,456 @@ +from __future__ import annotations + +import re +from dataclasses import dataclass +from typing import TYPE_CHECKING, Any + +from opendbc.car.uds import ACCESS_TYPE, DTC_REPORT_TYPE, RESET_TYPE, ROUTINE_CONTROL_TYPE, SERVICE_TYPE, SESSION_TYPE + +if TYPE_CHECKING: + from openpilot.starpilot.system.obdyssey.elm327 import Elm327, ElmContext, DiagnosticResponse + + +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})") + + +class UdsResponsePendingError(Exception): + """The ECU asked the tester to wait before returning the final response.""" + + def __init__(self, service_id: int): + self.service_id = service_id + super().__init__(f"UDS response pending for service 0x{service_id: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 + +from openpilot.starpilot.system.obdyssey.elm327 import ElmCommandError, ElmContext, ElmDisconnectedError, ElmNoDataError, ElmUnsupportedError + +# 0x7DF is a functional request. Leave CRA at the adapter default so every +# standard OBD responder (7E8..7EF) remains eligible to answer. +DEFAULT_OBD_CONTEXT = ElmContext(tx_header=0x7DF, flow_control=True, can_auto_format=True) + + +def _obd_payloads(response: DiagnosticResponse, service: int, pid: int | None = None) -> tuple[bytes, ...]: + expected_service = (service + 0x40) & 0xFF + payloads = tuple( + payload for payload in response.payloads + if payload and payload[0] == expected_service and (pid is None or len(payload) > 1 and payload[1] == pid) + ) + if not payloads: + if response.pending: + raise UdsResponsePendingError(service) + received = response.payload[0] if response.payload else None + expected = f"0x{expected_service:02X}" + (f" PID 0x{pid:02X}" if pid is not None else "") + raise ValueError(f"Unexpected OBD response {received!r}; expected {expected}") + return payloads + + +def read_current_data(elm: Elm327, pid: int, context: ElmContext | None = None) -> DiagnosticResponse: + """Mode 01: Read current powertrain diagnostic data.""" + ctx = context if context is not None else DEFAULT_OBD_CONTEXT + response = elm.request(bytes([0x01, pid & 0xFF]), ctx, retry=True) + _obd_payloads(response, 0x01, pid & 0xFF) + return response + + +def read_freeze_frame(elm: Elm327, pid: int, frame: int = 0, context: ElmContext | None = None) -> DiagnosticResponse: + """Mode 02: Read freeze frame data.""" + ctx = context if context is not None else DEFAULT_OBD_CONTEXT + response = elm.request(bytes([0x02, pid & 0xFF, frame & 0xFF]), ctx, retry=True) + _obd_payloads(response, 0x02, pid & 0xFF) + return response + + +def read_stored_dtcs(elm: Elm327, context: ElmContext | None = None) -> list[DiagnosticTroubleCode]: + """Mode 03: Read confirmed/stored emission-related DTCs.""" + ctx = context if context is not None else DEFAULT_OBD_CONTEXT + res = elm.request(bytes([0x03]), ctx, retry=True) + return [dtc for payload in _obd_payloads(res, 0x03) for dtc in parse_standard_dtcs(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.""" + ctx = context if context is not None else DEFAULT_OBD_CONTEXT + res = elm.request(bytes([0x07]), ctx, retry=True) + return [dtc for payload in _obd_payloads(res, 0x07) for dtc in parse_standard_dtcs(payload, source="OBD_PENDING")] + + +def read_permanent_dtcs(elm: Elm327, context: ElmContext | None = None) -> list[DiagnosticTroubleCode]: + """Mode 0A: Read permanent DTCs.""" + ctx = context if context is not None else DEFAULT_OBD_CONTEXT + res = elm.request(bytes([0x0A]), ctx, retry=True) + return [dtc for payload in _obd_payloads(res, 0x0A) for dtc in parse_standard_dtcs(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 (ElmCommandError, ElmNoDataError, ElmUnsupportedError): + pass + except ElmDisconnectedError: + # The controller owns reconnects. Do not turn a dropped link into an + # apparently successful empty scan by swallowing this exception. + raise + 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!""" + ctx = context if context is not None else DEFAULT_OBD_CONTEXT + response = elm.request(bytes([0x04]), ctx, retry=False) + _obd_payloads(response, 0x04) + return response + + +def read_vin(elm: Elm327, context: ElmContext | None = None) -> str: + """Mode 09 PID 02: Read Vehicle Identification Number (VIN).""" + ctx = context if context is not None else DEFAULT_OBD_CONTEXT + res = elm.request(bytes([0x09, 0x02]), ctx, retry=True) + payloads = _obd_payloads(res, 0x09, 0x02) + + def fragment(payload: bytes) -> tuple[int | None, str]: + # Response format: 49 02 [data item/line number] followed by ASCII VIN. + if len(payload) >= 3 and payload[:2] == bytes([0x49, 0x02]): + return payload[2], "".join(chr(b) for b in payload[3:] if 32 <= b <= 126) + return None, "".join(chr(b) for b in payload if 32 <= b <= 126) + + def complete_vin(group: tuple[bytes, ...]) -> str | None: + fragments = [fragment(payload) for payload in group] + for _sequence, text in fragments: + vin_match = re.search(r"([A-HJ-NPR-Z0-9]{17})", text) + if vin_match: + return vin_match.group(1) + + indexed = [(sequence, text) for sequence, text in fragments if sequence is not None] + if indexed and len({sequence for sequence, _text in indexed}) == len(indexed): + joined = "".join(text for _sequence, text in sorted(indexed)) + vin_match = re.search(r"([A-HJ-NPR-Z0-9]{17})", joined) + if vin_match: + return vin_match.group(1) + elif len(group) > 1 and not indexed: + # Multiple frames from one identified responder can omit line indexes. + joined = "".join(text for _sequence, text in fragments) + vin_match = re.search(r"([A-HJ-NPR-Z0-9]{17})", joined) + if vin_match: + return vin_match.group(1) + return None + + # Prefer a complete VIN from one responder. Headered functional responses + # are grouped by CAN ID by Elm327, so fragments from unrelated ECUs cannot + # be combined into a fabricated VIN. + groups = res.response_groups or ((None, payloads),) + for response_id, group in groups: + group_payloads = tuple(payload for payload in group if payload and payload[0] == 0x49 and + len(payload) > 1 and payload[1] == 0x02) + if not group_payloads: + continue + vin = complete_vin(group_payloads) + if vin: + return vin + + # Headerless adapters do not expose responder IDs. Only combine their + # indexed VIN fragments when every sequence number is unique; duplicate + # indexes indicate interleaved responders and are intentionally rejected. + headerless = [payload for response_id, group in groups if response_id is None for payload in group + if payload and payload[:2] == bytes([0x49, 0x02])] + if headerless: + indexed = [fragment(payload) for payload in headerless] + if all(sequence is not None for sequence, _text in indexed) and \ + len({sequence for sequence, _text in indexed}) == len(indexed): + joined = "".join(text for _sequence, text in sorted(indexed)) + vin_match = re.search(r"([A-HJ-NPR-Z0-9]{17})", joined) + if vin_match: + return vin_match.group(1) + return "" + + +# UDS (ISO 14229) Service Functions + +def _uds_payloads(response: DiagnosticResponse, service: int, *, minimum_length: int = 1) -> tuple[bytes, ...]: + """Return matching positive UDS responses while preserving responders.""" + expected_service = (service + 0x40) & 0xFF + payloads = tuple( + payload for payload in response.payloads + if len(payload) >= minimum_length and payload[0] == expected_service + ) + if not payloads: + if response.pending: + raise UdsResponsePendingError(service) + received = response.payload[0] if response.payload else None + raise ValueError(f"Unexpected UDS response service {received!r}; expected 0x{expected_service:02X}") + return payloads + +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 = _uds_payloads(res, SERVICE_TYPE.READ_DATA_BY_IDENTIFIER, minimum_length=3)[0] + + # Positive response: 0x62 + resp_did = (data[1] << 8) | data[2] + if resp_did != did: + raise ValueError(f"Unexpected UDS DID 0x{resp_did:04X}; expected 0x{did:04X}") + return data[3:] + + +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) + response = _uds_payloads(res, SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER, minimum_length=3)[0] + resp_did = (response[1] << 8) | response[2] + if resp_did != did: + raise ValueError(f"Unexpected UDS DID 0x{resp_did:04X}; expected 0x{did:04X}") + return response + + +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) + response = _uds_payloads(res, SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, minimum_length=2)[0] + if response[1] != (session_type & 0xFF): + raise ValueError(f"Unexpected UDS session 0x{response[1]:02X}; expected 0x{session_type & 0xFF:02X}") + return response + + +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) + response = _uds_payloads(res, SERVICE_TYPE.ECU_RESET, minimum_length=2)[0] + if response[1] != (reset_type & 0xFF): + raise ValueError(f"Unexpected UDS reset type 0x{response[1]:02X}; expected 0x{reset_type & 0xFF:02X}") + return response + + +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 [dtc for payload in _uds_payloads(res, SERVICE_TYPE.READ_DTC_INFORMATION, minimum_length=3) + for dtc in parse_uds_dtcs(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 _uds_payloads(res, SERVICE_TYPE.CLEAR_DIAGNOSTIC_INFORMATION)[0] + + +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) + response = _uds_payloads(res, SERVICE_TYPE.SECURITY_ACCESS, minimum_length=2)[0] + if response[1] != (access_type & 0xFF): + raise ValueError(f"Unexpected UDS access type 0x{response[1]:02X}; expected 0x{access_type & 0xFF:02X}") + return response + + +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) + response = _uds_payloads(res, SERVICE_TYPE.ROUTINE_CONTROL, minimum_length=4)[0] + if response[1:4] != bytes([routine_type & 0xFF, (routine_id >> 8) & 0xFF, routine_id & 0xFF]): + raise ValueError("UDS routine response does not match the requested routine") + return response + + +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) + response = _uds_payloads(res, SERVICE_TYPE.INPUT_OUTPUT_CONTROL_BY_IDENTIFIER, minimum_length=4)[0] + if response[1:3] != bytes([(did >> 8) & 0xFF, did & 0xFF]): + raise ValueError(f"Unexpected UDS DID in IO response for 0x{did:04X}") + return response + + +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) + response = _uds_payloads(res, SERVICE_TYPE.TESTER_PRESENT, minimum_length=2)[0] + if response[1] != (subfunction & 0xFF): + raise ValueError(f"Unexpected UDS tester-present subfunction 0x{response[1]:02X}") + return response + + +# 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: + # Unknown services are deliberately treated as mutating at the safety + # boundary. A raw request must opt into a known read-only service. + return service not in READ_ONLY_SERVICES + + +def is_read_only_payload(payload: bytes) -> bool: + if not payload: + return False + 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..33f4925b9 --- /dev/null +++ b/starpilot/system/obdyssey/elm327.py @@ -0,0 +1,590 @@ +from __future__ import annotations + +import re +import time +from dataclasses import dataclass +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 + can_auto_format: bool | None = None + + +@dataclass(frozen=True) +class DiagnosticResponse: + raw: bytes + payload: bytes + lines: tuple[str, ...] = () + service: int | None = None + responses: tuple[bytes, ...] = () + response_groups: tuple[tuple[int | None, tuple[bytes, ...]], ...] = () + pending: bool = False + + @property + def payloads(self) -> tuple[bytes, ...]: + """Return parsed responder payloads, including compatibility instances.""" + return self.responses or ((self.payload,) if self.payload else ()) + + +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, +} + +DEFAULT_PROTOCOL = "0" +DEFAULT_TIMEOUT = 0x32 +DEFAULT_FLOW_CONTROL = True +DEFAULT_CAN_AUTO_FORMAT = True + + +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) + # These are standard, but a number of inexpensive clones reject one or + # more of them. The parser already tolerates echo, spaces, and linefeeds; + # keep initialization usable when an optional convenience command is not + # implemented while still surfacing transport/bus failures. + for optional_cmd in ("ATE0", "ATL0", "ATS0", "ATR1"): + try: + self.command(optional_cmd, timeout=2.0) + except (ElmCommandError, ElmNoDataError, ElmTimeoutError): + pass + + # Identify and get voltage + identity_lines = self.command("ATI", timeout=2.0) + identity = " ".join(identity_lines).strip() + if not identity: + raise ElmUnsupportedError("Adapter did not return an identity for ATI") + + try: + voltage_lines = self.command("ATRV", timeout=2.0) + except (ElmCommandError, ElmNoDataError, ElmTimeoutError): + voltage_lines = [] + 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 (ElmCommandError, ElmNoDataError, ElmTimeoutError): + 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 _read_until_prompt(self, timeout: float, cmd_clean: str, *, require_prompt: bool = True) -> bytes: + deadline = time.monotonic() + timeout + buffer = bytearray() + while time.monotonic() < deadline: + 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: + continue + except Exception as err: + raise ElmError(f"Read failed for {cmd_clean}: {err}") from err + + if require_prompt and b">" not in buffer: + raise ElmTimeoutError(f"Timeout waiting for ELM prompt for '{cmd_clean}' (received: {bytes(buffer)!r})") + return bytes(buffer) + + @staticmethod + def _clean_lines(raw_text: str, cmd_clean: str = "") -> list[str]: + prompt_idx = raw_text.rfind(">") + if prompt_idx != -1: + raw_text = raw_text[:prompt_idx] + + raw_lines = [line.strip() for line in re.split(r"[\r\n]+", raw_text) if line.strip()] + cleaned_lines: list[str] = [] + cmd_norm = cmd_clean.replace(" ", "").upper() + + for line in raw_lines: + normalized = line.replace(" ", "").upper() + + # Strip command echo if clone still echoed it + if cmd_norm and (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 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 + buffer = self._read_until_prompt(timeout_val, cmd_clean) + return self._clean_lines(buffer.decode("ascii", errors="ignore"), cmd_clean) + + def apply_context(self, context: ElmContext) -> None: + if self.active_context == context: + return + + prev = self.active_context or ElmContext() + + # 1. Protocol change. ATSP0 restores automatic protocol selection. + if context.protocol != prev.protocol: + proto_key = str(context.protocol or DEFAULT_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: + if context.tx_header is None: + self.command("ATSH") + elif 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. CRA is sticky; an empty CRA restores the + # adapter's default receive filters for functional requests. + if context.rx_filter != prev.rx_filter: + if context.rx_filter is None: + self.command("ATCRA") + elif 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: + self.command("ATCEA") + + # Tester address is also sticky on adapters that support ISO 15765 + # addressing. Empty ATTA restores the default address. + if context.tester_address != prev.tester_address: + if context.tester_address is None: + self.command("ATTA") + else: + self.command(f"ATTA{context.tester_address:02X}") + + # 5. CAN formatting and flow-control changes are separate ELM settings. + if context.can_auto_format != prev.can_auto_format: + can_auto_format = DEFAULT_CAN_AUTO_FORMAT if context.can_auto_format is None else context.can_auto_format + self.command("ATCAF1" if can_auto_format else "ATCAF0") + if context.flow_control != prev.flow_control: + flow_control = DEFAULT_FLOW_CONTROL if context.flow_control is None else context.flow_control + self.command("ATCFC1" if flow_control else "ATCFC0") + + # 6. Timeout change (ATST in 4ms increments: 0-FF) + if context.timeout != prev.timeout: + st_val = DEFAULT_TIMEOUT if context.timeout is None else max(0, min(255, int(context.timeout))) + self.command(f"ATST{st_val:02X}") + + self.active_context = context + + @staticmethod + def _parse_response_line(line: str) -> tuple[int | None, int | None, bytes] | None: + """Parse one ELM hex line, optionally returning CAN and frame indexes.""" + text = line.strip() + frame_index: int | None = None + response_id: int | None = None + index_match = re.match(r"^([0-9A-Fa-f]+):\s*(.*)$", text) + if index_match: + prefix = index_match.group(1) + # ELM clones commonly print CAN headers as ``7E8: ...`` while a few + # print ISO-TP frame indexes as ``0: ...``. Do not mistake a 3/8-digit + # CAN identifier for a frame index. + if len(prefix) in (3, 8): + response_id = int(prefix, 16) + elif len(prefix) <= 2: + frame_index = int(prefix, 16) + else: + return None + text = index_match.group(2) + + tokens = text.split() + if not tokens: + return None + if len(tokens) == 1: + compact = tokens[0].replace(" ", "") + if not re.fullmatch(r"[0-9A-Fa-f]+", compact): + return None + # With spaces disabled, an 11-bit CAN header is sometimes prefixed to + # the frame as three nibbles (for example ``7E804410C...``). Its odd + # length makes the boundary unambiguous. + if response_id is None and len(compact) % 2 and len(compact) >= 5: + response_id = int(compact[:3], 16) + compact = compact[3:] + if len(compact) % 2: + return None + return response_id, frame_index, bytes.fromhex(compact) + if not all(re.fullmatch(r"[0-9A-Fa-f]+", token) for token in tokens): + return None + + if response_id is None and len(tokens[0]) in (3, 8): + response_id = int(tokens.pop(0), 16) + + if not all(len(token) % 2 == 0 for token in tokens): + return None + data = bytes.fromhex("".join(tokens)) + + # With headers enabled, some adapters include the CAN DLC after the ID. + # Remove it only when it exactly describes the remaining frame bytes; an + # ISO-TP first-frame byte (0x10..0x1F) must remain intact. + if response_id is not None and data and data[0] <= 8 and data[0] == len(data) - 1: + data = data[1:] + return response_id, frame_index, data + + @classmethod + def _parse_response_groups(cls, lines: list[str]) -> tuple[tuple[int | None, bytes], ...]: + """Keep independent ELM response lines separate and join ISO-TP frames.""" + responses: list[tuple[int | None, bytes]] = [] + indexed: bytearray | None = None + indexed_next: int | None = None + indexed_iso_id = -1 + # Keep one ISO-TP assembly buffer per response ID. Functional requests + # can produce interleaved multi-frame responses from more than one ECU. + iso_payloads: dict[int | None, tuple[bytearray, int]] = {} + iso_order: list[int | None] = [] + + def finish_indexed() -> None: + nonlocal indexed, indexed_next + if indexed: + responses.append((None, bytes(indexed))) + indexed = None + indexed_next = None + + def finish_iso(response_id: int | None) -> None: + state = iso_payloads.pop(response_id, None) + if state is None: + return + payload, expected = state + if payload: + responses.append((response_id, bytes(payload[:expected]))) + iso_order.remove(response_id) + + def finish_all_iso() -> None: + for response_id in tuple(iso_order): + finish_iso(response_id) + + for line in lines: + parsed = cls._parse_response_line(line) + if parsed is None: + continue + response_id, frame_index, data = parsed + if not data: + continue + + if frame_index is not None: + # Some clones prefix each ISO-TP frame with a local frame index rather + # than a CAN response ID. Treat those frames like the headerless + # stream below while retaining support for ordinary indexed payload + # fragments used by older adapters. + if data[0] >> 4 == 1 and len(data) >= 2: + finish_indexed() + finish_iso(indexed_iso_id) + expected = ((data[0] & 0x0F) << 8) | data[1] + payload = bytearray(data[2:]) + if len(payload) >= expected: + responses.append((None, bytes(payload[:expected]))) + else: + iso_payloads[indexed_iso_id] = (payload, expected) + iso_order.append(indexed_iso_id) + indexed_next = frame_index + 1 + continue + if data[0] >> 4 == 2 and indexed_iso_id in iso_payloads and indexed_next == frame_index: + finish_indexed() + payload, expected = iso_payloads[indexed_iso_id] + payload.extend(data[1:]) + if len(payload) >= expected: + finish_iso(indexed_iso_id) + indexed_next = frame_index + 1 + continue + if data[0] >> 4 == 0 and data[0] < len(data): + finish_indexed() + finish_iso(indexed_iso_id) + responses.append((response_id, bytes(data[1:1 + data[0]]))) + indexed_next = frame_index + 1 + continue + + finish_iso(indexed_iso_id) + if frame_index == 0 or indexed is None or indexed_next != frame_index: + finish_indexed() + indexed = bytearray(data) + else: + indexed.extend(data) + indexed_next = frame_index + 1 + continue + + finish_indexed() + if data[0] >> 4 == 1 and len(data) >= 2: + finish_iso(response_id) + expected = ((data[0] & 0x0F) << 8) | data[1] + payload = bytearray(data[2:]) + if len(payload) >= expected: + responses.append((response_id, bytes(payload[:expected]))) + else: + iso_payloads[response_id] = (payload, expected) + iso_order.append(response_id) + elif data[0] >> 4 == 2: + # A headerless clone has only one possible ISO-TP stream. For + # headered output, match the consecutive frame to its response ID; + # if exactly one stream is active, use it as a compatibility fallback. + stream_id = response_id if response_id in iso_payloads else None + if stream_id is None and response_id is None and None in iso_payloads: + stream_id = None + elif stream_id is None and len(iso_payloads) == 1: + stream_id = next(iter(iso_payloads)) + + if stream_id in iso_payloads: + payload, expected = iso_payloads[stream_id] + payload.extend(data[1:]) + if len(payload) >= expected: + finish_iso(stream_id) + else: + responses.append((response_id, data)) + elif data[0] >> 4 == 0 and data[0] < len(data): + # Raw ISO-TP single frames are occasionally exposed by cheap clones. + responses.append((response_id, bytes(data[1:1 + data[0]]))) + else: + responses.append((response_id, data)) + + finish_indexed() + finish_all_iso() + return tuple(responses) + + @classmethod + def _parse_responses(cls, lines: list[str]) -> tuple[bytes, ...]: + """Compatibility view containing only parsed responder payloads.""" + return tuple(payload for _response_id, payload in cls._parse_response_groups(lines)) + + def _read_pending_lines(self, timeout: float) -> list[str]: + if timeout <= 0: + return [] + buffer = self._read_until_prompt(timeout, "pending response", require_prompt=False) + if not buffer: + return [] + return self._clean_lines(buffer.decode("ascii", errors="ignore")) + + def _make_diagnostic_response(self, payload: bytes, lines: list[str]) -> DiagnosticResponse: + parsed_responses = self._parse_response_groups(lines) + responses = tuple(response for _response_id, response in parsed_responses) + response_groups: list[tuple[int | None, tuple[bytes, ...]]] = [] + for response_id, response in parsed_responses: + if response_id is None: + response_groups.append((None, (response,))) + continue + for index, (group_id, group_payloads) in enumerate(response_groups): + if group_id == response_id: + response_groups[index] = (group_id, group_payloads + (response,)) + break + else: + response_groups.append((response_id, (response,))) + + expected_service = (payload[0] + 0x40) & 0xFF + positive_responses = tuple(response for response in responses if response and response[0] == expected_service) + negative_responses = tuple(response for response in responses if len(response) >= 3 and response[0] == 0x7F and + response[1] == payload[0]) + pending_responses = tuple(response for response in negative_responses if response[2] == 0x78) + + # A functional request can have a negative response from one ECU and a + # valid positive response from another. Prefer the matching positive + # response and only raise when no responder succeeded. + selected = positive_responses[0] if positive_responses else (responses[0] if responses else b"") + if not positive_responses and negative_responses: + non_pending = tuple(response for response in negative_responses if response[2] != 0x78) + if non_pending: + raw_bytes = non_pending[0] + 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=b"".join(responses), + payload=selected, + lines=tuple(lines), + service=selected[0] if selected else None, + responses=responses, + response_groups=tuple(response_groups), + pending=bool(pending_responses and not positive_responses), + ) + + def request(self, payload: bytes, context: ElmContext | None = None, *, timeout: float | None = None, + retry: bool = True, pending_timeout: float | None = None) -> DiagnosticResponse: + # ``retry`` remains accepted for API compatibility with the prototype, but + # reconnect ownership belongs to OBDysseyController. Replaying a request + # here would make write safety and connection state ambiguous. + del retry + if not payload: + raise ValueError("Diagnostic payload must not be empty") + if context is not None: + self.apply_context(context) + + payload_hex = payload.hex().upper() + lines = self.command(payload_hex, timeout=timeout) + response = self._make_diagnostic_response(payload, lines) + if response.pending: + # A compliant ECU can emit the final response after the first ELM + # prompt. Read the existing stream before the controller considers a + # read-only retry; no diagnostic command is replayed here. + late_timeout = self.default_timeout if pending_timeout is None else pending_timeout + late_lines = self._read_pending_lines(late_timeout) + if late_lines: + lines.extend(late_lines) + response = self._make_diagnostic_response(payload, lines) + return response diff --git a/starpilot/system/obdyssey/obdb.py b/starpilot/system/obdyssey/obdb.py new file mode 100644 index 000000000..b9322206d --- /dev/null +++ b/starpilot/system/obdyssey/obdb.py @@ -0,0 +1,339 @@ +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 _parse_bool(value: Any, default: bool = False) -> bool: + if value is None: + return default + if isinstance(value, str): + normalized = value.strip().lower() + if normalized in {"1", "true", "yes", "on"}: + return True + if normalized in {"0", "false", "no", "off"}: + return False + return bool(value) + + +def _first_present(data: dict[str, Any], *keys: str) -> Any: + """Return the first explicitly supplied value, including numeric zero.""" + for key in keys: + if key in data and data[key] not in (None, ""): + return data[key] + return None + + +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(_first_present(cmd_data, "hdr", "header", "tx_header")) + rax = _parse_int(_first_present(cmd_data, "rax", "receive_filter", "rx_filter")) + eax = _parse_int(_first_present(cmd_data, "eax", "extended_address")) + pri = _parse_int(_first_present(cmd_data, "pri", "priority")) + tst = _parse_int(_first_present(cmd_data, "tst", "tester_address")) + tmo = _parse_int(_first_present(cmd_data, "tmo", "timeout")) + fcm1 = _parse_bool(cmd_data["fcm1"]) if "fcm1" in cmd_data else None + can_auto_format = _parse_bool(cmd_data["caf"], default=True) if "caf" in cmd_data else True + 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, + can_auto_format=can_auto_format, + ) + + service_int = _parse_int(cmd_data.get("service", 1)) or 1 + param_bytes = _parse_bytes(_first_present(cmd_data, "pid", "parameter")) + 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..4477931d1 --- /dev/null +++ b/starpilot/system/obdyssey/profiles.py @@ -0,0 +1,413 @@ +from __future__ import annotations + +import json +import os +import shutil +import tempfile +import urllib.request +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" +# Repository-local corrections are explicit overrides of upstream OBDb data. +# Keep the old constant as a compatibility alias for callers of the prototype. +PROFILE_OVERRIDES_DIR = Path(__file__).resolve().parent / "curated_profiles" +CURATED_PROFILES_DIR = PROFILE_OVERRIDES_DIR +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", + "CHEVROLET_BOLT_CC_2017": "Chevrolet-Bolt-EV", + "CHEVROLET_BOLT_CC_2018_2021": "Chevrolet-Bolt-EV", + "CHEVROLET_BOLT_CC_2019_2021": "Chevrolet-Bolt-EV", + "CHEVROLET_BOLT_ACC_2022_2023": "Chevrolet-Bolt-EV", + "CHEVROLET_BOLT_ACC_2022_2023_PEDAL": "Chevrolet-Bolt-EV", + "CHEVROLET_BOLT_CC_2022_2023": "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 + # Keep the ID and the identity it was selected for in one immutable value + # so readers cannot observe a partially updated session override. + self._selected_profile: tuple[str, tuple[str, str]] | None = None + self._ensure_storage() + + @staticmethod + def _valid_profile_id(profile_id: str) -> bool: + return bool(profile_id) and profile_id not in {".", ".."} and "\x00" not in profile_id and Path(profile_id).name == profile_id + + @classmethod + def _require_profile_id(cls, profile_id: str) -> str: + if not cls._valid_profile_id(profile_id): + raise ValueError(f"Invalid profile id: {profile_id!r}") + return profile_id + + def _ensure_storage(self) -> None: + try: + self.data_dir.mkdir(parents=True, exist_ok=True) + except Exception: + pass + + def _param_text(self, key: str) -> str: + try: + value = self.params.get(key, encoding="utf-8") + except TypeError: + value = self.params.get(key) + if isinstance(value, bytes): + return value.decode("utf-8", errors="replace") + return str(value or "") + + def _vehicle_identity(self) -> tuple[str, str]: + """Return the persisted vehicle identity used to scope a profile override.""" + return ( + self._param_text("CarMake").upper().strip(), + self._param_text("CarModel").upper().strip(), + ) + + 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 self._valid_profile_id(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.""" + self._require_profile_id(profile_id) + self._validate_profile_data(data) + profile = parse_obdb_profile(data, profile_id=profile_id) + if not profile.commands: + raise ValueError(f"Profile {profile_id!r} must contain at least one diagnostic command") + self._ensure_storage() + + target_dir = self.data_dir / profile_id + temp_dir = Path(tempfile.mkdtemp(prefix="obd_prof_", dir=self.data_dir)) + profile_path = target_dir / "profile.json" + metadata_path = target_dir / "metadata.json" + old_profile = profile_path.read_bytes() if profile_path.is_file() else None + old_metadata = metadata_path.read_bytes() if metadata_path.is_file() else None + try: + with open(temp_dir / "profile.json", "w", encoding="utf-8") as f: + json.dump(data, f, indent=2) + # Always stage metadata. An empty object deliberately clears a stale + # update source when a profile is replaced from inline data. + with open(temp_dir / "metadata.json", "w", encoding="utf-8") as f: + json.dump(metadata or {}, f, indent=2) + + # Replace files individually. os.replace() is atomic for each file and + # leaves the previous profile in place until the candidate is ready; + # deleting the whole target directory first could lose the last good + # profile if the process were interrupted between those operations. + target_dir.mkdir(parents=True, exist_ok=True) + # Replace metadata first so a failure cannot leave a new profile paired + # with an old source descriptor. The profile JSON remains last-known- + # good until its own atomic replacement succeeds. + os.replace(temp_dir / "metadata.json", target_dir / "metadata.json") + os.replace(temp_dir / "profile.json", target_dir / "profile.json") + self._cached_profiles[profile_id] = profile + return profile + except Exception: + # A profile update has two files because update provenance is kept next + # to the normalized data. Restore both originals if either replacement + # fails so a last-known-good profile is never paired with new metadata. + try: + if old_profile is None: + profile_path.unlink(missing_ok=True) + else: + profile_path.write_bytes(old_profile) + if old_metadata is None: + metadata_path.unlink(missing_ok=True) + else: + metadata_path.write_bytes(old_metadata) + except Exception as restore_err: + cloudlog.error(f"Could not restore profile {profile_id!r} after failed update: {restore_err}") + raise + finally: + if temp_dir.exists(): + shutil.rmtree(temp_dir, ignore_errors=True) + + @staticmethod + def _validate_profile_data(data: dict[str, Any]) -> None: + """Check the required OBDb command/signal shape before touching storage.""" + if not isinstance(data, dict): + raise ValueError("Profile data must be a JSON object") + + raw_commands = data.get("commands") + if raw_commands is None: + raw_commands = data.get("pids") + if not isinstance(raw_commands, list) or not raw_commands: + raise ValueError("Profile must contain a non-empty commands or pids list") + + for index, command in enumerate(raw_commands): + if not isinstance(command, dict): + raise ValueError(f"Profile command {index} must be an object") + if "signals" in command and not isinstance(command["signals"], list): + raise ValueError(f"Profile command {index} signals must be a list") + for signal_index, signal in enumerate(command.get("signals", [])): + if not isinstance(signal, dict) or not str(signal.get("id", "")).strip(): + raise ValueError(f"Profile command {index} signal {signal_index} must have an id") + + raw_signals = data.get("signals", []) + if not isinstance(raw_signals, list): + raise ValueError("Profile signals must be a list") + for index, signal in enumerate(raw_signals): + if not isinstance(signal, dict) or not str(signal.get("id", "")).strip(): + raise ValueError(f"Profile signal {index} must have an id") + + @staticmethod + def _load_profile_source(source: str) -> dict[str, Any]: + """Load a JSON profile from a local file/directory or an explicit URL.""" + source = source.strip() + if not source: + raise ValueError("A profile source is required") + + if source.startswith(("http://", "https://")): + request = urllib.request.Request(source, headers={"User-Agent": "StarPilot-OBDyssey/1"}) + with urllib.request.urlopen(request, timeout=30.0) as response: + raw = response.read(8 * 1024 * 1024 + 1) + if len(raw) > 8 * 1024 * 1024: + raise ValueError("Profile source is too large") + data = json.loads(raw.decode("utf-8")) + else: + source_path = Path(source) + if source_path.is_dir(): + source_path /= "profile.json" + with open(source_path, "r", encoding="utf-8") as f: + data = json.load(f) + + if not isinstance(data, dict): + raise ValueError("Profile source must contain a JSON object") + return data + + def install_profile_source(self, source: str, profile_id: str = "", provider: str = "", + metadata: dict[str, Any] | None = None) -> VehicleProfile: + """Fetch, validate, and install a profile while retaining its update source.""" + data = self._load_profile_source(source) + source_metadata = dict(metadata or {}) + source_metadata["source"] = source + if provider: + source_metadata.setdefault("provider", provider) + + raw_meta = data.get("metadata") + if not profile_id and isinstance(raw_meta, dict): + profile_id = str(raw_meta.get("id", "")) + if not profile_id: + raise ValueError("Profile source does not define a profile id") + return self.install_profile_data(profile_id, data, source_metadata) + + def update_profile(self, profile_id: str) -> VehicleProfile: + """Refresh an installed profile from the source recorded at installation.""" + self._require_profile_id(profile_id) + metadata_path = self.data_dir / profile_id / "metadata.json" + if not metadata_path.is_file(): + raise RuntimeError(f"Profile {profile_id!r} has no update source") + try: + with open(metadata_path, "r", encoding="utf-8") as f: + metadata = json.load(f) + except (OSError, json.JSONDecodeError) as err: + raise RuntimeError(f"Profile {profile_id!r} metadata is invalid") from err + if not isinstance(metadata, dict) or not isinstance(metadata.get("source"), str): + raise RuntimeError(f"Profile {profile_id!r} has no update source") + return self.install_profile_source(metadata["source"], profile_id=profile_id, + provider=str(metadata.get("provider", "")), metadata=metadata) + + def remove_profile(self, profile_id: str) -> bool: + self._require_profile_id(profile_id) + 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 self._selected_profile is not None and self._selected_profile[0] == profile_id: + self._selected_profile = None + self._active_profile = None + elif self._active_profile is not None and self._active_profile.id == profile_id: + self._active_profile = None + if target_dir.exists(): + shutil.rmtree(target_dir, ignore_errors=True) + return True + return False + + def select_profile(self, profile_id: str) -> VehicleProfile: + """Select a profile for this daemon session without changing persistent settings.""" + prof = self.get_profile(profile_id) + if prof is None: + raise KeyError(f"Profile {profile_id} not found") + self._selected_profile = (profile_id, self._vehicle_identity()) + self._active_profile = prof + return prof + + def resolve_active_profile(self, explicit_id: str | None = None) -> VehicleProfile: + """Resolve an explicit/session profile, then exact vehicle mapping, then SAE J1979.""" + if explicit_id: + prof = self.get_profile(explicit_id) + if prof is not None: + self._active_profile = prof + return prof + + # An explicitly requested but unavailable profile must never silently + # resolve to a different OEM profile. + cloudlog.warning(f"OBDyssey profile {explicit_id!r} is unavailable; using SAE J1979") + prof = self.load_bundled_saej1979() + self._active_profile = prof + return prof + + vehicle_identity = self._vehicle_identity() + selection = self._selected_profile + if selection is not None: + selected_id, selected_vehicle = selection + if selected_vehicle == vehicle_identity: + prof = self.get_profile(selected_id) + if prof is not None: + self._active_profile = prof + return prof + cloudlog.warning( + f"Selected OBDyssey profile {selected_id!r} is unavailable; deriving a profile" + ) + else: + cloudlog.info("OBDyssey vehicle settings changed; clearing session profile selection") + self._selected_profile = None + + # Check exact StarPilot fingerprint/model values only. Fuzzy OEM matches + # can select the wrong diagnostic headers and are unsafe. + car_make, car_model = vehicle_identity + exact_keys = [car_model] + if car_make and car_model: + exact_keys.append(f"{car_make} {car_model}") + mapped_id = next((VEHICLE_PROFILE_MAPPINGS[key] for key in exact_keys if key in VEHICLE_PROFILE_MAPPINGS), None) + + 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..f70b5acd8 --- /dev/null +++ b/starpilot/system/obdyssey/protocol.py @@ -0,0 +1,228 @@ +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_pending_dtcs": 15.0, + "read_permanent_dtcs": 15.0, + "read_all_dtcs": 30.0, + "read_freeze_frame": 10.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, address: str = "") -> dict[str, Any]: + return self.call("connect", **({"address": address} if address else {})) + + 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_pending_dtcs(self) -> list[dict[str, Any]]: + res = self.call("read_pending_dtcs") + return res.get("dtcs", []) + + def read_permanent_dtcs(self) -> list[dict[str, Any]]: + res = self.call("read_permanent_dtcs") + return res.get("dtcs", []) + + def read_all_dtcs(self) -> list[dict[str, Any]]: + res = self.call("read_all_dtcs") + return res.get("dtcs", []) + + def read_freeze_frame(self, pid: int, frame: int = 0) -> dict[str, Any]: + return self.call("read_freeze_frame", pid=pid, frame=frame) + + 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 = "", profile_id: str = "", + data: dict[str, Any] | None = None, metadata: dict[str, Any] | None = None) -> dict[str, Any]: + kwargs: dict[str, Any] = {"provider": provider, "repository": repository} + if profile_id: + kwargs["profile_id"] = profile_id + if data is not None: + kwargs["data"] = data + if metadata is not None: + kwargs["metadata"] = metadata + return self.call("install_profile", **kwargs) + + def update_profile(self, profile_id: str = "") -> dict[str, Any]: + return self.call("update_profile", **({"profile_id": profile_id} if profile_id else {})) + + 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..510269394 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_daemon.py @@ -0,0 +1,546 @@ +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_explicit_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_daemon_is_inert_until_explicit_connect(tmp_path): + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + bt_client = FakeBluetoothClient() + factory_calls: list[str] = [] + + def transport_factory(address): + factory_calls.append(address) + return FakeElmTransport() + + controller = OBDysseyController( + params=params, + transport_factory=transport_factory, + profile_manager=prof_mgr, + bluetooth_client=bt_client, + ) + + controller.reconnect_step() + + status = controller.status() + assert status["enabled"] is True + assert status["state"] == "idle" + assert status["connected"] is False + assert factory_calls == [] + + +def test_adapter_selection_is_session_only_and_not_reconnected_after_restart(tmp_path): + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + first_transport = FakeElmTransport() + first_controller = OBDysseyController( + params=params, + transport_factory=lambda _addr: first_transport, + profile_manager=ProfileManager(data_dir=tmp_path / "profiles", params=params), + bluetooth_client=FakeBluetoothClient(), + ) + assert first_controller.connect_adapter("AA:BB:CC:DD:EE:FF") + + reconnect_calls: list[str] = [] + second_controller = OBDysseyController( + params=params, + transport_factory=lambda address: reconnect_calls.append(address) or FakeElmTransport(), + profile_manager=ProfileManager(data_dir=tmp_path / "profiles", params=params), + bluetooth_client=FakeBluetoothClient(), + ) + second_controller.reconnect_step() + + assert reconnect_calls == [] + assert second_controller.status()["adapter_address"] == "" + + +def test_no_address_connect_refuses_ambiguous_adapter_selection(tmp_path): + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + bt_client = FakeBluetoothClient(devices=[ + BluetoothDevice("AA:BB:CC:DD:EE:FF", "OBDLink MX+", paired=True, trusted=True, serial=True), + BluetoothDevice("11:22:33:44:55:66", "Vgate iCar", paired=True, trusted=True, serial=True), + ]) + factory_calls: list[str] = [] + + def transport_factory(address): + factory_calls.append(address) + return FakeElmTransport() + + controller = OBDysseyController( + params=params, + transport_factory=transport_factory, + profile_manager=prof_mgr, + bluetooth_client=bt_client, + ) + + assert controller.connect_adapter() is False + + status = controller.status() + assert status["state"] == "error" + assert "select" in status["last_error"].lower() + assert factory_calls == [] + + +def test_diagnostic_request_does_not_probe_without_active_adapter(tmp_path): + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + factory_calls: list[str] = [] + + def transport_factory(address): + factory_calls.append(address) + return FakeElmTransport() + + controller = OBDysseyController( + params=params, + transport_factory=transport_factory, + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + ) + + with pytest.raises(RuntimeError, match="active OBD adapter session"): + controller.handle({"command": "read_signal", "id": "SAE_ENGINE_RPM"}) + + assert factory_calls == [] + + +def test_bluetooth_disable_tears_down_active_diagnostic_link(tmp_path): + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + transport = FakeElmTransport() + factory_calls: list[str] = [] + + def transport_factory(address): + factory_calls.append(address) + return transport + + controller = OBDysseyController( + params=params, + transport_factory=transport_factory, + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + ) + assert controller.connect_adapter("AA:BB:CC:DD:EE:FF") + + params.values["BluetoothEnabled"] = False + controller.reconnect_step() + + assert controller.status()["state"] == "idle" + assert controller.status()["link_connected"] is False + assert transport.connected is False + + params.values["BluetoothEnabled"] = True + controller.reconnect_step() + assert factory_calls == ["AA:BB:CC:DD:EE:FF"] + + +def test_profile_selection_is_session_only_and_tracks_vehicle_identity(tmp_path): + params = FakeParams(BluetoothEnabled=True, IsOffroad=True, + CarMake="Toyota", CarModel="RAV4") + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + controller = OBDysseyController( + params=params, + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + ) + + result = controller.handle({ + "command": "select_profile", + "profile_id": "Chevrolet-Bolt-EV", + "model_year": 2023, + }) + + assert result["active_profile"] == "Chevrolet-Bolt-EV" + assert not any(key.startswith("OBDyssey") for key in params.values) + + params.values["CarMake"] = "Subaru" + params.values["CarModel"] = "Forester" + assert controller.status()["profile"] == "saej1979" + + +def test_read_request_reconnects_once_in_controller(tmp_path): + class DisconnectingReadTransport(FakeElmTransport): + def __init__(self, disconnect_on_read: bool): + super().__init__(default_responses={"010C": "41 0C 1F 40\r\n>"}) + self.disconnect_on_read = disconnect_on_read + + def write(self, data): + if self.disconnect_on_read and data.decode("ascii").strip().upper() == "010C": + self.disconnect_on_read = False + self.connected = False + raise ConnectionResetError("simulated read disconnect") + super().write(data) + + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + transports = [DisconnectingReadTransport(True), DisconnectingReadTransport(False)] + created: list[str] = [] + + def transport_factory(address): + created.append(address) + return transports.pop(0) + + controller = OBDysseyController( + params=params, + transport_factory=transport_factory, + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + ) + assert controller.connect_adapter("AA:BB:CC:DD:EE:FF") + assert not any(key.startswith("OBDyssey") for key in params.values) + + result = controller.handle({"command": "read_signal", "id": "SAE_ENGINE_RPM"}) + + assert result["signal"]["value"] == 2000 + assert created == ["AA:BB:CC:DD:EE:FF", "AA:BB:CC:DD:EE:FF"] + + +def test_mutating_disconnect_is_not_replayed(tmp_path): + class DisconnectingClearTransport(FakeElmTransport): + def __init__(self): + super().__init__(default_responses={"04": "44\r\n>"}) + self.clear_attempts = 0 + + def write(self, data): + if data.decode("ascii").strip().upper() == "04": + self.clear_attempts += 1 + self.connected = False + raise ConnectionResetError("ambiguous clear disconnect") + super().write(data) + + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + transport = DisconnectingClearTransport() + controller = OBDysseyController( + params=params, + transport_factory=lambda _addr: transport, + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + ) + assert controller.connect_adapter("AA:BB:CC:DD:EE:FF") + + with pytest.raises(Exception, match="disconnect"): + controller.handle({"command": "clear_dtcs"}) + + assert transport.clear_attempts == 1 + + +def test_read_response_pending_is_retried_by_controller(tmp_path): + class PendingThenSuccessTransport(FakeElmTransport): + def __init__(self): + super().__init__(default_responses={"22F190": "7F 22 78\r\n>"}) + self.read_attempts = 0 + + def write(self, data): + if data.decode("ascii").strip().upper() == "22F190": + self.read_attempts += 1 + self.default_responses["22F190"] = ( + "7F 22 78\r\n>" if self.read_attempts == 1 else "62 F1 90 31 47 31 46\r\n>" + ) + super().write(data) + + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + transport = PendingThenSuccessTransport() + controller = OBDysseyController( + params=params, + transport_factory=lambda _addr: transport, + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + sleep=lambda _delay: None, + ) + assert controller.connect_adapter("AA:BB:CC:DD:EE:FF") + + result = controller.handle({ + "command": "uds_request", + "tx_addr": "7E0", + "rx_addr": "7E8", + "payload": "22F190", + }) + + assert result["response"] == "62F19031473146" + assert transport.read_attempts == 2 + + +def test_mutating_response_pending_is_not_replayed(tmp_path): + class PendingWriteTransport(FakeElmTransport): + def __init__(self): + super().__init__(default_responses={"2EF19001": "7F 2E 78\r\n>"}) + self.write_attempts = 0 + + def write(self, data): + if data.decode("ascii").strip().upper() == "2EF19001": + self.write_attempts += 1 + super().write(data) + + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + transport = PendingWriteTransport() + controller = OBDysseyController( + params=params, + transport_factory=lambda _addr: transport, + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + ) + assert controller.connect_adapter("AA:BB:CC:DD:EE:FF") + + with pytest.raises(Exception, match="pending"): + controller.handle({ + "command": "uds_request", + "tx_addr": "7E0", + "rx_addr": "7E8", + "payload": "2EF19001", + }) + + assert transport.write_attempts == 1 + + +def test_uds_clear_requires_explicit_addresses(tmp_path): + params = FakeParams(BluetoothEnabled=True, IsOffroad=True) + prof_mgr = ProfileManager(data_dir=tmp_path / "profiles", params=params) + controller = OBDysseyController( + params=params, + transport_factory=lambda _addr: FakeElmTransport(), + profile_manager=prof_mgr, + bluetooth_client=FakeBluetoothClient(), + ) + + with pytest.raises(ValueError, match="tx_addr"): + controller.handle({"command": "clear_uds_dtcs"}) + + +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..231bc8f11 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_diagnostics.py @@ -0,0 +1,246 @@ +import pytest +from openpilot.starpilot.system.obdyssey.diagnostics import ( + DEFAULT_OBD_CONTEXT, + 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, ElmNoDataError +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_functional_obd_keeps_responses_from_multiple_ecus(): + transport = FakeElmTransport(default_responses={ + "010C": "41 0C 1F 40\r\n41 0C 20 00\r\n>", + }) + transport.connect() + elm = Elm327(transport) + + response = read_current_data(elm, 0x0C) + + assert DEFAULT_OBD_CONTEXT.rx_filter is None + assert response.responses == ( + bytes.fromhex("410C1F40"), + bytes.fromhex("410C2000"), + ) + assert response.payload == bytes.fromhex("410C1F40") + assert "ATCRA7E8" not in [w.decode("ascii").strip() for w in transport.writes] + + +def test_functional_dtc_reads_aggregate_multiple_ecus(): + transport = FakeElmTransport(default_responses={ + "03": "7E8 04 43 01 33 00\r\n7E9 04 43 03 00 00\r\n>", + }) + transport.connect() + elm = Elm327(transport) + + dtcs = read_stored_dtcs(elm) + + assert [dtc.code for dtc in dtcs] == ["P0133", "P0300"] + + +def test_standard_clear_dtc_does_not_fallback_to_broadcast_uds_clear(): + commands: list[str] = [] + + def handler(data: bytes) -> bytes: + command = data.decode("ascii").strip().upper() + commands.append(command) + return b"NO DATA\r\n>" if command == "04" else b"54\r\n>" + + transport = FakeElmTransport(handler=handler) + transport.connect() + elm = Elm327(transport) + + with pytest.raises(ElmNoDataError): + clear_dtcs(elm) + + assert "04" in commands + assert "14FFFFFF" not in commands + + +def test_vin_does_not_join_fragments_from_different_headered_responders(): + transport = FakeElmTransport(default_responses={ + "0902": ( + "7E8 09 49 02 01 31 47 31 46 58 36\r\n" + "7E9 09 49 02 02 53 30 35 48 34 31\r\n>" + ), + }) + transport.connect() + elm = Elm327(transport) + + assert read_vin(elm) == "" + + +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 + assert not is_read_only_payload(bytes([0x99])) # Unknown raw service is conservative + assert not is_read_only_payload(b"") + assert is_mutating_service(0x99) diff --git a/starpilot/system/obdyssey/tests/test_elm327.py b/starpilot/system/obdyssey/tests/test_elm327.py new file mode 100644 index 000000000..8a4110424 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_elm327.py @@ -0,0 +1,381 @@ +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_context_uses_can_flow_control_without_disabling_auto_format(): + transport = FakeElmTransport() + transport.connect() + elm = Elm327(transport) + + elm.apply_context(ElmContext(flow_control=True, can_auto_format=True)) + + writes = [w.decode("ascii").strip() for w in transport.writes] + assert "ATCFC1" in writes + assert "ATCAF1" in writes + assert "ATCAF0" not in writes + + +def test_elm327_context_clears_receive_filter_when_returning_to_functional_obd(): + transport = FakeElmTransport() + transport.connect() + elm = Elm327(transport) + + elm.apply_context(ElmContext(tx_header=0x7E4, rx_filter=0x7EC)) + transport.writes.clear() + + elm.apply_context(ElmContext(tx_header=0x7DF, rx_filter=None)) + + writes = [w.decode("ascii").strip() for w in transport.writes] + assert "ATSH7DF" in writes + assert "ATCRA" in writes + assert "ATCRA7EC" not in writes + + +def test_elm327_context_restores_sticky_settings_when_cleared(): + transport = FakeElmTransport() + transport.connect() + elm = Elm327(transport) + + elm.apply_context(ElmContext( + protocol="6", + tx_header=0x7E4, + rx_filter=0x7EC, + extended_address=0xF1, + timeout=0x10, + flow_control=False, + can_auto_format=False, + )) + transport.writes.clear() + + elm.apply_context(ElmContext(tx_header=0x7DF, can_auto_format=True, flow_control=True)) + + writes = [w.decode("ascii").strip() for w in transport.writes] + assert "ATSP0" in writes + assert "ATCRA" in writes + assert "ATCEA" in writes + assert "ATCAF1" in writes + assert "ATCFC1" in writes + assert "ATST32" in writes + + +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 + + +def test_elm327_request_does_not_own_transport_reconnects(): + class DisconnectOnceTransport(FakeElmTransport): + def __init__(self): + super().__init__(default_responses={"010C": "41 0C 1F 40\r\n>"}) + self.connect_calls = 0 + self.fail_next_request = True + + def connect(self): + self.connect_calls += 1 + super().connect() + + def write(self, data): + if self.fail_next_request and data.decode("ascii").strip().upper() == "010C": + self.fail_next_request = False + self.connected = False + raise ConnectionResetError("simulated disconnect") + super().write(data) + + transport = DisconnectOnceTransport() + transport.connect() + elm = Elm327(transport) + + with pytest.raises(ElmDisconnectedError): + elm.request(bytes.fromhex("010C"), retry=True) + + assert transport.connect_calls == 1 + + +def test_elm327_request_reassembles_headered_isotp_response(): + transport = FakeElmTransport(default_responses={ + "22F190": ( + "7E8 10 14 62 F1 90 31 47\r\n" + "7E8 21 31 46 58 36 53 30 35\r\n" + "7E8 22 48 34 31 30 30 30 30 30\r\n>" + ), + }) + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("22F190")) + + assert response.responses == (bytes.fromhex("62F1903147314658365330354834313030303030"),) + assert response.payload == response.responses[0] + + +def test_elm327_request_reassembles_colon_headered_isotp_response(): + transport = FakeElmTransport(default_responses={ + "22F190": ( + "7E8: 10 0C 62 F1 90 31 47 31\r\n" + "7E8: 21 46 58 36 53 30 35\r\n>" + ), + }) + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("22F190")) + + assert response.payload == bytes.fromhex("62F190314731465836533035") + + +def test_elm327_request_reassembles_indexed_isotp_response(): + transport = FakeElmTransport(default_responses={ + "22F190": ( + "0: 10 0C 62 F1 90 31 47 31\r\n" + "1: 21 46 58 36 53 30 35\r\n>" + ), + }) + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("22F190")) + + assert response.payload == bytes.fromhex("62F190314731465836533035") + + +def test_elm327_request_keeps_interleaved_functional_isotp_responders_separate(): + transport = FakeElmTransport(default_responses={ + "22F190": ( + "7E8 10 0A 62 F1 90 41 42 43\r\n" + "7E9 10 0A 62 F1 90 58 59 5A\r\n" + "7E8 21 44 45 46 47\r\n" + "7E9 21 5B 5C 5D 5E\r\n>" + ), + }) + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("22F190")) + + assert response.responses == ( + bytes.fromhex("62F19041424344454647"), + bytes.fromhex("62F19058595A5B5C5D5E"), + ) + + +def test_elm327_request_prefers_positive_responder_over_negative_responder(): + transport = FakeElmTransport(default_responses={ + "010C": "7E8 03 7F 01 78\r\n7E9 04 41 0C 1F 40\r\n>", + }) + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("010C")) + + assert response.payload == bytes.fromhex("410C1F40") + assert response.responses == (bytes.fromhex("7F0178"), bytes.fromhex("410C1F40")) + + +def test_elm327_request_accepts_response_pending_before_final_response(): + transport = FakeElmTransport(default_responses={ + "22F190": "7F 22 78\r\n62 F1 90 31 47 31 46\r\n>", + }) + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("22F190")) + + assert response.payload == bytes.fromhex("62F19031473146") + + +def test_elm327_request_reports_standalone_response_pending(): + transport = FakeElmTransport(default_responses={ + "22F190": "7F 22 78\r\n>", + }) + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("22F190"), pending_timeout=0.01) + + assert response.pending is True + assert response.payload == bytes.fromhex("7F2278") + + +def test_elm327_request_reads_final_response_after_pending_prompt(): + class LateResponseTransport(FakeElmTransport): + def __init__(self): + super().__init__(default_responses={"22F190": "7F 22 78\r\n>"}) + self._queue_late_response = False + self._late_response_queued = False + + def write(self, data): + super().write(data) + if data.decode("ascii").strip().upper() == "22F190": + self._queue_late_response = True + + def read(self, size=4096): + with self._lock: + should_queue = self._queue_late_response and not self._late_response_queued and not self._read_buffer + if should_queue: + self._late_response_queued = True + if should_queue: + self.queue_response(b"62 F1 90 31 47 31 46\r\n>") + return super().read(size) + + transport = LateResponseTransport() + transport.connect() + elm = Elm327(transport) + + response = elm.request(bytes.fromhex("22F190"), pending_timeout=0.1) + + assert response.pending is False + assert response.payload == bytes.fromhex("62F19031473146") + assert transport.writes.count(b"22F190\r") == 1 diff --git a/starpilot/system/obdyssey/tests/test_obdb.py b/starpilot/system/obdyssey/tests/test_obdb.py new file mode 100644 index 000000000..852a120ae --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_obdb.py @@ -0,0 +1,151 @@ +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, + "fcm1": "0", + "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.context.flow_control is False + assert cmd.context.can_auto_format is True + 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..37e0dde26 --- /dev/null +++ b/starpilot/system/obdyssey/tests/test_profiles.py @@ -0,0 +1,220 @@ +import json + +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 + assert all(command.context.rx_filter is None for command in profile.commands) + + +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_vehicle_settings(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_session_profile_override_is_not_persisted_and_resets_on_vehicle_change(tmp_path): + params = FakeParams(CarMake="Subaru", CarModel="Forester") + manager = ProfileManager(data_dir=tmp_path, params=params) + + manager.select_profile("saej1979") + assert manager.resolve_active_profile().id == "saej1979" + + params.values["CarMake"] = "Chevrolet" + params.values["CarModel"] = "BOLT EV" + assert manager.resolve_active_profile().id == "Chevrolet-Bolt-EV" + + # A new manager has no access to the old process-local selection. + new_manager = ProfileManager(data_dir=tmp_path, params=FakeParams( + CarMake="Subaru", CarModel="Forester", + )) + assert new_manager.resolve_active_profile().id == "saej1979" + + +def test_invalid_explicit_profile_does_not_fuzzy_match_an_oem_profile(tmp_path): + params = FakeParams(CarMake="Chevrolet", CarModel="BOLT EV") + manager = ProfileManager(data_dir=tmp_path, params=params) + + assert manager.resolve_active_profile(explicit_id="not-a-real-profile").id == "saej1979" + + +def test_group_signals_by_command(tmp_path): + manager = ProfileManager(data_dir=tmp_path) + custom_data = { + "metadata": {"name": "Multi-Signal Profile"}, + "commands": [ + { + "id": "CMD_MULTI_1", + "service": 1, + "pid": "01", + "signals": [ + {"id": "SIG_A", "name": "Signal A"}, + {"id": "SIG_B", "name": "Signal B"}, + ] + }, + { + "id": "CMD_MULTI_2", + "service": 1, + "pid": "02", + "signals": [ + {"id": "SIG_C", "name": "Signal C"}, + ] + } + ] + } + profile = manager.install_profile_data("multi_test", custom_data) + requested = ["SIG_A", "SIG_B", "SIG_C"] + grouped = ProfileManager.group_signals_by_command(profile, requested) + + assert len(grouped) == 2 + assert grouped[0][0].id == "CMD_MULTI_1" + assert len(grouped[0][1]) == 2 + assert [s.id for s in grouped[0][1]] == ["SIG_A", "SIG_B"] + assert grouped[1][0].id == "CMD_MULTI_2" + assert len(grouped[1][1]) == 1 + assert [s.id for s in grouped[1][1]] == ["SIG_C"] + + +def test_profile_install_failure_preserves_last_known_good(tmp_path, monkeypatch): + manager = ProfileManager(data_dir=tmp_path) + old_data = { + "metadata": {"name": "Old profile"}, + "commands": [{"id": "OLD", "service": 1, "pid": "0C", "signals": [{"id": "OLD_RPM"}]}], + } + manager.install_profile_data("vehicle", old_data) + old_bytes = (tmp_path / "vehicle" / "profile.json").read_bytes() + + def fail_replace(_source, _target): + raise OSError("simulated interrupted profile replacement") + + monkeypatch.setattr("openpilot.starpilot.system.obdyssey.profiles.os.replace", fail_replace) + + with pytest.raises(OSError, match="replacement"): + manager.install_profile_data("vehicle", { + "metadata": {"name": "New profile"}, + "commands": [{"id": "NEW", "service": 1, "pid": "0D", "signals": [{"id": "NEW_SPEED"}]}], + }) + + assert (tmp_path / "vehicle" / "profile.json").read_bytes() == old_bytes + + +def test_profile_install_failure_restores_metadata_with_last_known_good(tmp_path, monkeypatch): + manager = ProfileManager(data_dir=tmp_path) + old_data = { + "metadata": {"name": "Old profile"}, + "commands": [{"id": "OLD", "service": 1, "pid": "0C", "signals": [{"id": "OLD_RPM"}]}], + } + manager.install_profile_data("vehicle", old_data, {"source": "old.json"}) + old_profile = (tmp_path / "vehicle" / "profile.json").read_bytes() + old_metadata = (tmp_path / "vehicle" / "metadata.json").read_bytes() + + import openpilot.starpilot.system.obdyssey.profiles as profiles_module + real_replace = profiles_module.os.replace + replace_calls = 0 + + def fail_second_replace(source, target): + nonlocal replace_calls + replace_calls += 1 + if replace_calls == 2: + raise OSError("simulated profile replacement failure") + real_replace(source, target) + + monkeypatch.setattr(profiles_module.os, "replace", fail_second_replace) + + with pytest.raises(OSError, match="replacement"): + manager.install_profile_data("vehicle", { + "metadata": {"name": "New profile"}, + "commands": [{"id": "NEW", "service": 1, "pid": "0D", "signals": [{"id": "NEW_SPEED"}]}], + }, {"source": "new.json"}) + + assert (tmp_path / "vehicle" / "profile.json").read_bytes() == old_profile + assert (tmp_path / "vehicle" / "metadata.json").read_bytes() == old_metadata + + +def test_profile_install_rejects_malformed_command_shape(tmp_path): + manager = ProfileManager(data_dir=tmp_path) + + with pytest.raises(ValueError, match="commands or pids"): + manager.install_profile_data("invalid", {"metadata": {}}) + + with pytest.raises(ValueError, match="signals must be a list"): + manager.install_profile_data("invalid", { + "commands": [{"service": 1, "pid": "0C", "signals": {}}], + }) + + +def test_profile_source_can_be_updated_without_losing_last_good(tmp_path): + source_path = tmp_path / "source.json" + source_data = { + "metadata": {"id": "source_car", "name": "Source car", "revision": "v1"}, + "commands": [{"id": "SOURCE", "service": 1, "pid": "0C", "signals": [{"id": "SOURCE_RPM"}]}], + } + source_path.write_text(json.dumps(source_data), encoding="utf-8") + + manager = ProfileManager(data_dir=tmp_path / "profiles") + installed = manager.install_profile_source(str(source_path), provider="test") + assert installed.id == "source_car" + assert manager.update_profile("source_car").revision == "v1" + assert json.loads((tmp_path / "profiles" / "source_car" / "metadata.json").read_text())[ + "source" + ] == str(source_path) + + +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..1eedcf909 --- /dev/null +++ b/starpilot/system/obdyssey/transport.py @@ -0,0 +1,228 @@ +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>", + "ATCFC1": "OK\r\n>", + "ATCFC0": "OK\r\n>", + "ATST": "OK\r\n>", + "ATCEA": "OK\r\n>", + "ATTA": "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..4868e7547 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, address: str = ""): + from openpilot.system.ui.widgets.obdyssey import OBDysseyScreen + gui_app.push_widget(OBDysseyScreen(adapter_address=address)) + 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(device.address) 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..4ef9b8f55 --- /dev/null +++ b/system/ui/widgets/obdyssey.py @@ -0,0 +1,464 @@ +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"), +} + +# The screen is a smoke-test consumer until a demand-driven watch API exists. +# Keep its sample set intentionally small; it must not turn every OBDb signal +# into a continuously polled vehicle request. +SMOKE_TEST_SIGNAL_IDS = ( + "SAE_ENGINE_RPM", + "SAE_VEHICLE_SPEED", + "SAE_ENGINE_COOLANT_TEMP", + "BOLT_HVBAT_SOC", + "BOLT_HVBAT_VOLTAGE", +) + + +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, adapter_address: str = ""): + super().__init__() + self._client = client or OBDysseyClient() + self.params = params or Params() + self._adapter_address = adapter_address + + 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._connect_adapter() + 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 _connect_adapter(self): + address = getattr(self, "_adapter_address", "") + return self._client.connect(address) if address else self._client.connect() + + def _read_smoke_test_signals(self): + if not self._diagnostic_ready(): + return + available = {signal["id"]: signal for signal in self._client.list_signals()} + sample_ids = [signal_id for signal_id in SMOKE_TEST_SIGNAL_IDS if signal_id in available] + self._available_signals = [available[signal_id] for signal_id in sample_ids] + if sample_ids: + self._live_telemetry = self._client.read_signals(sample_ids) + + 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 = "" + if dtcs: + gui_app.push_widget(alert_dialog(tr("Clear request completed, but diagnostic trouble codes remain."))) + else: + 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): + # Fetch status once upon entry. Auto-connect only through the daemon's + # explicit/validated adapter selection. + try: + self._status = self._client.status() + if not self._diagnostic_ready() and (not self._status or self._status.state in ("idle", "disabled")): + self._connect_adapter() + self._status = self._client.status() + self._read_smoke_test_signals() + except Exception as err: + self._last_error = str(err) + + # Main status loop. Vehicle reads remain demand-driven; this screen takes + # one small sample after connection/recovery rather than polling all PIDs. + 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() and not was_diagnostic_ready: + self._read_smoke_test_signals() + 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): + # Build list of signals to display + signals_to_render: list[tuple[str, Any, str, str]] = [] + if self._available_signals: + for sig in self._available_signals: + sig_id = sig["id"] + val = self._live_telemetry.get(sig_id, None) + common_info = COMMON_SIGNAL_NAMES.get(sig_id, (sig.get("name") or sig_id.replace("SAE_", "").replace("BOLT_", "").replace("_", " ").title(), sig.get("unit") or "")) + name = sig.get("name") or common_info[0] + unit = sig.get("unit") or common_info[1] + signals_to_render.append((sig_id, val, name, unit)) + elif self._live_telemetry: + for sig_id, val in self._live_telemetry.items(): + name, unit = COMMON_SIGNAL_NAMES.get(sig_id, (sig_id.replace("SAE_", "").replace("BOLT_", "").replace("_", " ").title(), "")) + signals_to_render.append((sig_id, val, name, unit)) + + num_cards = len(signals_to_render) + 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 140 + 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 + gui_label(rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 50), + tr("Sample Sensor Readings"), font_size=46, font_weight=FontWeight.BOLD) + cur_y += 65 + + if signals_to_render: + for idx, (sig_id, val, name, unit) in enumerate(signals_to_render): + 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 + 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 unit: + val_str += f" {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 + else: + state_rect = rl.Rectangle(rect.x + HEADER_PADDING, cur_y, rect.width - 2 * HEADER_PADDING, 100) + if rl.check_collision_recs(state_rect, rect): + rl.draw_rectangle_rounded(state_rect, 0.12, 16, CARD_BACKGROUND) + msg = tr("Connecting to adapter to load sensor telemetry...") if (self._status and self._status.state in ("connecting", "reconnecting", "initializing")) else tr("Connect to adapter to view live sensor telemetry.") + gui_label(rl.Rectangle(state_rect.x + 30, state_rect.y + 30, state_rect.width - 60, 40), + msg, font_size=36, color=TEXT_SECONDARY) + cur_y += 130 + + # 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..261be93b0 --- /dev/null +++ b/third_party/obdb_saej1979/profile.json @@ -0,0 +1,292 @@ +{ + "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", + "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", + "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", + "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", + "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", + "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", + "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", + "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", + "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", + "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", + "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", + "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", + "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", + "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" + } + } + ] + } + ] +}