mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-04 13:24:13 +08:00
blueee
This commit is contained in:
@@ -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"]
|
||||
|
||||
Reference in New Issue
Block a user