diff --git a/selfdrive/ui/tests/test_bluetooth_big_ui.py b/selfdrive/ui/tests/test_bluetooth_big_ui.py index ce2fbea052..717b510c9b 100644 --- a/selfdrive/ui/tests/test_bluetooth_big_ui.py +++ b/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) diff --git a/starpilot/system/bluetooth/bluez.py b/starpilot/system/bluetooth/bluez.py index bfa6799dc6..0e308fb66b 100644 --- a/starpilot/system/bluetooth/bluez.py +++ b/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): diff --git a/starpilot/system/bluetooth/daemon.py b/starpilot/system/bluetooth/daemon.py index d23985737b..d4c7638e57 100644 --- a/starpilot/system/bluetooth/daemon.py +++ b/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: diff --git a/starpilot/system/bluetooth/phone_gps.py b/starpilot/system/bluetooth/phone_gps.py index eec14f9948..f5c98e7d71 100644 --- a/starpilot/system/bluetooth/phone_gps.py +++ b/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: diff --git a/starpilot/system/bluetooth/phone_gps_fix.py b/starpilot/system/bluetooth/phone_gps_fix.py index 188401bf4c..065699d8fd 100644 --- a/starpilot/system/bluetooth/phone_gps_fix.py +++ b/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") diff --git a/starpilot/system/bluetooth/protocol.py b/starpilot/system/bluetooth/protocol.py index 8a7ba43e19..87c6a45657 100644 --- a/starpilot/system/bluetooth/protocol.py +++ b/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", "")), ) diff --git a/starpilot/system/bluetooth/tests/test_bluetooth.py b/starpilot/system/bluetooth/tests/test_bluetooth.py index cd73a77a24..31998f4a7f 100644 --- a/starpilot/system/bluetooth/tests/test_bluetooth.py +++ b/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() diff --git a/starpilot/system/bluetooth/tests/test_phone_gps_fix.py b/starpilot/system/bluetooth/tests/test_phone_gps_fix.py index 0800c7b4a2..a546b199ee 100644 --- a/starpilot/system/bluetooth/tests/test_phone_gps_fix.py +++ b/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"] diff --git a/system/manager/process_config.py b/system/manager/process_config.py index e0a79d329a..ced95985f7 100644 --- a/system/manager/process_config.py +++ b/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), diff --git a/system/ui/widgets/bluetooth.py b/system/ui/widgets/bluetooth.py index def4d61825..1439ef5901 100644 --- a/system/ui/widgets/bluetooth.py +++ b/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: