This commit is contained in:
whoisdomi
2026-09-29 10:40:09 -05:00
parent 244dccfae2
commit f20731225f
4 changed files with 277 additions and 125 deletions
+111 -123
View File
@@ -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()
@@ -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.
@@ -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) == ("", "")
@@ -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"]