Add minimal ELM327 RFCOMM support

pyproject.toml

@@ -179,6 +179,8 @@ testpaths = [
   "system/tests",
   "system/ubloxd",
   "system/webrtc",
+  "starpilot/system/bluetooth/tests",
+  "starpilot/system/obd/tests",
   "tools/lib/tests",
   "tools/replay",
   "tools/cabana",

selfdrive/ui/mici/layouts/settings/bluetooth.py

@@ -56,6 +56,8 @@ class BluetoothDeviceButton(BigButton):
         capabilities.append("audio")
       if self.device.controller:
         capabilities.append("controller")
+      if self.device.serial:
+        capabilities.append("serial")
       self.set_value("connected" + (f" / {' / '.join(capabilities)}" if capabilities else ""))
       self.set_enabled(True)
     elif self.device.paired:
@@ -67,6 +69,8 @@ class BluetoothDeviceButton(BigButton):
         capabilities.append("audio")
       if self.device.controller:
         capabilities.append("controller")
+      if self.device.serial:
+        capabilities.append("serial")
       self.set_value("pair" + (f" / {' / '.join(capabilities)}" if capabilities else ""))
       self.set_enabled(self._offroad)
 
@@ -264,7 +268,7 @@ class BluetoothLayoutMici(NavScroller):
       status.offroad,
       status.selected_audio,
       status.pairing_address,
-      tuple((device.address, device.name, device.paired, device.connected, device.audio, device.controller) for device in status.devices),
+      tuple((device.address, device.name, device.paired, device.connected, device.audio, device.controller, device.serial) for device in status.devices),
     )
     if signature != self._last_signature:
       self._last_signature = signature

selfdrive/ui/tests/test_bluetooth_big_ui.py

@@ -103,9 +103,11 @@ def test_settings_constructs_a_dedicated_bluetooth_panel(monkeypatch):
 
 def test_device_status_prioritizes_operations_then_connection_and_capabilities():
   device = make_device(paired=True, connected=True, audio=True, controller=True)
+  serial_device = make_device(serial=True)
 
   assert device_status_text(device, "connecting", ADDRESS) == "Connecting..."
   assert device_status_text(device, "", ADDRESS) == "Connected / audio output / controller"
+  assert device_status_text(serial_device, "", "") == "Tap to pair / serial"
 
 
 def test_device_action_policy_matches_the_daemon_onroad_rules():

starpilot/system/bluetooth/bluez.py

@@ -235,7 +235,7 @@ class BlueZClient:
         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", "")))
+      audio, controller, serial = device_capabilities(uuids, int(props.get("Class", 0)), str(props.get("Icon", "")))
       device = {
         "path": path,
         "address": str(props.get("Address", "")),
@@ -248,9 +248,10 @@ class BlueZClient:
         "uuids": uuids,
         "audio": audio,
         "controller": controller,
+        "serial": serial,
       }
       if include_hidden or show_pairing_device(device["address"], device["name"], device["paired"], device["trusted"], device["connected"],
-                                               device["blocked"], audio, controller, include_discovering):
+                                               device["blocked"], audio, controller, serial, include_discovering):
         devices.append(device)
     return sorted(devices, key=lambda device: (not device["connected"], not device["paired"], -(device["rssi"] or -127), device["name"].lower()))
 

starpilot/system/bluetooth/protocol.py

@@ -16,6 +16,7 @@ BLUETOOTH_RADIO_HELPER = "/usr/comma/bluetooth-radio"
 A2DP_SINK_UUID = "0000110b-0000-1000-8000-00805f9b34fb"
 HID_UUID = "00001124-0000-1000-8000-00805f9b34fb"
 HOG_UUID = "00001812-0000-1000-8000-00805f9b34fb"
+SPP_UUID = "00001101-0000-1000-8000-00805f9b34fb"
 COMMAND_TIMEOUTS = {
   "set_power": 90.0,
   "start_scan": 20.0,
@@ -40,6 +41,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 +56,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,21 +89,22 @@ class BluetoothStatus:
     )
 
 
-def device_capabilities(uuids: list[str] | tuple[str, ...], bluetooth_class: int = 0, icon: str = "") -> tuple[bool, bool]:
+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, discovering: bool = False) -> bool:
+                        audio: bool, controller: bool, serial: bool, discovering: bool = False) -> bool:
   known = paired or trusted or connected
   normalized_address = "".join(character for character in address.upper() if character.isalnum())
   normalized_name = "".join(character for character in name.upper() if character.isalnum())
   named = bool(name) and name != "Unknown device" and normalized_name != normalized_address
-  return known or (named and not blocked and (audio or controller))
+  return known or (named and not blocked and (audio or controller or serial))
 
 
 class _DesktopFakeBluetooth:

starpilot/system/bluetooth/tests/test_bluetooth.py

@@ -8,7 +8,7 @@ import pytest
 from openpilot.starpilot.system.bluetooth.audio import BluetoothAudioSink
 from openpilot.starpilot.system.bluetooth.bluez import PairingAgent
 from openpilot.starpilot.system.bluetooth.daemon import BluetoothController
-from openpilot.starpilot.system.bluetooth.protocol import (A2DP_SINK_UUID, HID_UUID, BluetoothClient, BluetoothDevice, BluetoothStatus,
+from openpilot.starpilot.system.bluetooth.protocol import (A2DP_SINK_UUID, HID_UUID, SPP_UUID, BluetoothClient, BluetoothDevice, BluetoothStatus,
                                                            device_capabilities, show_pairing_device)
 from openpilot.system import hardware
 from openpilot.system.ui.lib.bluetooth_manager import BluetoothManager
@@ -169,23 +169,26 @@ 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, SPP_UUID],
+                 "audio": True, "controller": True, "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, SPP_UUID),
+                                            audio=True, controller=True, 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", "Media Remote", False, False, False, False, False, True, True)
-  assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, 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", "Serial Adapter", False, False, False, False, False, False, True)
+  assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True, False, True)
+  assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False, False, True)
+  assert show_pairing_device("00:11:22:33:44:55", "Known device", True, True, False, False, False, False, False)
 
 
 def test_desktop_fake_bluetooth_is_stateful_and_interactive(monkeypatch, tmp_path):

starpilot/system/obd/__init__.py

@@ -0,0 +1 @@
+

starpilot/system/obd/elm327.py

@@ -0,0 +1,171 @@
+import socket
+import threading
+import time
+
+from typing import Any
+
+
+DEFAULT_COMMAND_TIMEOUT = 5.0
+DEFAULT_CONNECT_TIMEOUT = 10.0
+ELM_IDENTITY_MARKERS = ("ELM", "STN", "OBD")
+
+
+class RFCOMMTransport:
+  def __init__(self, connect_timeout: float = DEFAULT_CONNECT_TIMEOUT):
+    self.connect_timeout = connect_timeout
+    self.channel: int | None = None
+    self._socket: socket.socket | None = None
+
+  def connect(self, mac: str) -> None:
+    self.close()
+    address_family = getattr(socket, "AF_BLUETOOTH", None)
+    protocol = getattr(socket, "BTPROTO_RFCOMM", None)
+    if address_family is None or protocol is None:
+      raise RuntimeError("Native Bluetooth RFCOMM sockets are unavailable")
+    channels = tuple(dict.fromkeys((self.channel, 1, 2))) if self.channel is not None else (1, 2)
+    last_error: OSError | None = None
+    for channel in channels:
+      sock = socket.socket(address_family, socket.SOCK_STREAM, protocol)
+      try:
+        sock.settimeout(self.connect_timeout)
+        sock.connect((mac, channel))
+      except OSError as error:
+        last_error = error
+        sock.close()
+        continue
+      self._socket = sock
+      self.channel = channel
+      return
+    raise ConnectionError(f"Unable to connect to {mac} on RFCOMM channels 1 or 2") from last_error
+
+  def close(self) -> None:
+    sock = self._socket
+    self._socket = None
+    if sock is not None:
+      sock.close()
+
+  def read(self, size: int = 4096, timeout: float | None = None) -> bytes:
+    if self._socket is None:
+      raise ConnectionError("RFCOMM transport is not connected")
+    self._socket.settimeout(timeout)
+    data = self._socket.recv(size)
+    if not data:
+      self.close()
+      raise ConnectionError("RFCOMM device disconnected")
+    return data
+
+  def write(self, data: bytes) -> None:
+    if self._socket is None:
+      raise ConnectionError("RFCOMM transport is not connected")
+    try:
+      self._socket.sendall(data)
+    except OSError:
+      self.close()
+      raise
+
+
+class ELM327:
+  def __init__(self, mac: str, transport: RFCOMMTransport | None = None, command_timeout: float = DEFAULT_COMMAND_TIMEOUT,
+               reconnect_attempts: int = 3, reconnect_backoff: float = 1.0, sleep=time.sleep):
+    self.mac = mac
+    self.command_timeout = command_timeout
+    self.reconnect_attempts = reconnect_attempts
+    self.reconnect_backoff = reconnect_backoff
+    self._transport: Any = transport or RFCOMMTransport()
+    self._lock = threading.RLock()
+    self._connected = False
+    self._initialized = False
+    self._sleep = sleep
+
+  def connect(self) -> str:
+    with self._lock:
+      self.close()
+      try:
+        return self._open_and_identify()
+      except Exception:
+        self.close()
+        raise
+
+  def close(self) -> None:
+    with self._lock:
+      self._transport.close()
+      self._connected = False
+      self._initialized = False
+
+  def command(self, cmd: str, timeout: float | None = None) -> str:
+    with self._lock:
+      try:
+        return self._command_once(cmd, self.command_timeout if timeout is None else timeout)
+      except OSError:
+        initialized = self._initialized
+        self.close()
+        self._reconnect(initialized)
+        raise
+
+  def initialize(self) -> None:
+    with self._lock:
+      try:
+        self._initialize_once()
+      except OSError:
+        self.close()
+        self._reconnect(initialize=True)
+        raise
+
+  def _open_and_identify(self) -> str:
+    self._transport.connect(self.mac)
+    self._connected = True
+    identity = self._command_once("ATI", self.command_timeout)
+    if not any(marker in identity.upper() for marker in ELM_IDENTITY_MARKERS):
+      raise RuntimeError("RFCOMM device did not identify as an ELM-compatible adapter")
+    return identity
+
+  def _initialize_once(self) -> None:
+    for command in ("ATZ", "ATE0", "ATL0", "ATI", "ATSP0"):
+      self._command_once(command, self.command_timeout)
+    self._initialized = True
+
+  def _reconnect(self, initialize: bool) -> None:
+    for attempt in range(self.reconnect_attempts):
+      if attempt:
+        self._sleep(self.reconnect_backoff * (2 ** (attempt - 1)))
+      try:
+        self._open_and_identify()
+        if initialize:
+          self._initialize_once()
+        return
+      except OSError:
+        self.close()
+
+  def _command_once(self, cmd: str, timeout: float) -> str:
+    if not self._connected:
+      raise ConnectionError("ELM327 is not connected")
+    command = cmd.rstrip("\r\n")
+    self._transport.write(command.encode("ascii") + b"\r")
+    deadline = time.monotonic() + timeout
+    response = bytearray()
+    while b">" not in response:
+      remaining = deadline - time.monotonic()
+      if remaining <= 0:
+        raise TimeoutError(f"Timed out waiting for ELM327 response to {command!r}")
+      try:
+        chunk = self._transport.read(4096, timeout=remaining)
+      except TimeoutError as error:
+        raise TimeoutError(f"Timed out waiting for ELM327 response to {command!r}") from error
+      if not chunk:
+        raise ConnectionError("ELM327 disconnected before sending a prompt")
+      response.extend(chunk)
+    raw = bytes(response).split(b">", 1)[0]
+    return self._clean_response(raw, command)
+
+  @staticmethod
+  def _clean_response(raw: bytes, command: str) -> str:
+    lines = raw.decode("ascii", errors="replace").replace("\r\n", "\n").replace("\r", "\n").split("\n")
+    while lines and not lines[0].strip():
+      lines.pop(0)
+    while lines and not lines[-1].strip():
+      lines.pop()
+    if lines and lines[0].strip().upper() == command.upper():
+      lines.pop(0)
+      while lines and not lines[0].strip():
+        lines.pop(0)
+    return "\n".join(lines)

starpilot/system/obd/tests/__init__.py

@@ -0,0 +1 @@
+

starpilot/system/obd/tests/test_elm327.py

@@ -0,0 +1,226 @@
+from collections import deque
+
+import pytest
+
+import openpilot.starpilot.system.obd.elm327 as elm327_module
+from openpilot.starpilot.system.obd.elm327 import ELM327, RFCOMMTransport
+
+
+class ScriptedTransport:
+  def __init__(self, responses):
+    self.responses = deque(responses)
+    self.current = deque()
+    self.connects = []
+    self.writes = []
+    self.connected = False
+
+  def connect(self, mac):
+    self.connects.append(mac)
+    self.connected = True
+
+  def close(self):
+    self.connected = False
+
+  def write(self, data):
+    if not self.connected:
+      raise ConnectionError("transport is closed")
+    self.writes.append(data)
+    self.current = deque(self.responses.popleft())
+
+  def read(self, _size=4096, timeout=None):
+    item = self.current.popleft()
+    if isinstance(item, BaseException):
+      raise item
+    return item
+
+
+class FakeRFCOMMSocket:
+  def __init__(self, attempts, failing_channels):
+    self.attempts = attempts
+    self.failing_channels = failing_channels
+    self.closed = False
+    self.timeout = None
+
+  def settimeout(self, timeout):
+    self.timeout = timeout
+
+  def connect(self, address):
+    self.attempts.append(address)
+    if address[1] in self.failing_channels:
+      raise OSError("channel unavailable")
+
+  def close(self):
+    self.closed = True
+
+
+def test_command_returns_clean_normal_response():
+  transport = ScriptedTransport([
+    [b"ATI\rELM327 v1.5\r>"],
+    [b"41 00 BE 3E B8 13\r>"],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  assert elm.connect() == "ELM327 v1.5"
+  assert elm.command("0100") == "41 00 BE 3E B8 13"
+  assert transport.writes == [b"ATI\r", b"0100\r"]
+
+
+def test_command_handles_echo_crlf_chunks_and_multiline_response():
+  transport = ScriptedTransport([
+    [b"ATI\r\rELM327 v1.5\r\r>"],
+    [b"010C\r\rSEARCHING...\r\n", b"41 0C 1A F8\r\n>"],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  elm.connect()
+
+  assert elm.command("010C") == "SEARCHING...\n41 0C 1A F8"
+
+
+def test_command_preserves_substantive_spacing_and_blank_lines():
+  transport = ScriptedTransport([
+    [b"ELM327 v1.5\r>"],
+    [b"0100\r\r  VALUE  \r\rSECOND\r>"],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  elm.connect()
+
+  assert elm.command("0100") == "  VALUE  \n\nSECOND"
+
+
+@pytest.mark.parametrize(("raw", "expected"), [
+  (b"NO DATA\r>", "NO DATA"),
+  (b"?\r>", "?"),
+])
+def test_command_returns_valid_adapter_states(raw, expected):
+  transport = ScriptedTransport([
+    [b"ELM327 v1.5\r>"],
+    [raw],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  elm.connect()
+
+  assert elm.command("0100") == expected
+
+
+def test_initialize_uses_the_minimal_obd_command_sequence():
+  transport = ScriptedTransport([
+    [b"ELM327 v1.5\r>"],
+    [b"ATZ\rELM327 v1.5\r>"],
+    [b"ATE0\rOK\r>"],
+    [b"OK\r>"],
+    [b"ELM327 v1.5\r>"],
+    [b"OK\r>"],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  elm.connect()
+  elm.initialize()
+
+  assert transport.writes == [b"ATI\r", b"ATZ\r", b"ATE0\r", b"ATL0\r", b"ATI\r", b"ATSP0\r"]
+
+
+@pytest.mark.parametrize("identity", [b"OK\r>", b"NO DATA\r>", b"CAN ERROR\r>", b"garbage\r>"])
+def test_connect_requires_a_sensible_elm_identity(identity):
+  transport = ScriptedTransport([[identity]])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  with pytest.raises(RuntimeError, match="identify"):
+    elm.connect()
+
+  assert not transport.connected
+
+
+def test_timeout_is_an_exception_and_reconnects_the_session():
+  transport = ScriptedTransport([
+    [b"ELM327 v1.5\r>"],
+    [TimeoutError("read timed out")],
+    [b"ELM327 v1.5\r>"],
+    [b"NO DATA\r>"],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  elm.connect()
+  with pytest.raises(TimeoutError, match="0100"):
+    elm.command("0100", timeout=0.01)
+
+  assert transport.connects == ["00:11:22:33:44:55", "00:11:22:33:44:55"]
+  assert elm.command("0100") == "NO DATA"
+
+
+def test_disconnect_before_prompt_reconnects_and_reinitializes():
+  initialization = [
+    [b"ELM327 v1.5\r>"],
+    [b"OK\r>"],
+    [b"OK\r>"],
+    [b"ELM327 v1.5\r>"],
+    [b"OK\r>"],
+  ]
+  transport = ScriptedTransport([
+    [b"ELM327 v1.5\r>"],
+    *initialization,
+    [b"41 00 BE", b""],
+    [b"ELM327 v1.5\r>"],
+    *initialization,
+    [b"41 00 BE 3E B8 13\r>"],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  elm.connect()
+  elm.initialize()
+  with pytest.raises(ConnectionError, match="prompt"):
+    elm.command("0100")
+
+  assert transport.connects == ["00:11:22:33:44:55", "00:11:22:33:44:55"]
+  assert transport.writes[-6:] == [b"ATI\r", b"ATZ\r", b"ATE0\r", b"ATL0\r", b"ATI\r", b"ATSP0\r"]
+  assert elm.command("0100") == "41 00 BE 3E B8 13"
+
+
+def test_initialize_failure_reconnects_and_restarts_initialization():
+  transport = ScriptedTransport([
+    [b"ELM327 v1.5\r>"],
+    [b"ELM327 v1.5\r>"],
+    [b""],
+    [b"ELM327 v1.5\r>"],
+    [b"ELM327 v1.5\r>"],
+    [b"OK\r>"],
+    [b"OK\r>"],
+    [b"ELM327 v1.5\r>"],
+    [b"OK\r>"],
+    [b"NO DATA\r>"],
+  ])
+  elm = ELM327("00:11:22:33:44:55", transport=transport)
+
+  elm.connect()
+  with pytest.raises(ConnectionError, match="prompt"):
+    elm.initialize()
+
+  assert transport.writes[-6:] == [b"ATI\r", b"ATZ\r", b"ATE0\r", b"ATL0\r", b"ATI\r", b"ATSP0\r"]
+  assert elm.command("0100") == "NO DATA"
+
+
+def test_rfcomm_transport_falls_back_then_reuses_the_working_channel(monkeypatch):
+  attempts = []
+  failing_channels = {1}
+
+  def socket_factory(*_args):
+    return FakeRFCOMMSocket(attempts, failing_channels)
+
+  monkeypatch.setattr(elm327_module.socket, "AF_BLUETOOTH", 31, raising=False)
+  monkeypatch.setattr(elm327_module.socket, "BTPROTO_RFCOMM", 3, raising=False)
+  monkeypatch.setattr(elm327_module.socket, "socket", socket_factory)
+  transport = RFCOMMTransport()
+
+  transport.connect("00:11:22:33:44:55")
+  assert transport.channel == 2
+  transport.close()
+  failing_channels.clear()
+  transport.connect("00:11:22:33:44:55")
+
+  assert attempts == [
+    ("00:11:22:33:44:55", 1),
+    ("00:11:22:33:44:55", 2),
+    ("00:11:22:33:44:55", 2),
+  ]

system/ui/widgets/bluetooth.py

@@ -45,6 +45,8 @@ 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:
+    capabilities.append(tr("serial"))
   capability_text = " / ".join(capabilities)
 
   if device.connected: