From ee223ec1f2c65c4e755d6a2e0920b1975a3ab0ea Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 28 Sep 2026 14:20:11 -0500 Subject: [PATCH] GPS over phone bluetooth --- selfdrive/ui/onroad/starpilot/gnss_health.py | 20 +- starpilot/system/bluetooth/bluez.py | 5 +- starpilot/system/bluetooth/phone_gps.py | 229 ++++++++++++++++++ starpilot/system/bluetooth/phone_gps_fix.py | 222 +++++++++++++++++ starpilot/system/bluetooth/protocol.py | 9 +- .../system/bluetooth/tests/test_bluetooth.py | 12 +- .../bluetooth/tests/test_phone_gps_fix.py | 109 +++++++++ system/manager/process_config.py | 7 + system/qcomgpsd/qcomgpsd.py | 28 +++ 9 files changed, 633 insertions(+), 8 deletions(-) create mode 100644 starpilot/system/bluetooth/phone_gps.py create mode 100644 starpilot/system/bluetooth/phone_gps_fix.py create mode 100644 starpilot/system/bluetooth/tests/test_phone_gps_fix.py diff --git a/selfdrive/ui/onroad/starpilot/gnss_health.py b/selfdrive/ui/onroad/starpilot/gnss_health.py index b5e261f040..dda82313be 100644 --- a/selfdrive/ui/onroad/starpilot/gnss_health.py +++ b/selfdrive/ui/onroad/starpilot/gnss_health.py @@ -6,8 +6,11 @@ with it plugged in that collapses to 0% while the tracked satellite count actual satellite counts are therefore misleading on their own - the demodulation rate and C/No are what show whether a cable, ferrite or antenna placement change helped. """ +import time + import pyray as rl +from cereal import log from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.text_measure import measure_text_cached @@ -37,6 +40,9 @@ CNO_WARN = 800.0 # Right edges already match mathematically; this is a small optical correction for "R"'s shape. SATS_OPTICAL_NUDGE = 3 +# gpsLocation arrives at ~1Hz; older than this means nothing is publishing a fix any more. +FIX_STALE_S = 2.0 + def _grade(value: float, good: float, warn: float) -> rl.Color: if value >= good: @@ -56,15 +62,22 @@ class GnssHealth: self._gps_sv = 0 self._glonass_sv = 0 self._has_fix = False + self._phone_fix = False def _update(self) -> None: sm = ui_state.sm # qcomgpsd publishes gpsLocation; gpsLocationExternal is only used by ublox/car-GPS devices, # so checking that socket alone leaves hasFix stuck False on this hardware. + # qcomgpsd publishes nothing while it has neither its own fix nor a phone fix, so a stale message + # (rather than a fresh fixless one) is how "no fix" usually shows up - don't keep showing the last fix. + self._has_fix = False + self._phone_fix = False for service in ("gpsLocation", "gpsLocationExternal"): if sm.valid.get(service, False) and sm.recv_frame[service] > 0: - self._has_fix = sm[service].hasFix + fresh = time.monotonic() - sm.recv_time[service] < FIX_STALE_S + self._has_fix = fresh and sm[service].hasFix + self._phone_fix = self._has_fix and sm[service].source == log.GpsLocationData.SensorSource.android break if not sm.valid.get("qcomGnss", False): @@ -119,8 +132,9 @@ class GnssHealth: ty = int(y + PADDING) rl.draw_text_ex(self._font, "GNSS", rl.Vector2(tx, ty), TITLE_SIZE, 0, _LABEL) - fix_text = "FIX" if self._has_fix else "NO FIX" - fix_color = _GOOD if self._has_fix else _BAD + # A phone fix is shown in amber: the car has a position, but the comma's own receiver still doesn't. + fix_text = "PHONE FIX" if self._phone_fix else ("FIX" if self._has_fix else "NO FIX") + fix_color = _WARN if self._phone_fix else (_GOOD if self._has_fix else _BAD) fix_width = measure_text_cached(self._font, fix_text, TITLE_SIZE).x rl.draw_text_ex(self._font, fix_text, rl.Vector2(right - fix_width, ty), TITLE_SIZE, 0, fix_color) diff --git a/starpilot/system/bluetooth/bluez.py b/starpilot/system/bluetooth/bluez.py index 12de66a157..bfa6799dc6 100644 --- a/starpilot/system/bluetooth/bluez.py +++ b/starpilot/system/bluetooth/bluez.py @@ -9,7 +9,7 @@ from jeepney.io.threading import DBusRouter, open_dbus_connection from jeepney.low_level import HeaderFields, MessageType from jeepney.wrappers import Properties -from openpilot.starpilot.system.bluetooth.protocol import device_capabilities, show_pairing_device +from openpilot.starpilot.system.bluetooth.protocol import device_capabilities, is_phone, show_pairing_device BLUEZ = "org.bluez" @@ -236,6 +236,7 @@ class BlueZClient: 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", ""))) + phone = is_phone(int(props.get("Class", 0)), str(props.get("Icon", ""))) device = { "path": path, "address": str(props.get("Address", "")), @@ -250,7 +251,7 @@ class BlueZClient: "controller": controller, } 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, include_discovering, phone): devices.append(device) return sorted(devices, key=lambda device: (not device["connected"], not device["paired"], -(device["rssi"] or -127), device["name"].lower())) diff --git a/starpilot/system/bluetooth/phone_gps.py b/starpilot/system/bluetooth/phone_gps.py new file mode 100644 index 0000000000..eec14f9948 --- /dev/null +++ b/starpilot/system/bluetooth/phone_gps.py @@ -0,0 +1,229 @@ +"""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. +""" +import os +import select +import signal +import threading +import time + +from jeepney import DBusAddress, MatchRule, new_error, new_method_call, new_method_return +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.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 +# The app streams at ~1Hz, so this long with no bytes means the link or the app has died. +STALL_TIMEOUT_S = 10.0 + + +def is_phone_candidate(props: dict) -> bool: + if not props.get("Paired", False) or props.get("Blocked", False): + return False + uuids = {str(uuid).lower() for uuid in props.get("UUIDs", [])} + return SPP_UUID in uuids or is_phone(int(props.get("Class", 0)), str(props.get("Icon", ""))) + + +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._state_lock = threading.Lock() + self._stop = threading.Event() + self._stopped = False + self._registered = False + self._fd: int | None = None + self._device_path = "" + 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) + 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}") + threading.Thread(target=self._read_loop, args=(fd,), daemon=True).start() + + 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: + return + try: + os.close(fd) + except OSError: + pass + clear_phone_fix() + cloudlog.warning(f"phone_gpsd: disconnected from {device_path} ({reason})") + + def _read_loop(self, fd: int) -> None: + accumulator = NmeaAccumulator() + last_data = time.monotonic() + first_fix = True + reason = "stopped" + try: + while not self._stop.is_set(): + with self._state_lock: + if self._fd != fd: + return + readable, _, _ = select.select([fd], [], [], 1.0) + now = time.monotonic() + if not readable: + if now - last_data > STALL_TIMEOUT_S: + reason = "no data" + break + 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) + if first_fix: + first_fix = False + cloudlog.warning(f"phone_gpsd: first fix {fix['latitude']:.5f},{fix['longitude']:.5f} sats={fix['satellites']}") + except OSError as error: + reason = str(error) + self._close_connection(reason) + + def _connect_candidates(self) -> None: + body = self._call("/", OBJECT_MANAGER, "GetManagedObjects") + objects = unwrap_variant(body[0]) if body else {} + now = time.monotonic() + for path, interfaces in objects.items(): + props = interfaces.get(DEVICE_IFACE) + if props is None or not is_phone_candidate(props): + continue + delay, retry_at = self._retry_after.get(path, (0.0, 0.0)) + if now < retry_at: + continue + try: + self._call(path, DEVICE_IFACE, "ConnectProfile", "s", (SPP_UUID,), timeout=25.0) + 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") + + def run(self) -> None: + clear_phone_fix() + while not self._stop.is_set(): + try: + if not self._registered: + self._register_profile() + with self._state_lock: + connected = self._fd 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) + + def stop(self) -> None: + self._stop.set() + if self._stopped: + 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() + + +def main() -> None: + daemon = PhoneGpsDaemon() + + def handle_signal(_signum, _frame): + # Only flag it here; run() returns within CONNECT_POLL_S and stop() then cleans up exactly once. + daemon._stop.set() + + signal.signal(signal.SIGTERM, handle_signal) + signal.signal(signal.SIGINT, handle_signal) + daemon.run() + daemon.stop() + + +if __name__ == "__main__": + main() diff --git a/starpilot/system/bluetooth/phone_gps_fix.py b/starpilot/system/bluetooth/phone_gps_fix.py new file mode 100644 index 0000000000..188401bf4c --- /dev/null +++ b/starpilot/system/bluetooth/phone_gps_fix.py @@ -0,0 +1,222 @@ +"""Phone GPS fallback: NMEA parsing and the fix hand-off between phone_gpsd and qcomgpsd. + +The comma's own GNSS receiver is badly desensed by the eGPU's USB 3 link and can take minutes to get a +fix (or never get one). A phone running an NMEA-over-Bluetooth app almost always already has a fix, so +phone_gpsd reads its stream and qcomgpsd publishes it on gpsLocation whenever the modem has nothing. + +This module is deliberately stdlib-only: qcomgpsd imports it, and nothing here may be able to take +qcomgpsd down. Every reader path swallows errors and returns None. +""" +import datetime +import json +import math +import os +import time + +PHONE_GPS_FIX_PATH = "/dev/shm/starpilot_phone_gps.json" + +# 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. +PHONE_FIX_MAX_AGE_S = 3.0 + +KNOTS_TO_MS = 0.514444 + + +def nmea_checksum_ok(sentence: str) -> bool: + sentence = sentence.strip() + if not sentence.startswith("$") or "*" not in sentence: + return False + body, _, checksum = sentence[1:].partition("*") + if len(checksum) < 2: + return False + calculated = 0 + for char in body: + calculated ^= ord(char) + try: + return calculated == int(checksum[:2], 16) + except ValueError: + return False + + +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. + if not value or hemisphere not in ("N", "S", "E", "W"): + return None + try: + dot = value.index(".") if "." in value else len(value) + degrees = float(value[:dot - 2]) + minutes = float(value[dot - 2:]) + except ValueError: + return None + if not 0.0 <= minutes < 60.0: + return None + result = degrees + minutes / 60.0 + return -result if hemisphere in ("S", "W") else result + + +def _float(value: str) -> float | None: + try: + result = float(value) + except (TypeError, ValueError): + return None + return result if math.isfinite(result) else None + + +def _unix_ms(date: str, clock: str) -> int | None: + # RMC carries ddmmyy + hhmmss(.ss) in UTC. + try: + day, month, year = int(date[0:2]), int(date[2:4]), 2000 + int(date[4:6]) + hour, minute = int(clock[0:2]), int(clock[2:4]) + seconds = float(clock[4:]) + whole = int(seconds) + stamp = datetime.datetime(year, month, day, hour, minute, whole, int(round((seconds - whole) * 1e6)), + tzinfo=datetime.UTC) + except (ValueError, IndexError): + return None + return int(stamp.timestamp() * 1000) + + +class NmeaAccumulator: + """Combines per-epoch RMC (position/speed/course/date) and GGA (fix quality/sats/HDOP/altitude). + + A fix is emitted on each valid RMC. GGA is merged in when it belongs to the same epoch, matched on + the UTC time-of-day field, since apps differ on which of the two they send first. + """ + + def __init__(self): + self._gga: dict | None = None + self._gga_time = "" + self._buffer = "" + + def feed_bytes(self, data: bytes) -> list[dict]: + self._buffer += data.decode("ascii", errors="ignore") + # Bound the buffer in case the stream is not NMEA at all. + if len(self._buffer) > 8192: + self._buffer = self._buffer[-1024:] + fixes = [] + while "\n" in self._buffer: + line, self._buffer = self._buffer.split("\n", 1) + fix = self.feed_line(line) + if fix is not None: + fixes.append(fix) + return fixes + + def feed_line(self, line: str) -> dict | None: + line = line.strip() + if not nmea_checksum_ok(line): + return None + fields = line[1:line.index("*")].split(",") + kind = fields[0][-3:] + if kind == "GGA": + self._parse_gga(fields) + return None + if kind == "RMC": + return self._parse_rmc(fields) + return None + + def _parse_gga(self, fields: list[str]) -> None: + if len(fields) < 10: + return + try: + quality = int(fields[6] or 0) + except ValueError: + return + try: + satellites = int(fields[7] or 0) + except ValueError: + satellites = 0 + self._gga = { + "quality": quality, + "satellites": satellites, + "hdop": _float(fields[8]), + "altitude": _float(fields[9]), + } + self._gga_time = fields[1] + + def _parse_rmc(self, fields: list[str]) -> dict | None: + if len(fields) < 10 or fields[2] != "A": + return None + latitude = _coordinate(fields[3], fields[4]) + longitude = _coordinate(fields[5], fields[6]) + unix_ms = _unix_ms(fields[9], fields[1]) + if latitude is None or longitude is None or unix_ms is None: + return None + if not (-90.0 <= latitude <= 90.0 and -180.0 <= longitude <= 180.0): + return None + + speed_knots = _float(fields[7]) + course = _float(fields[8]) + gga = self._gga if self._gga is not None and self._gga_time == fields[1] else None + if gga is not None and gga["quality"] <= 0: + return None + + return { + "latitude": latitude, + "longitude": longitude, + "altitude": gga["altitude"] if gga and gga["altitude"] is not None else 0.0, + "speed": max(0.0, speed_knots * KNOTS_TO_MS) if speed_knots is not None else 0.0, + "bearing_deg": course % 360.0 if course is not None else 0.0, + "bearing_valid": course is not None, + "unix_ms": unix_ms, + "satellites": gga["satellites"] if gga else 0, + "hdop": gga["hdop"] if gga else None, + } + + +def write_phone_fix(fix: dict, path: str = PHONE_GPS_FIX_PATH, now: float | None = None) -> None: + payload = dict(fix) + # CLOCK_MONOTONIC is system-wide on Linux, so qcomgpsd can age this against its own clock. + payload["mono"] = time.monotonic() if now is None else now + tmp_path = f"{path}.tmp" + with open(tmp_path, "w") as f: + json.dump(payload, f) + os.replace(tmp_path, path) + + +def clear_phone_fix(path: str = PHONE_GPS_FIX_PATH) -> None: + try: + os.remove(path) + except OSError: + pass + + +def read_phone_fix(path: str = PHONE_GPS_FIX_PATH, max_age: float = PHONE_FIX_MAX_AGE_S, + now: float | None = None) -> dict | None: + try: + with open(path) as f: + fix = json.load(f) + now = time.monotonic() if now is None else now + age = now - float(fix["mono"]) + if not 0.0 <= age <= max_age: + return None + float(fix["latitude"]) + float(fix["longitude"]) + return fix + except Exception: + return None + + +def phone_fix_fields(fix: dict) -> dict: + """gpsLocation field values for a phone fix. Accuracies are conservative estimates from HDOP.""" + hdop = fix.get("hdop") + horizontal = max(1.0, float(hdop) * 5.0) if hdop else 10.0 + speed = float(fix.get("speed", 0.0)) + bearing = float(fix.get("bearing_deg", 0.0)) + bearing_rad = math.radians(bearing) + moving = speed > 1.0 and bool(fix.get("bearing_valid", False)) + return { + "latitude": float(fix["latitude"]), + "longitude": float(fix["longitude"]), + "altitude": float(fix.get("altitude", 0.0)), + "speed": speed, + "bearingDeg": bearing, + "unixTimestampMillis": int(fix["unix_ms"]), + "vNED": [speed * math.cos(bearing_rad), speed * math.sin(bearing_rad), 0.0], + "horizontalAccuracy": horizontal, + "verticalAccuracy": horizontal * 1.5, + "bearingAccuracyDeg": max(5.0, horizontal) if moving else 180.0, + "speedAccuracy": 0.5, + "hasFix": True, + "satelliteCount": int(fix.get("satellites", 0)), + } diff --git a/starpilot/system/bluetooth/protocol.py b/starpilot/system/bluetooth/protocol.py index 8c648ccd47..8a7ba43e19 100644 --- a/starpilot/system/bluetooth/protocol.py +++ b/starpilot/system/bluetooth/protocol.py @@ -94,13 +94,18 @@ def device_capabilities(uuids: list[str] | tuple[str, ...], bluetooth_class: int return audio, controller +def is_phone(bluetooth_class: int = 0, icon: str = "") -> bool: + # Phones are listed so they can be paired as a GPS source for phone_gpsd. + return ((int(bluetooth_class) >> 8) & 0x1F) == 0x02 or icon == "phone" + + 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, discovering: bool = False, phone: 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 phone)) class _DesktopFakeBluetooth: diff --git a/starpilot/system/bluetooth/tests/test_bluetooth.py b/starpilot/system/bluetooth/tests/test_bluetooth.py index 019d65f31b..cd73a77a24 100644 --- a/starpilot/system/bluetooth/tests/test_bluetooth.py +++ b/starpilot/system/bluetooth/tests/test_bluetooth.py @@ -9,7 +9,7 @@ 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, - device_capabilities, show_pairing_device) + device_capabilities, is_phone, show_pairing_device) from openpilot.system import hardware from openpilot.system.ui.lib.bluetooth_manager import BluetoothManager @@ -186,6 +186,16 @@ def test_pairing_list_filters_anonymous_and_irrelevant_advertisements(): 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) + # Phones are listed so they can be paired as the phone_gpsd GPS source. + assert show_pairing_device("00:11:22:33:44:55", "Pixel 8 Pro", False, False, False, False, False, False, True, phone=True) + assert not show_pairing_device("00:11:22:33:44:55", "Pixel 8 Pro", False, False, False, True, False, False, True, phone=True) + + +def test_phone_detection(): + assert is_phone(0x5A020C) # smartphone: major class 0x02 + assert is_phone(icon="phone") + assert not is_phone(0x240404) # headset: major class 0x04 + assert not is_phone() def test_desktop_fake_bluetooth_is_stateful_and_interactive(monkeypatch, tmp_path): diff --git a/starpilot/system/bluetooth/tests/test_phone_gps_fix.py b/starpilot/system/bluetooth/tests/test_phone_gps_fix.py new file mode 100644 index 0000000000..0800c7b4a2 --- /dev/null +++ b/starpilot/system/bluetooth/tests/test_phone_gps_fix.py @@ -0,0 +1,109 @@ +import datetime +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) + + +def sentence(body: str) -> str: + checksum = 0 + for char in body: + checksum ^= ord(char) + return f"${body}*{checksum:02X}" + + +RMC = sentence("GNRMC,153012.00,A,2613.5790,N,09817.4880,W,30.5,84.4,280926,,,A") +GGA = sentence("GNGGA,153012.00,2613.5790,N,09817.4880,W,1,11,0.8,34.2,M,-24.1,M,,") + + +def test_checksum(): + assert nmea_checksum_ok(RMC) + assert nmea_checksum_ok(RMC + "\r\n") + assert not nmea_checksum_ok(RMC[:-2] + "00") + assert not nmea_checksum_ok(RMC.replace("*", "")) + assert not nmea_checksum_ok("GNRMC,no,dollar*00") + + +def test_rmc_with_matching_gga(): + acc = NmeaAccumulator() + assert acc.feed_line(GGA) is None + fix = acc.feed_line(RMC) + assert fix is not None + assert fix["latitude"] == pytest.approx(26 + 13.5790 / 60) + assert fix["longitude"] == pytest.approx(-(98 + 17.4880 / 60)) + assert fix["speed"] == pytest.approx(30.5 * KNOTS_TO_MS) + assert fix["bearing_deg"] == pytest.approx(84.4) + assert fix["satellites"] == 11 + assert fix["hdop"] == pytest.approx(0.8) + assert fix["altitude"] == pytest.approx(34.2) + expected = datetime.datetime(2026, 9, 28, 15, 30, 12, tzinfo=datetime.UTC) + assert fix["unix_ms"] == int(expected.timestamp() * 1000) + + +def test_rmc_alone_still_gives_a_fix(): + fix = NmeaAccumulator().feed_line(RMC) + assert fix is not None + assert fix["satellites"] == 0 and fix["hdop"] is None + + +def test_gga_from_another_epoch_is_not_merged(): + acc = NmeaAccumulator() + acc.feed_line(sentence("GNGGA,153011.00,2613.5790,N,09817.4880,W,1,4,9.9,1.0,M,,M,,")) + fix = acc.feed_line(RMC) + assert fix["satellites"] == 0 + + +def test_void_rmc_and_no_fix_gga_rejected(): + assert NmeaAccumulator().feed_line(sentence("GPRMC,153012.00,V,,,,,,,280926,,,N")) is None + acc = NmeaAccumulator() + acc.feed_line(sentence("GNGGA,153012.00,2613.5790,N,09817.4880,W,0,0,,,M,,M,,")) + assert acc.feed_line(RMC) is None + + +def test_bad_checksum_and_other_sentences_ignored(): + acc = NmeaAccumulator() + assert acc.feed_line(RMC[:-2] + "00") is None + assert acc.feed_line(sentence("GPGSV,3,1,11,01,40,083,46")) is None + + +def test_feed_bytes_across_chunk_boundaries(): + acc = NmeaAccumulator() + data = (GGA + "\r\n" + RMC + "\r\n").encode() + fixes = [] + for i in range(0, len(data), 7): + fixes += acc.feed_bytes(data[i:i + 7]) + assert len(fixes) == 1 + assert fixes[0]["satellites"] == 11 + + +def test_fix_file_roundtrip_and_staleness(tmp_path): + path = str(tmp_path / "phone_gps.json") + fix = NmeaAccumulator().feed_line(RMC) + write_phone_fix(fix, path, now=100.0) + assert read_phone_fix(path, now=101.0)["latitude"] == pytest.approx(fix["latitude"]) + assert read_phone_fix(path, now=104.0) is None + assert read_phone_fix(path, now=99.0) is None + clear_phone_fix(path) + assert read_phone_fix(path, now=101.0) is None + + +def test_read_never_raises(tmp_path): + path = tmp_path / "phone_gps.json" + path.write_text("{not json") + assert read_phone_fix(str(path), now=0.0) is None + path.write_text(json.dumps({"mono": 0.0})) + assert read_phone_fix(str(path), now=0.0) is None + + +def test_phone_fix_fields(): + fields = phone_fix_fields(NmeaAccumulator().feed_line(RMC)) + assert fields["hasFix"] + assert fields["horizontalAccuracy"] == pytest.approx(10.0) + assert fields["satelliteCount"] == 0 + assert fields["bearingAccuracyDeg"] < 180.0 + + stopped = phone_fix_fields(NmeaAccumulator().feed_line(sentence("GNRMC,153012.00,A,2613.5790,N,09817.4880,W,0.0,,280926,,,A"))) + assert stopped["bearingAccuracyDeg"] == 180.0 + assert stopped["vNED"] == pytest.approx([0.0, 0.0, 0.0]) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index c2324867bb..e0a79d329a 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -129,6 +129,12 @@ 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") @@ -212,6 +218,7 @@ 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), 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/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index d2c97d56c0..8d8d18c6d5 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -23,6 +23,7 @@ from openpilot.common.time_helpers import system_time_valid from openpilot.system.hardware.tici.pins import GPIO from openpilot.common.swaglog import cloudlog from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv +from openpilot.starpilot.system.bluetooth.phone_gps_fix import phone_fix_fields, read_phone_fix from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, relist, gps_measurement_report, gps_measurement_report_sv, glonass_measurement_report, glonass_measurement_report_sv, @@ -234,6 +235,28 @@ def teardown_quectel(diag): try_setup_logs(diag, []) +def send_phone_fallback(pm) -> bool: + """Publish the phone's latest fix (from phone_gpsd) on gpsLocation. Returns True if one was sent. + + The modem's receiver is desensed by the eGPU's USB 3 link and can go minutes without a fix, so a phone + streaming NMEA over Bluetooth stands in until it recovers. Any failure here must not take qcomgpsd down. + """ + try: + fix = read_phone_fix() + if fix is None: + return False + msg = messaging.new_message('gpsLocation', valid=True) + gps = msg.gpsLocation + for name, value in phone_fix_fields(fix).items(): + setattr(gps, name, value) + gps.source = log.GpsLocationData.SensorSource.android + pm.send('gpsLocation', msg) + return True + except Exception: + cloudlog.exception("phone GPS fallback failed") + return False + + def wait_for_modem(): cloudlog.warning("waiting for modem to come up") while True: @@ -364,6 +387,8 @@ def main() -> NoReturn: elif log_type == LOG_GNSS_POSITION_REPORT: report = unpack_position(log_payload) if report["u_PosSource"] != 2: + # No Kalman solution from the modem this epoch - publish the phone's fix instead if one is fresh. + send_phone_fallback(pm) continue vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]] vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]] @@ -391,6 +416,9 @@ def main() -> NoReturn: if gps.hasFix: want_assistance = False stop_download_event.set() + elif send_phone_fallback(pm): + # The modem's own report is fixless; the phone's fix went out in its place this epoch. + continue pm.send('gpsLocation', msg) elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: