bluey gps

selfdrive/ui/tests/test_bluetooth_big_ui.py

@@ -7,8 +7,9 @@ from openpilot.starpilot.system.bluetooth.protocol import BluetoothDevice, Bluet
 import openpilot.selfdrive.ui.layouts.settings.settings as settings_module
 from openpilot.selfdrive.ui.layouts.settings.settings import PanelType, SettingsLayout
 from openpilot.system.ui.widgets import DialogResult
+import openpilot.system.ui.widgets.bluetooth as bluetooth_widgets
 from openpilot.system.ui.widgets.bluetooth import (BluetoothManagerUI, PANEL_BACKGROUND, ROW_BORDER,
-                                                   device_action_allowed, device_status_text)
+                                                   device_action_allowed, device_status_connected, device_status_text)
 
 
 ADDRESS = "00:11:22:33:44:55"
@@ -139,6 +140,31 @@ def test_primary_device_action_is_pair_then_connect_then_manage():
   assert managed == [ADDRESS]
 
 
+def test_paired_phone_is_shown_as_a_gps_source():
+  phone = make_device(paired=True, phone=True)
+  assert device_status_text(phone, "", "") == "Phone - GPS source"
+  assert device_status_text(make_device(paired=True, phone=True, connected=True, gps="streaming"), "", "") == "Phone - GPS source - streaming"
+  assert device_status_text(make_device(paired=True, phone=True, gps="no_fix"), "", "") == "Phone - GPS source - connected, no GPS lock on phone"
+  assert device_status_text(make_device(phone=True), "", "") == "Tap to pair / phone"
+
+  # Only an actual GPS stream is green; BlueZ's Connected flag also covers failed audio/call attempts.
+  assert device_status_connected(make_device(paired=True, phone=True, gps="streaming"))
+  assert not device_status_connected(make_device(paired=True, phone=True, connected=True, gps="no_fix"))
+  assert not device_status_connected(make_device(paired=True, phone=True, connected=True))
+
+
+def test_tapping_a_paired_phone_explains_instead_of_connecting(monkeypatch):
+  manager = FakeBluetoothManager(BluetoothStatus(offroad=True, devices=(make_device(paired=True, phone=True),)))
+  ui = make_ui(manager)
+  pushed = []
+  monkeypatch.setattr(bluetooth_widgets.gui_app, "push_widget", pushed.append)
+
+  ui._select_device(ADDRESS)
+
+  assert manager.calls == []
+  assert len(pushed) == 1
+
+
 def test_scan_is_only_requested_when_the_existing_daemon_policy_allows_it():
   manager = FakeBluetoothManager(BluetoothStatus(enabled=True, offroad=True))
   ui = make_ui(manager)

starpilot/system/bluetooth/bluez.py

@@ -249,6 +249,7 @@ class BlueZClient:
         "uuids": uuids,
         "audio": audio,
         "controller": controller,
+        "phone": phone,
       }
       if include_hidden or show_pairing_device(device["address"], device["name"], device["paired"], device["trusted"], device["connected"],
                                                device["blocked"], audio, controller, include_discovering, phone):

starpilot/system/bluetooth/daemon.py

@@ -9,6 +9,7 @@ from typing import Any
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.starpilot.system.bluetooth.bluez import BlueZClient
+from openpilot.starpilot.system.bluetooth.phone_gps_fix import read_phone_status
 from openpilot.starpilot.system.bluetooth.protocol import BLUETOOTH_SOCKET_PATH
 from openpilot.starpilot.system.bluetooth.radio import BluetoothRadio
 
@@ -115,6 +116,10 @@ class BluetoothController:
       try:
         result.update(self._client().status())
         result["available"] = True
+        gps_address, gps_state = read_phone_status()
+        for device in result["devices"]:
+          if gps_address and device["address"].upper() == gps_address:
+            device["gps"] = gps_state
         self._bluez.agent.set_auto_accept_incoming(result["offroad"])
         prompt = result.get("prompt")
         if prompt is not None and self._pairing_address:

starpilot/system/bluetooth/phone_gps.py

@@ -16,8 +16,9 @@ from jeepney.io.threading import DBusRouter, open_dbus_connection
 from jeepney.low_level import HeaderFields, MessageType
 
 from openpilot.common.swaglog import cloudlog
-from openpilot.starpilot.system.bluetooth.bluez import BLUEZ, DEVICE_IFACE, OBJECT_MANAGER, unwrap_variant
-from openpilot.starpilot.system.bluetooth.phone_gps_fix import NmeaAccumulator, clear_phone_fix, write_phone_fix
+from openpilot.starpilot.system.bluetooth.bluez import ADAPTER_IFACE, BLUEZ, DEVICE_IFACE, OBJECT_MANAGER, unwrap_variant
+from openpilot.starpilot.system.bluetooth.phone_gps_fix import (PHONE_GPS_STATUS_PATH, NmeaAccumulator, address_from_device_path,
+                                                                clear_phone_fix, write_phone_fix, write_phone_status)
 from openpilot.starpilot.system.bluetooth.protocol import is_phone
 
 SPP_UUID = "00001101-0000-1000-8000-00805f9b34fb"
@@ -112,7 +113,16 @@ class PhoneGpsDaemon:
       self._fd = fd
       self._device_path = device_path
     cloudlog.warning(f"phone_gpsd: connected to {device_path}")
-    threading.Thread(target=self._read_loop, args=(fd,), daemon=True).start()
+    self._publish_status(device_path, None, None)
+    threading.Thread(target=self._read_loop, args=(fd, device_path), daemon=True).start()
+
+  @staticmethod
+  def _publish_status(device_path: str, last_data: float | None, last_fix: float | None) -> None:
+    # Only feeds the Bluetooth settings screen; never let it break the GPS link.
+    try:
+      write_phone_status(address_from_device_path(device_path), last_data, last_fix)
+    except OSError:
+      pass
 
   def _close_connection(self, reason: str) -> None:
     with self._state_lock:
@@ -125,11 +135,14 @@ class PhoneGpsDaemon:
     except OSError:
       pass
     clear_phone_fix()
+    clear_phone_fix(PHONE_GPS_STATUS_PATH)
     cloudlog.warning(f"phone_gpsd: disconnected from {device_path} ({reason})")
 
-  def _read_loop(self, fd: int) -> None:
+  def _read_loop(self, fd: int, device_path: str) -> None:
     accumulator = NmeaAccumulator()
     last_data = time.monotonic()
+    last_fix: float | None = None
+    last_status = 0.0
     first_fix = True
     reason = "stopped"
     try:
@@ -151,9 +164,13 @@ class PhoneGpsDaemon:
         last_data = now
         for fix in accumulator.feed_bytes(data):
           write_phone_fix(fix)
+          last_fix = now
           if first_fix:
             first_fix = False
             cloudlog.warning(f"phone_gpsd: first fix {fix['latitude']:.5f},{fix['longitude']:.5f} sats={fix['satellites']}")
+        if now - last_status >= 1.0:
+          last_status = now
+          self._publish_status(device_path, last_data, last_fix)
     except OSError as error:
       reason = str(error)
     self._close_connection(reason)
@@ -161,6 +178,9 @@ class PhoneGpsDaemon:
   def _connect_candidates(self) -> None:
     body = self._call("/", OBJECT_MANAGER, "GetManagedObjects")
     objects = unwrap_variant(body[0]) if body else {}
+    # Runs offroad too, so stay off the radio while bluetooth_managerd is scanning for devices to pair.
+    if any(interfaces.get(ADAPTER_IFACE, {}).get("Discovering", False) for interfaces in objects.values()):
+      return
     now = time.monotonic()
     for path, interfaces in objects.items():
       props = interfaces.get(DEVICE_IFACE)
@@ -180,6 +200,7 @@ class PhoneGpsDaemon:
 
   def run(self) -> None:
     clear_phone_fix()
+    clear_phone_fix(PHONE_GPS_STATUS_PATH)
     while not self._stop.is_set():
       try:
         if not self._registered:

starpilot/system/bluetooth/phone_gps_fix.py

@@ -14,6 +14,11 @@ import os
 import time
 
 PHONE_GPS_FIX_PATH = "/dev/shm/starpilot_phone_gps.json"
+# Link state for the Bluetooth settings screen, written by phone_gpsd, read by bluetooth_managerd.
+PHONE_GPS_STATUS_PATH = "/dev/shm/starpilot_phone_gps_status.json"
+
+# The phone app sends NMEA at ~1Hz; this long without bytes (or without a valid fix) counts as stale.
+PHONE_STATUS_STALE_S = 5.0
 
 # qcomgpsd only substitutes a phone fix this recent. Phones emit at 1Hz, so this tolerates a couple of
 # dropped epochs without ever publishing a position the car has already driven away from.
@@ -197,6 +202,40 @@ def read_phone_fix(path: str = PHONE_GPS_FIX_PATH, max_age: float = PHONE_FIX_MA
     return None
 
 
+def address_from_device_path(device_path: str) -> str:
+  # /org/bluez/hci0/dev_D4_3A_2C_63_2A_50 -> D4:3A:2C:63:2A:50
+  return device_path.rsplit("/", 1)[-1].removeprefix("dev_").replace("_", ":").upper()
+
+
+def write_phone_status(address: str, last_data: float | None, last_fix: float | None,
+                       path: str = PHONE_GPS_STATUS_PATH) -> None:
+  tmp_path = f"{path}.tmp"
+  with open(tmp_path, "w") as f:
+    json.dump({"address": address.upper(), "last_data": last_data, "last_fix": last_fix}, f)
+  os.replace(tmp_path, path)
+
+
+def read_phone_status(path: str = PHONE_GPS_STATUS_PATH, now: float | None = None) -> tuple[str, str]:
+  """Returns (address, state) where state is "streaming", "no_fix", "connected", or "" when not connected.
+
+  "no_fix" means NMEA is arriving but the phone has no GPS lock yet (e.g. indoors), which is still proof
+  the Bluetooth link works.
+  """
+  try:
+    with open(path) as f:
+      status = json.load(f)
+    now = time.monotonic() if now is None else now
+    address = str(status["address"]).upper()
+    last_fix, last_data = status.get("last_fix"), status.get("last_data")
+    if last_fix is not None and now - float(last_fix) <= PHONE_STATUS_STALE_S:
+      return address, "streaming"
+    if last_data is not None and now - float(last_data) <= PHONE_STATUS_STALE_S:
+      return address, "no_fix"
+    return address, "connected"
+  except Exception:
+    return "", ""
+
+
 def phone_fix_fields(fix: dict) -> dict:
   """gpsLocation field values for a phone fix. Accuracies are conservative estimates from HDOP."""
   hdop = fix.get("hdop")

starpilot/system/bluetooth/protocol.py

@@ -40,6 +40,9 @@ class BluetoothDevice:
   uuids: tuple[str, ...] = ()
   audio: bool = False
   controller: bool = False
+  phone: bool = False
+  # phone_gpsd link state for a phone: "streaming", "no_fix", "connected", or "" when not linked.
+  gps: str = ""
 
   @classmethod
   def from_dict(cls, value: dict[str, Any]) -> "BluetoothDevice":
@@ -54,6 +57,8 @@ 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)),
+      phone=bool(value.get("phone", False)),
+      gps=str(value.get("gps", "")),
     )
 
 

starpilot/system/bluetooth/tests/test_bluetooth.py

@@ -7,6 +7,7 @@ import pytest
 
 from openpilot.starpilot.system.bluetooth.audio import BluetoothAudioSink
 from openpilot.starpilot.system.bluetooth.bluez import PairingAgent
+import openpilot.starpilot.system.bluetooth.daemon as daemon_module
 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, is_phone, show_pairing_device)
@@ -426,6 +427,18 @@ def test_audio_uses_soundd_engage_alert_and_cleans_up():
   assert not params.get_bool("BluetoothAudioTestActive")
 
 
+def test_status_attaches_phone_gps_state_to_the_linked_device(monkeypatch):
+  params = FakeParams(IsOffroad=True, BluetoothEnabled=True)
+  client = FakeBlueZ()
+  controller = BluetoothController(params, lambda: client, FakeRadio())
+
+  monkeypatch.setattr(daemon_module, "read_phone_status", lambda: ("00:11:22:33:44:55", "streaming"))
+  assert controller.status()["devices"][0]["gps"] == "streaming"
+
+  monkeypatch.setattr(daemon_module, "read_phone_status", lambda: ("AA:BB:CC:DD:EE:FF", "streaming"))
+  assert "gps" not in controller.status()["devices"][0]
+
+
 def test_audio_requires_connected_device_and_offroad():
   params = FakeParams(IsOffroad=True, BluetoothEnabled=True)
   client = FakeBlueZ()

starpilot/system/bluetooth/tests/test_phone_gps_fix.py

@@ -3,8 +3,9 @@ import json
 
 import pytest
 
-from openpilot.starpilot.system.bluetooth.phone_gps_fix import (KNOTS_TO_MS, NmeaAccumulator, clear_phone_fix, nmea_checksum_ok,
-                                                                phone_fix_fields, read_phone_fix, write_phone_fix)
+from openpilot.starpilot.system.bluetooth.phone_gps_fix import (KNOTS_TO_MS, NmeaAccumulator, address_from_device_path, clear_phone_fix,
+                                                                nmea_checksum_ok, phone_fix_fields, read_phone_fix, read_phone_status,
+                                                                write_phone_fix, write_phone_status)
 
 
 def sentence(body: str) -> str:
@@ -97,6 +98,26 @@ def test_read_never_raises(tmp_path):
   assert read_phone_fix(str(path), now=0.0) is None
 
 
+def test_phone_status_states(tmp_path):
+  path = str(tmp_path / "status.json")
+  assert read_phone_status(path, now=0.0) == ("", "")
+
+  assert address_from_device_path("/org/bluez/hci0/dev_d4_3a_2c_63_2a_50") == "D4:3A:2C:63:2A:50"
+  write_phone_status("d4:3a:2c:63:2a:50", None, None, path)
+  assert read_phone_status(path, now=100.0) == ("D4:3A:2C:63:2A:50", "connected")
+
+  write_phone_status("D4:3A:2C:63:2A:50", 100.0, None, path)
+  assert read_phone_status(path, now=101.0)[1] == "no_fix"
+
+  write_phone_status("D4:3A:2C:63:2A:50", 100.0, 100.0, path)
+  assert read_phone_status(path, now=101.0)[1] == "streaming"
+  # Data stopped long ago but the link file is still there: connected, not streaming.
+  assert read_phone_status(path, now=200.0)[1] == "connected"
+
+  clear_phone_fix(path)
+  assert read_phone_status(path, now=101.0) == ("", "")
+
+
 def test_phone_fix_fields():
   fields = phone_fix_fields(NmeaAccumulator().feed_line(RMC))
   assert fields["hasFix"]

system/manager/process_config.py

@@ -129,12 +129,6 @@ def bluetooth_enabled(started: bool, params: Params, CP: car.CarParams, starpilo
   return params.get_bool("BluetoothEnabled")
 
 
-def phone_gps_enabled(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
-  # Onroad only: the fallback only matters while driving, and offroad it would be opening links to the
-  # phone while bluetooth_managerd is scanning or pairing.
-  return started and params.get_bool("BluetoothEnabled")
-
-
 def soundd_run(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
   return driverview(started, params, CP, starpilot_toggles) or params.get_bool("BluetoothAudioTestActive")
 
@@ -218,7 +212,8 @@ procs = [
 # StarPilot variables
 procs += [
   PythonProcess("bluetooth_managerd", "starpilot.system.bluetooth.daemon", bluetooth_enabled, enabled=TICI),
-  PythonProcess("phone_gpsd", "starpilot.system.bluetooth.phone_gps", phone_gps_enabled, enabled=TICI, nice=10),
+  # Runs offroad too so the Bluetooth settings screen shows the phone link working before a drive.
+  PythonProcess("phone_gpsd", "starpilot.system.bluetooth.phone_gps", bluetooth_enabled, enabled=TICI, nice=10),
   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),

system/ui/widgets/bluetooth.py

@@ -35,16 +35,31 @@ TEXT_DISABLED = rl.Color(150, 150, 150, 255)
 TEXT_CONNECTED = rl.Color(113, 209, 135, 255)
 
 
+PHONE_GPS_STATE_TEXT = {
+  "streaming": "streaming",
+  "no_fix": "connected, no GPS lock on phone",
+  "connected": "connected, waiting for data",
+}
+
+
 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:
     return operation.capitalize() + "..."
 
+  # A paired phone is only ever a GPS source (phone_gpsd connects it by itself), so describe that link
+  # rather than BlueZ's generic Connected flag, which also covers its failed audio/call profile attempts.
+  if device.phone and device.paired:
+    state = PHONE_GPS_STATE_TEXT.get(device.gps)
+    return tr("Phone - GPS source") + (f" - {tr(state)}" if state else "")
+
   capabilities = []
   if device.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.phone:
+    capabilities.append(tr("phone"))
   capability_text = " / ".join(capabilities)
 
   if device.connected:
@@ -54,6 +69,13 @@ def device_status_text(device: BluetoothDevice, operation: str, selected_audio:
   return tr("Tap to pair") + (f" / {capability_text}" if capability_text else "")
 
 
+def device_status_connected(device: BluetoothDevice) -> bool:
+  """Whether the status line is drawn in the connected (green) color."""
+  if device.phone and device.paired:
+    return device.gps == "streaming"
+  return device.connected
+
+
 def device_action_allowed(device: BluetoothDevice, operation: str, offroad: bool) -> bool:
   """Mirror the daemon's operation policy before a row can receive a tap."""
   if operation:
@@ -126,7 +148,7 @@ class BluetoothDeviceRow(Widget):
 
     status_rect = rl.Rectangle(text_rect.x, rect.y + 82, text_rect.width, 52)
     status = device_status_text(state.device, state.operation, state.selected_audio)
-    status_color = TEXT_CONNECTED if state.device.connected and not state.operation else TEXT_SECONDARY
+    status_color = TEXT_CONNECTED if device_status_connected(state.device) and not state.operation else TEXT_SECONDARY
     if not enabled:
       status_color = TEXT_DISABLED
     gui_label(status_rect, status, font_size=39, color=status_color)
@@ -264,6 +286,14 @@ class BluetoothManagerUI(Widget):
       return
     if not device.paired:
       self._manager.pair(device.address)
+    elif device.phone:
+      # A generic Connect only tries audio/call profiles, which the comma has none of for a phone, so it
+      # just hangs on "connecting". phone_gpsd opens the GPS link by itself; explain that instead.
+      if device.gps:
+        message = tr("Receiving GPS from this phone.")
+      else:
+        message = tr("Start the Bluetooth stream in your phone's GPS app. The comma connects to it automatically.")
+      gui_app.push_widget(alert_dialog(message))
     elif not device.connected:
       self._manager.connect(device.address)
     else: