diff --git a/selfdrive/ui/onroad/starpilot/gnss_health.py b/selfdrive/ui/onroad/starpilot/gnss_health.py new file mode 100644 index 0000000000..73d59a9643 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/gnss_health.py @@ -0,0 +1,140 @@ +"""Onroad GNSS health readout for diagnosing RF desense from the eGPU's USB-C link. + +Under desense the receiver keeps tracking satellites (the count can even rise) but decodes satellite +time from none of them, so the decode rate is the signal to watch, not the satellite count. +""" +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 + +WIDTH = 320 +HEIGHT = 150 +MARGIN = 30 +PADDING = 14 +TITLE_SIZE = 26 +ROW_SIZE = 30 + +_BG = rl.Color(0, 0, 0, 180) +_LABEL = rl.Color(255, 255, 255, 140) +_GOOD = rl.Color(34, 197, 94, 255) +_WARN = rl.Color(234, 179, 8, 255) +_BAD = rl.Color(239, 68, 68, 255) + +# Under ~40% has never produced a fix in logged drives. +SAT_TIME_GOOD = 60.0 +SAT_TIME_WARN = 25.0 + +# Modem's raw carrier-noise units, ~2550 on a clean fix. It can stay high while desense blocks decoding, +# so it isn't a health signal on its own. +CNO_GOOD = 2000.0 +CNO_WARN = 800.0 + +# "R"'s diagonal leg looks left of its edge next to a digit, even though both rows end at the same x. +SATS_OPTICAL_NUDGE = 3 + +# gpsLocation arrives at ~1Hz. +FIX_STALE_S = 2.0 + + +def _grade(value: float, good: float, warn: float) -> rl.Color: + if value >= good: + return _GOOD + if value >= warn: + return _WARN + return _BAD + + +class GnssHealth: + def __init__(self): + self._font = gui_app.font(FontWeight.SEMI_BOLD) + self._sat_time_pct = 0.0 + self._cno = 0.0 + 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 devices publish gpsLocation; gpsLocationExternal is only ublox/car GPS. qcomgpsd usually + # signals "no fix" by publishing nothing at all, so a stale message has to count as no 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: + 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): + return + + # sm["qcomGnss"] is only the last message of a burst, usually not the measurementReport. + for msg in sm.drained.get("qcomGnss", []): + if msg.which() != "qcomGnss": + continue + gnss = msg.qcomGnss + if gnss.which() != "measurementReport": + continue + + report = gnss.measurementReport + svs = list(report.sv) + source = str(report.source) + + # The modem never sets satelliteTimeIsKnown on GLONASS satellites, so decode rate is GPS-only. + is_glonass = "glonass" in source + if is_glonass: + self._glonass_sv = len(svs) + else: + self._gps_sv = len(svs) + + if svs and not is_glonass: + known = sum(1 for sv in svs if sv.measurementStatus.satelliteTimeIsKnown) + self._sat_time_pct = 100.0 * known / len(svs) + + if svs: + noise = [sv.carrierNoise for sv in svs if sv.carrierNoise > 0] + if noise: + self._cno = sum(noise) / len(noise) + + def render(self, bounds: rl.Rectangle) -> None: + self._update() + + x = bounds.x + bounds.width - WIDTH - MARGIN + y = bounds.y + bounds.height - HEIGHT - MARGIN + rect = rl.Rectangle(x, y, WIDTH, HEIGHT) + rl.draw_rectangle_rounded(rect, 0.12, 10, _BG) + + tx = int(x + PADDING) + right = int(x + WIDTH - PADDING) + ty = int(y + PADDING) + + rl.draw_text_ex(self._font, "GNSS", rl.Vector2(tx, ty), TITLE_SIZE, 0, _LABEL) + 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) + + ty += 34 + self._draw_row(tx, right, ty, "Decode %", f"{self._sat_time_pct:.0f}%", + _grade(self._sat_time_pct, SAT_TIME_GOOD, SAT_TIME_WARN)) + + ty += 36 + self._draw_row(tx, right, ty, "Radio Signal", f"{self._cno:.0f}", + _grade(self._cno, CNO_GOOD, CNO_WARN)) + + ty += 36 + self._draw_row(tx, right + SATS_OPTICAL_NUDGE, ty, "Tracked Sats", + f"{self._gps_sv}G {self._glonass_sv}R", rl.WHITE) + + def _draw_row(self, tx: int, right: int, ty: int, label: str, value: str, value_color: rl.Color) -> None: + rl.draw_text_ex(self._font, label, rl.Vector2(tx, ty), ROW_SIZE, 0, _LABEL) + value_width = measure_text_cached(self._font, value, ROW_SIZE).x + rl.draw_text_ex(self._font, value, rl.Vector2(right - value_width, ty), ROW_SIZE, 0, value_color) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 688e0c2d1d..3f3600dbc5 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -16,6 +16,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.stopping_point import render_stoppi from openpilot.selfdrive.ui.onroad.starpilot.pause_indicators import render_lateral_paused, render_longitudinal_paused from openpilot.selfdrive.ui.onroad.starpilot.pulse_glide import get_pulse_glide_border_color, render_pulse_glide from openpilot.selfdrive.ui.onroad.starpilot.pip_sidecam import PipSideCamera +from openpilot.selfdrive.ui.onroad.starpilot.gnss_health import GnssHealth from openpilot.selfdrive.ui.onroad.starpilot.favorite_radial_menu import FavoriteRadialMenu from openpilot.selfdrive.ui.onroad.starpilot.weather_icon import render_weather_icon from openpilot.selfdrive.ui.lib.starpilot_status import ( @@ -48,6 +49,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._avg_fps = 0.0 self._pip_sidecam = self._child(PipSideCamera()) + self._gnss_health = GnssHealth() self._favorite_radial_menu = FavoriteRadialMenu( ui_state.ui_params, ui_state.params_memory, @@ -139,6 +141,9 @@ class StarPilotOnroadView(AugmentedRoadView): self._pip_sidecam.render(self._content_rect) + # Not behind a toggle: a new params_keys.h key needs a rebuild, and this is a temporary diagnostic. + self._gnss_health.render(self._content_rect) + # The picker is an app-drawer modal, so it intentionally draws above # PiP and other on-road overlays while active. if self._draw_hud_controls and not self._full_alert_showing(): diff --git a/selfdrive/ui/tests/test_bluetooth_big_ui.py b/selfdrive/ui/tests/test_bluetooth_big_ui.py index ce2fbea052..e2511c6aec 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,30 @@ 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" + + 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/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 81d855df8c..0bddd927ce 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -65,6 +65,8 @@ class UIState: "selfdriveState", "longitudinalPlan", "gpsLocationExternal", + "gpsLocation", + "qcomGnss", "mapdOut", "carOutput", "carControl", @@ -78,7 +80,9 @@ class UIState: "liveDelay", "liveTorqueParameters", ], - drain_services=["carState"], + # qcomGnss sends several report types in bursts; conflating keeps only the last, which drops the + # measurementReport gnss_health.py needs. + drain_services=["carState", "qcomGnss"], ) self.prime_state = PrimeState() diff --git a/starpilot/system/bluetooth/bluez.py b/starpilot/system/bluetooth/bluez.py index 12de66a157..0e308fb66b 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", "")), @@ -248,9 +249,10 @@ 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): + 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/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 new file mode 100644 index 0000000000..43343dd08e --- /dev/null +++ b/starpilot/system/bluetooth/phone_gps.py @@ -0,0 +1,231 @@ +"""Phone GPS over Bluetooth SPP, used by qcomgpsd only while the modem has no fix. + +Phones advertise several serial ports at once (other GPS apps, Android's Nearby Share), and BlueZ's +ConnectProfile connects to whichever it finds first, so every port is probed for real NMEA instead. +""" +import signal +import socket +import subprocess +import threading +import time + +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, 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" + +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 +# NMEA apps send at ~1Hz even without a fix. +PROBE_TIMEOUT_S = 6.0 +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", ""))) + + +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]: + 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): + self.router = DBusRouter(open_dbus_connection(bus="SYSTEM")) + self._state_lock = threading.Lock() + self._stop = threading.Event() + self._stopped = False + self._sock: socket.socket | None = None + self._retry_after: dict[str, tuple[float, float]] = {} + + 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 unwrap_variant(reply.body[0]) if reply.body else {} + + @staticmethod + 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, last_data, last_fix) + except OSError: + pass + + def _close_connection(self, reason: str) -> None: + with self._state_lock: + sock, self._sock = self._sock, None + if sock is None: + return + try: + sock.close() + except OSError: + pass + clear_phone_fix() + clear_phone_fix(PHONE_GPS_STATUS_PATH) + cloudlog.warning(f"phone_gpsd: disconnected ({reason})") + + 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._sock is not sock: + return + now = time.monotonic() + 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 + 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 + if not data: + reason = "closed by phone" + break + except OSError as 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: + 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 + 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 + name = str(props.get("Alias") or props.get("Address") or path) + try: + 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: {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: + with self._state_lock: + 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. + 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") + 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..7ce007bd9b --- /dev/null +++ b/starpilot/system/bluetooth/phone_gps_fix.py @@ -0,0 +1,274 @@ +"""NMEA parsing and the phone fix hand-off between phone_gpsd and qcomgpsd. + +Stdlib-only because qcomgpsd imports it, and nothing here may be able to take qcomgpsd down. +""" +import datetime +import json +import math +import os +import re +import time + +PHONE_GPS_FIX_PATH = "/dev/shm/starpilot_phone_gps.json" +# Written by phone_gpsd, read by bluetooth_managerd for the Bluetooth settings screen. +PHONE_GPS_STATUS_PATH = "/dev/shm/starpilot_phone_gps_status.json" + +PHONE_STATUS_STALE_S = 5.0 + +# Phones emit at 1Hz: tolerates a couple of dropped epochs without 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 + + +SERIAL_PORT_CLASS = '"Serial Port" (0x1101)' +# Android registers Nearby Share with the Serial Port class too; it never carries NMEA and resets the connection. +IGNORED_SERIAL_SERVICES = ("nearbysharing", "nearby") +GPS_SERVICE_HINTS = ("gps", "nmea", "gnss") + + +def parse_serial_ports(sdp_output: str) -> list[tuple[str, int]]: + 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. + 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: + # Apps differ on whether RMC or GGA comes first in an epoch, so GGA is matched to RMC on the UTC time field. + + 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") + 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 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]: + # "no_fix" still proves the Bluetooth link works: NMEA is arriving, the phone just has no lock (e.g. indoors). + 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: + # NMEA gives no accuracies, so these 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..0a3d898fdc 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: "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", "")), ) @@ -94,13 +99,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: + # Major device class 0x02 is Phone. + 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..3ce8a8745d 100644 --- a/starpilot/system/bluetooth/tests/test_bluetooth.py +++ b/starpilot/system/bluetooth/tests/test_bluetooth.py @@ -7,9 +7,10 @@ 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, 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 +187,15 @@ 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) + 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): @@ -416,6 +426,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.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 new file mode 100644 index 0000000000..64a6204466 --- /dev/null +++ b/starpilot/system/bluetooth/tests/test_phone_gps_fix.py @@ -0,0 +1,171 @@ +import datetime +import json + +import pytest + +from openpilot.starpilot.system.bluetooth.phone_gps_fix import (KNOTS_TO_MS, NmeaAccumulator, address_from_device_path, clear_phone_fix, + 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: + 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_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" + assert read_phone_status(path, now=200.0)[1] == "connected" + + clear_phone_fix(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"] + 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..ced95985f7 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -212,6 +212,8 @@ procs = [ # StarPilot variables procs += [ PythonProcess("bluetooth_managerd", "starpilot.system.bluetooth.daemon", bluetooth_enabled, enabled=TICI), + # 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/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index d2c97d56c0..05ddc6f2bd 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,24 @@ def teardown_quectel(diag): try_setup_logs(diag, []) +def send_phone_fallback(pm) -> bool: + # The eGPU's USB 3 link desenses the modem's receiver, which can then go minutes without a fix. + 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 +383,7 @@ def main() -> NoReturn: elif log_type == LOG_GNSS_POSITION_REPORT: report = unpack_position(log_payload) if report["u_PosSource"] != 2: + 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 +411,8 @@ def main() -> NoReturn: if gps.hasFix: want_assistance = False stop_download_event.set() + elif send_phone_fallback(pm): + continue pm.send('gpsLocation', msg) elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: diff --git a/system/ui/widgets/bluetooth.py b/system/ui/widgets/bluetooth.py index def4d61825..d2d103e871 100644 --- a/system/ui/widgets/bluetooth.py +++ b/system/ui/widgets/bluetooth.py @@ -35,16 +35,30 @@ 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() + "..." + # BlueZ's Connected flag also covers a phone's failed audio/call profile attempts, so show phone_gpsd's link instead. + 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 +68,12 @@ 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: + 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 +146,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 +284,13 @@ class BluetoothManagerUI(Widget): return if not device.paired: self._manager.pair(device.address) + elif device.phone: + # A generic Connect only tries audio/call profiles and hangs on "connecting"; phone_gpsd opens the GPS link itself. + 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: