From f20731225ff43af1a6e1fee8040095d67466f2c4 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Tue, 29 Sep 2026 10:40:09 -0500 Subject: [PATCH] blueee --- starpilot/system/bluetooth/phone_gps.py | 234 +++++++++--------- starpilot/system/bluetooth/phone_gps_fix.py | 35 +++ .../system/bluetooth/tests/test_phone_gps.py | 87 +++++++ .../bluetooth/tests/test_phone_gps_fix.py | 46 +++- 4 files changed, 277 insertions(+), 125 deletions(-) create mode 100644 starpilot/system/bluetooth/tests/test_phone_gps.py diff --git a/starpilot/system/bluetooth/phone_gps.py b/starpilot/system/bluetooth/phone_gps.py index f5c98e7d71..ef1856e549 100644 --- a/starpilot/system/bluetooth/phone_gps.py +++ b/starpilot/system/bluetooth/phone_gps.py @@ -1,34 +1,38 @@ """phone_gpsd: read a phone's GPS over Bluetooth SPP so qcomgpsd can fall back to it. -Apps like "GPS NMEA Tether" run an SPP (RFCOMM serial) server on the phone that streams NMEA 0183. BlueZ's -Profile API does the SDP lookup and RFCOMM connect for us and hands over the connected socket as a file -descriptor, so this needs no AF_BLUETOOTH support in Python. Each parsed fix goes to a small file in -/dev/shm (see phone_gps_fix.py); qcomgpsd publishes it on gpsLocation only while the modem has no fix. +Apps like "GPS NMEA Tether" run an SPP (RFCOMM serial) server on the phone that streams NMEA 0183. A phone +usually advertises several serial ports at once (other GPS apps, Android's own Nearby Share, ...), so this +lists them with sdptool, tries GPS-named services first, and keeps the first one that actually sends valid +NMEA. BlueZ's ConnectProfile can't be used for this: it connects to whichever serial port it finds first. + +Each parsed fix goes to a small file in /dev/shm (see phone_gps_fix.py); qcomgpsd publishes it on gpsLocation +only while the modem has no fix. """ -import os -import select import signal +import socket +import subprocess import threading import time -from jeepney import DBusAddress, MatchRule, new_error, new_method_call, new_method_return +from jeepney import DBusAddress, new_method_call 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 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.phone_gps_fix import (PHONE_GPS_STATUS_PATH, NmeaAccumulator, clear_phone_fix, contains_nmea, + parse_serial_ports, write_phone_fix, write_phone_status) from openpilot.starpilot.system.bluetooth.protocol import is_phone SPP_UUID = "00001101-0000-1000-8000-00805f9b34fb" -PROFILE_PATH = "/link/firestar/starpilot/phone_gps" -PROFILE_IFACE = "org.bluez.Profile1" -PROFILE_MANAGER_IFACE = "org.bluez.ProfileManager1" CONNECT_POLL_S = 2.0 RETRY_MIN_S = 15.0 RETRY_MAX_S = 120.0 +SDP_TIMEOUT_S = 20.0 +CONNECT_TIMEOUT_S = 15.0 +# How long a freshly opened serial port gets to prove it speaks NMEA. Apps send at ~1Hz even without a fix. +PROBE_TIMEOUT_S = 6.0 # The app streams at ~1Hz, so this long with no bytes means the link or the app has died. STALL_TIMEOUT_S = 10.0 @@ -40,144 +44,139 @@ def is_phone_candidate(props: dict) -> bool: return SPP_UUID in uuids or is_phone(int(props.get("Class", 0)), str(props.get("Icon", ""))) +def browse_serial_ports(address: str) -> list[tuple[str, int]]: + result = subprocess.run(["sdptool", "browse", address], capture_output=True, text=True, timeout=SDP_TIMEOUT_S, check=False) + return parse_serial_ports(result.stdout) + + +def probe_nmea(address: str, channel: int) -> tuple[socket.socket | None, bytes, str]: + """Open an RFCOMM connection and keep it only if NMEA arrives. Returns (socket, bytes read, failure reason).""" + sock = socket.socket(socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM) + received = b"" + connected = False + try: + # Connecting includes paging the phone and encrypting the link, so it gets its own, longer timeout. + sock.settimeout(CONNECT_TIMEOUT_S) + sock.connect((address, channel)) + connected = True + sock.settimeout(PROBE_TIMEOUT_S) + deadline = time.monotonic() + PROBE_TIMEOUT_S + while time.monotonic() < deadline: + data = sock.recv(1024) + if not data: + break + received += data + if contains_nmea(received): + return sock, received, "" + reason = "no NMEA" + except TimeoutError: + reason = "no NMEA" if connected else "connect timed out" + except OSError as error: + reason = error.strerror or str(error) + sock.close() + return None, b"", reason + + class PhoneGpsDaemon: def __init__(self): - # enable_fds: BlueZ passes the connected RFCOMM socket to NewConnection as a unix fd. - self.router = DBusRouter(open_dbus_connection(bus="SYSTEM", enable_fds=True)) - self._call_lock = threading.Lock() + self.router = DBusRouter(open_dbus_connection(bus="SYSTEM")) self._state_lock = threading.Lock() self._stop = threading.Event() self._stopped = False - self._registered = False - self._fd: int | None = None - self._device_path = "" + self._sock: socket.socket | None = None self._retry_after: dict[str, tuple[float, float]] = {} - self._profile_filter = self.router.filter(MatchRule(type="method_call", interface=PROFILE_IFACE, path=PROFILE_PATH), bufsize=10) - self._profile_queue = self._profile_filter.__enter__() - self._profile_thread = threading.Thread(target=self._profile_loop, daemon=True) - self._profile_thread.start() - - 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) - with self._call_lock: - reply = self.router.send_and_get_reply(message, timeout=timeout) + def _managed_objects(self) -> dict: + message = new_method_call(DBusAddress("/", bus_name=BLUEZ, interface=OBJECT_MANAGER), "GetManagedObjects") + reply = self.router.send_and_get_reply(message, timeout=15.0) if reply.header.message_type == MessageType.error: raise RuntimeError(str(reply.body[0] if reply.body else reply.header.fields.get(HeaderFields.error_name, "failed"))) - return reply.body - - def _register_profile(self) -> None: - options = { - "Name": ("s", "StarPilot Phone GPS"), - "Role": ("s", "client"), - "AutoConnect": ("b", False), - } - try: - self._call("/org/bluez", PROFILE_MANAGER_IFACE, "RegisterProfile", "osa{sv}", (PROFILE_PATH, SPP_UUID, options)) - except RuntimeError as error: - if "alreadyexists" not in str(error).replace(" ", "").lower(): - raise - self._registered = True - cloudlog.warning("phone_gpsd: SPP client profile registered") - - def _profile_loop(self) -> None: - while not self._stop.is_set(): - message = self._profile_queue.get() - if message is None: - break - member = message.header.fields.get(HeaderFields.member, "") - try: - if member == "NewConnection": - self._on_new_connection(str(message.body[0]), message.body[1]) - elif member == "RequestDisconnection": - self._close_connection("phone requested disconnection") - elif member == "Release": - self._registered = False - else: - raise RuntimeError(f"Unsupported profile call: {member}") - self.router.send(new_method_return(message)) - except Exception as error: - cloudlog.exception(f"phone_gpsd: profile call {member} failed") - try: - self.router.send(new_error(message, "org.bluez.Error.Rejected", "s", (str(error),))) - except Exception: - pass - - def _on_new_connection(self, device_path: str, fd_obj) -> None: - fd = fd_obj.to_raw_fd() - with self._state_lock: - if self._fd is not None: - os.close(fd) - return - self._fd = fd - self._device_path = device_path - cloudlog.warning(f"phone_gpsd: connected to {device_path}") - self._publish_status(device_path, None, None) - threading.Thread(target=self._read_loop, args=(fd, device_path), daemon=True).start() + return unwrap_variant(reply.body[0]) if reply.body else {} @staticmethod - def _publish_status(device_path: str, last_data: float | None, last_fix: float | None) -> None: + def _publish_status(address: 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) + write_phone_status(address, last_data, last_fix) except OSError: pass def _close_connection(self, reason: str) -> None: with self._state_lock: - fd, self._fd = self._fd, None - device_path, self._device_path = self._device_path, "" - if fd is None: + sock, self._sock = self._sock, None + if sock is None: return try: - os.close(fd) + sock.close() except OSError: pass clear_phone_fix() clear_phone_fix(PHONE_GPS_STATUS_PATH) - cloudlog.warning(f"phone_gpsd: disconnected from {device_path} ({reason})") + cloudlog.warning(f"phone_gpsd: disconnected ({reason})") - def _read_loop(self, fd: int, device_path: str) -> None: + def _read_loop(self, sock: socket.socket, address: str, initial: bytes) -> None: accumulator = NmeaAccumulator() + sock.settimeout(1.0) last_data = time.monotonic() last_fix: float | None = None last_status = 0.0 first_fix = True reason = "stopped" + data = initial try: while not self._stop.is_set(): with self._state_lock: - if self._fd != fd: + if self._sock is not sock: return - readable, _, _ = select.select([fd], [], [], 1.0) now = time.monotonic() - if not readable: - if now - last_data > STALL_TIMEOUT_S: - reason = "no data" - break + publish_now = False + if data: + last_data = now + for fix in accumulator.feed_bytes(data): + write_phone_fix(fix) + last_fix = now + if first_fix: + first_fix = False + publish_now = True # flip the settings row to "streaming" right away, not on the next tick + cloudlog.warning(f"phone_gpsd: first fix {fix['latitude']:.5f},{fix['longitude']:.5f} sats={fix['satellites']}") + elif now - last_data > STALL_TIMEOUT_S: + reason = "no data" + break + if publish_now or now - last_status >= 1.0: + last_status = now + self._publish_status(address, last_data, last_fix) + try: + data = sock.recv(4096) + except TimeoutError: + data = b"" continue - data = os.read(fd, 4096) if not data: reason = "closed by phone" break - 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) + reason = error.strerror or str(error) self._close_connection(reason) + def _connect_phone(self, address: str, name: str) -> bool: + ports = browse_serial_ports(address) + if not ports: + raise RuntimeError("phone offers no serial port - is the GPS app's Bluetooth stream running?") + tried = [] + for service, channel in ports: + sock, initial, failure = probe_nmea(address, channel) + if sock is None: + tried.append(f"{service or '?'} ch{channel}: {failure}") + continue + with self._state_lock: + self._sock = sock + cloudlog.warning(f"phone_gpsd: connected to {name} via \"{service}\" (channel {channel})") + self._publish_status(address, time.monotonic(), None) + threading.Thread(target=self._read_loop, args=(sock, address, initial), daemon=True).start() + return True + raise RuntimeError("no serial port sent NMEA (" + "; ".join(tried) + ")") + def _connect_candidates(self) -> None: - body = self._call("/", OBJECT_MANAGER, "GetManagedObjects") - objects = unwrap_variant(body[0]) if body else {} + objects = self._managed_objects() # 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 @@ -189,29 +188,27 @@ class PhoneGpsDaemon: delay, retry_at = self._retry_after.get(path, (0.0, 0.0)) if now < retry_at: continue + name = str(props.get("Alias") or props.get("Address") or path) try: - self._call(path, DEVICE_IFACE, "ConnectProfile", "s", (SPP_UUID,), timeout=25.0) - self._retry_after.pop(path, None) - return + if self._connect_phone(str(props["Address"]), name): + self._retry_after.pop(path, None) + return except Exception as error: delay = min(RETRY_MAX_S, max(RETRY_MIN_S, delay * 2)) self._retry_after[path] = (delay, time.monotonic() + delay) - cloudlog.warning(f"phone_gpsd: SPP connect to {props.get('Alias') or path} failed ({error}); retry in {delay:.0f}s") + cloudlog.warning(f"phone_gpsd: {name}: {error}; retry in {delay:.0f}s") 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: - self._register_profile() with self._state_lock: - connected = self._fd is not None + connected = self._sock is not None if not connected: self._connect_candidates() except Exception: # BlueZ restarting or the adapter powering down; keep trying rather than exiting. - self._registered = False cloudlog.exception("phone_gpsd: loop error") self._stop.wait(CONNECT_POLL_S) @@ -221,15 +218,6 @@ class PhoneGpsDaemon: return self._stopped = True self._close_connection("shutting down") - try: - self._call("/org/bluez", PROFILE_MANAGER_IFACE, "UnregisterProfile", "o", (PROFILE_PATH,), timeout=5.0) - except Exception: - pass - try: - self._profile_queue.put_nowait(None) - except Exception: - pass - self._profile_filter.__exit__(None, None, None) self.router.close() diff --git a/starpilot/system/bluetooth/phone_gps_fix.py b/starpilot/system/bluetooth/phone_gps_fix.py index 065699d8fd..5ab25ef436 100644 --- a/starpilot/system/bluetooth/phone_gps_fix.py +++ b/starpilot/system/bluetooth/phone_gps_fix.py @@ -11,6 +11,7 @@ import datetime import json import math import os +import re import time PHONE_GPS_FIX_PATH = "/dev/shm/starpilot_phone_gps.json" @@ -43,6 +44,40 @@ def nmea_checksum_ok(sentence: str) -> bool: return False +SERIAL_PORT_CLASS = '"Serial Port" (0x1101)' +# Android registers some of its own services with the Serial Port class too (Nearby Share showed up next to +# the GPS app on a Pixel 8 Pro); they never carry NMEA and connecting to them just gets reset. +IGNORED_SERIAL_SERVICES = ("nearbysharing", "nearby") +GPS_SERVICE_HINTS = ("gps", "nmea", "gnss") + + +def parse_serial_ports(sdp_output: str) -> list[tuple[str, int]]: + """(service name, RFCOMM channel) for each Serial Port record in `sdptool browse` output, GPS apps first. + + A phone can advertise several serial ports at once (GPS apps, Nearby Share, ...), so picking the first one, + as BlueZ's ConnectProfile does, can land on a service that never sends NMEA. + """ + ports = [] + for record in re.split(r"\n\s*\n", sdp_output): + if SERIAL_PORT_CLASS not in record: + continue + channel = re.search(r"Channel: (\d+)", record) + if channel is None: + continue + name_match = re.search(r"Service Name: (.*)", record) + name = name_match.group(1).strip() if name_match else "" + squashed = name.lower().replace(" ", "") + if any(ignored in squashed for ignored in IGNORED_SERIAL_SERVICES): + continue + ports.append((name, int(channel.group(1)))) + ports.sort(key=lambda port: not any(hint in port[0].lower() for hint in GPS_SERVICE_HINTS)) + return ports + + +def contains_nmea(data: bytes) -> bool: + return any(nmea_checksum_ok(line) for line in data.decode("ascii", errors="ignore").splitlines()) + + def _coordinate(value: str, hemisphere: str) -> float | None: # NMEA packs coordinates as (d)ddmm.mmmm; the degree digits are everything before the last two # integer digits. diff --git a/starpilot/system/bluetooth/tests/test_phone_gps.py b/starpilot/system/bluetooth/tests/test_phone_gps.py new file mode 100644 index 0000000000..79c7e6eeed --- /dev/null +++ b/starpilot/system/bluetooth/tests/test_phone_gps.py @@ -0,0 +1,87 @@ +import functools +import socket +import threading + +import pytest + +import openpilot.starpilot.system.bluetooth.phone_gps as phone_gps +import openpilot.starpilot.system.bluetooth.phone_gps_fix as fix_module + + +def sentence(body: str) -> bytes: + checksum = 0 + for char in body: + checksum ^= ord(char) + return f"${body}*{checksum:02X}\r\n".encode() + + +GGA = sentence("GNGGA,153012.00,2613.5790,N,09817.4880,W,1,11,0.8,34.2,M,-24.1,M,,") +RMC = sentence("GNRMC,153012.00,A,2613.5790,N,09817.4880,W,30.5,84.4,280926,,,A") + + +@pytest.fixture +def daemon(monkeypatch, tmp_path): + fix_path, status_path = str(tmp_path / "fix.json"), str(tmp_path / "status.json") + monkeypatch.setattr(phone_gps, "write_phone_fix", functools.partial(fix_module.write_phone_fix, path=fix_path)) + monkeypatch.setattr(phone_gps, "write_phone_status", functools.partial(fix_module.write_phone_status, path=status_path)) + monkeypatch.setattr(phone_gps, "PHONE_GPS_STATUS_PATH", status_path) + monkeypatch.setattr(phone_gps, "clear_phone_fix", lambda path=fix_path: fix_module.clear_phone_fix(path)) + # No D-Bus: _connect_phone and _read_loop don't touch the router. + d = object.__new__(phone_gps.PhoneGpsDaemon) + d._state_lock = threading.Lock() + d._stop = threading.Event() + d._sock = None + d._retry_after = {} + d.fix_path, d.status_path = fix_path, status_path + return d + + +def test_connect_skips_ports_that_do_not_speak_nmea(daemon, monkeypatch): + monkeypatch.setattr(phone_gps, "browse_serial_ports", lambda _address: [("GPS NMEA Tether", 16), ("BT1", 21)]) + ours, theirs = socket.socketpair() + probed = [] + + def probe(_address, channel): + probed.append(channel) + return (None, b"", "Device or resource busy") if channel == 16 else (ours, GGA, "") + monkeypatch.setattr(phone_gps, "probe_nmea", probe) + monkeypatch.setattr(threading, "Thread", lambda **_kwargs: type("T", (), {"start": lambda self: None})()) + + assert daemon._connect_phone("D4:3A:2C:63:2A:50", "Pixel 8 Pro") + assert probed == [16, 21] + assert daemon._sock is ours + theirs.close() + ours.close() + + +def test_connect_reports_every_failed_port(daemon, monkeypatch): + monkeypatch.setattr(phone_gps, "browse_serial_ports", lambda _address: [("GPS NMEA Tether", 16), ("BT1", 21)]) + monkeypatch.setattr(phone_gps, "probe_nmea", lambda _address, channel: (None, b"", "Connection refused")) + with pytest.raises(RuntimeError, match="GPS NMEA Tether ch16: Connection refused; BT1 ch21: Connection refused"): + daemon._connect_phone("D4:3A:2C:63:2A:50", "Pixel 8 Pro") + + monkeypatch.setattr(phone_gps, "browse_serial_ports", lambda _address: []) + with pytest.raises(RuntimeError, match="no serial port"): + daemon._connect_phone("D4:3A:2C:63:2A:50", "Pixel 8 Pro") + + +def test_read_loop_writes_fixes_and_cleans_up_when_phone_closes(daemon): + ours, theirs = socket.socketpair() + daemon._sock = ours + reader = threading.Thread(target=daemon._read_loop, args=(ours, "D4:3A:2C:63:2A:50", GGA)) + reader.start() + theirs.sendall(RMC) + + for _ in range(50): + if fix_module.read_phone_fix(daemon.fix_path) is not None: + break + threading.Event().wait(0.05) + assert fix_module.read_phone_fix(daemon.fix_path)["satellites"] == 11 + assert fix_module.read_phone_status(daemon.status_path)[1] == "streaming" + + theirs.close() + reader.join(timeout=5) + assert not reader.is_alive() + assert daemon._sock is None + assert fix_module.read_phone_fix(daemon.fix_path) is None + assert fix_module.read_phone_status(daemon.status_path) == ("", "") diff --git a/starpilot/system/bluetooth/tests/test_phone_gps_fix.py b/starpilot/system/bluetooth/tests/test_phone_gps_fix.py index a546b199ee..f23b157060 100644 --- a/starpilot/system/bluetooth/tests/test_phone_gps_fix.py +++ b/starpilot/system/bluetooth/tests/test_phone_gps_fix.py @@ -4,8 +4,8 @@ import json import pytest 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) + contains_nmea, nmea_checksum_ok, parse_serial_ports, phone_fix_fields, + read_phone_fix, read_phone_status, write_phone_fix, write_phone_status) def sentence(body: str) -> str: @@ -118,6 +118,48 @@ def test_phone_status_states(tmp_path): assert read_phone_status(path, now=101.0) == ("", "") +def sdp_record(name: str, channel: int, service_class: str = '"Serial Port" (0x1101)') -> str: + return "\n".join([ + f"Service Name: {name}", + f"Service RecHandle: 0x100{channel:02d}", + "Service Class ID List:", + f" {service_class}", + "Protocol Descriptor List:", + ' "L2CAP" (0x0100)', + ' "RFCOMM" (0x0003)', + f" Channel: {channel}", + "Profile Descriptor List:", + f" {service_class}", + " Version: 0x0102", + "", + ]) + + +# Modeled on a Pixel 8 Pro running GPS NMEA Tether plus a second GPS app ("BT1") and Nearby Share. +PIXEL_SDP = "Browsing D4:3A:2C:63:2A:50 ...\n" + "\n".join([ + sdp_record("Headset Gateway", 3, '"Headset Audio Gateway" (0x1112)'), + sdp_record("Handsfree Gateway", 4, '"Handsfree Audio Gateway" (0x111f)'), + sdp_record("BT1", 21), + sdp_record("NearbySharing", 20), + sdp_record("GPS NMEA Tether", 16), +]) + + +def test_parse_serial_ports_prefers_gps_apps_and_skips_android_services(): + assert parse_serial_ports(PIXEL_SDP) == [("GPS NMEA Tether", 16), ("BT1", 21)] + assert parse_serial_ports("Browsing D4:3A:2C:63:2A:50 ...\n") == [] + assert parse_serial_ports(sdp_record("Handsfree Gateway", 4, '"Handsfree Audio Gateway" (0x111f)')) == [] + + +def test_contains_nmea(): + # A real capture from GPS NMEA Tether with no satellite lock; the proprietary $PGLOR line has a bogus checksum. + pglor = b"$PGLOR,12,STA,151137.08,0.000,0.225,249,297,9999,0,P,F,L,1,C,0,S,00000000,0,2,R,00000000,TPEF,41,84105,LC,,,DR,0,,*00\r\n" + gga = b"$GPGGA,151137.08,,,,,0,00,999.9,,M,,M,,*6E\r\n" + assert contains_nmea(pglor + gga) + assert not contains_nmea(pglor) + assert not contains_nmea(b"\x00\x01binary junk") + + def test_phone_fix_fields(): fields = phone_fix_fields(NmeaAccumulator().feed_line(RMC)) assert fields["hasFix"]