GPS fix + log directory redesign + dashcamd binary

GPS: fix AT response parsing (strip mmcli `response: '...'` wrapper),
fix capnp field names (horizontalAccuracy, hasFix), set system clock
directly from first GPS fix when time is invalid, kill system gpsd on
startup.

Logging: replace module-level log dir creation with init_log_dir()
called from manager_init(). Active session always at /data/log2/current
(real dir until time resolves, then symlink to timestamped dir). Add
LogDirInitialized param. Redirect both stdout+stderr for all processes.

Also: thorough process cleanup in launch scripts, dashcamd binary,
CLAUDE.md updates for GPS/telemetry/bench/logging docs.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

CLAUDE.md

@@ -14,8 +14,13 @@ ClearPilot is a custom fork of **FrogPilot** (itself a fork of comma.ai's openpi
 - **ClearPilot service**: Node.js service at `selfdrive/clearpilot/` with behavior scripts for lane change and longitudinal control
 - **Native dashcamd**: C++ process capturing raw camera frames via VisionIPC with OMX H.264 hardware encoding
 - **Standstill power saving**: model inference throttled to 1fps and fan quieted when car is stopped
-- **Clean offroad UI**: grid launcher replacing stock home screen
+- **ClearPilot menu**: sidebar settings panel replacing stock home screen (Home, Dashcam, Debug panels)
+- **Status window**: live system stats (temp, fan, storage, RAM, WiFi, VPN, GPS, telemetry status)
 - **Debug button (LFA)**: steering wheel button repurposed for screen toggle and future UI actions
+- **Telemetry system**: diff-based CSV logger via ZMQ IPC, toggleable from Debug panel
+- **Bench mode**: `--bench` flag for onroad UI testing without car connected
+- **GPS**: custom AT-command based GPS daemon (`system/clearpilot/gpsd.py`) replacing broken qcomgpsd diag interface
+- **OpenVPN tunnel**: auto-connecting VPN to expose device on remote network for SSH access
 
 See `GOALS.md` for feature roadmap.
 
@@ -72,10 +77,11 @@ su - comma -c "bash /data/openpilot/build_only.sh"
 su - comma -c "bash /data/openpilot/launch_openpilot.sh"
 
 # 4. Review the aggregate session log for errors
-cat /data/log2/$(ls -t /data/log2/ | head -1)/session.log
+cat /data/log2/current/session.log
 
-# 5. Check per-process stderr logs if needed
-ls /data/log2/$(ls -t /data/log2/ | head -1)/
+# 5. Check per-process stdout/stderr logs if needed
+ls /data/log2/current/
+cat /data/log2/current/gpsd.log
 ```
 
 ### Adding New Params
@@ -128,14 +134,19 @@ su - comma -c "PYTHONPATH=/data/openpilot python3 -m selfdrive.clearpilot.bench_
 The UI has a SIGSEGV/SIGABRT crash handler (`selfdrive/ui/main.cc`) that prints a stack trace to stderr, captured in the per-process log:
 
 ```bash
-# Check for crash traces
-grep -A 30 "CRASH" /data/log2/$(ls -t /data/log2/ | head -1)/ui.log
+# Check for crash traces (use /data/log2/current which is always the active session)
+grep -A 30 "CRASH" /data/log2/current/ui.log
 
 # Resolve addresses to source lines
 addr2line -e /data/openpilot/selfdrive/ui/ui -f 0xADDRESS
 
 # bench_cmd dump detects crash loops automatically:
 # if UI process uptime < 5s, it reports "likely crash looping"
+
+# Check per-process logs
+ls /data/log2/current/
+cat /data/log2/current/session.log
+cat /data/log2/current/gpsd.log
 ```
 
 ### UI Introspection RPC
@@ -150,9 +161,9 @@ The UI process runs a ZMQ REP server at `ipc:///tmp/clearpilot_ui_rpc`. Send `"d
 
 - `launch_openpilot.sh --bench` sets `BENCH_MODE=1` env var
 - `manager.py` reads `BENCH_MODE`, blocks real car processes, enables `bench_onroad` process
-- `bench_onroad.py` publishes fake `deviceState`, `pandaStates`, `carState`, `controlsState` at correct frequencies
-- The UI receives these messages identically to real car data
-- Blocked processes in bench mode: pandad, thermald, controlsd, radard, plannerd, calibrationd, torqued, paramsd, locationd, sensord, ubloxd, pigeond, dmonitoringmodeld, dmonitoringd, modeld, soundd, camerad, loggerd, micd, dashcamd
+- `bench_onroad.py` publishes fake `pandaStates` (ignition=true), `carState`, `controlsState` — thermald reads the fake pandaStates to determine ignition and publishes `deviceState.started=true` on its own
+- thermald and camerad run normally in bench mode (thermald manages CPU cores needed for camerad)
+- Blocked processes in bench mode: pandad, controlsd, radard, plannerd, calibrationd, torqued, paramsd, locationd, sensord, ubloxd, pigeond, dmonitoringmodeld, dmonitoringd, modeld, soundd, loggerd, micd, dashcamd
 
 ### Key Files
 
@@ -174,12 +185,17 @@ The UI process runs a ZMQ REP server at `ipc:///tmp/clearpilot_ui_rpc`. Send `"d
 
 ## Session Logging
 
-Per-process stderr and an aggregate event log are captured in `/data/log2/{session}/`.
+Per-process stderr and an aggregate event log are captured in `/data/log2/current/`.
 
 ### Log Directory
 
-- Created at manager import time with timestamp: `/data/log2/YYYY-MM-DD-HH-MM-SS/`
-- If system clock is invalid (cold boot, no WiFi, RTC stuck at 1970): uses `/data/log2/boot-{monotonic}/`, renamed to real timestamp once GPS/NTP resolves the time
+- `/data/log2/current/` is always the active session directory
+- `init_log_dir()` is called once from `manager_init()` — creates a fresh `/data/log2/current/` real directory
+- If a previous `current/` real directory exists (unresolved session), it's renamed using its mtime timestamp
+- If a previous `current` symlink exists, it's removed
+- Once system time is valid (GPS/NTP), the real directory is renamed to `/data/log2/YYYY-MM-DD-HH-MM-SS/` and `current` becomes a symlink to it
+- `LogDirInitialized` param: `"0"` until time resolves, then `"1"`
+- Open file handles survive the rename (same inode, same filesystem)
 - Session directories older than 30 days are deleted on manager startup
 
 ### Per-Process Logs
@@ -424,8 +440,25 @@ Power On
 
 ### GPS
 
-- `ubloxd` + `pigeond` for u-blox GPS hardware
-- `qcomgpsd`, `ugpsd`, `navd` currently **commented out** in process_config
+- Device has **no u-blox chip** (`/dev/ttyHS0` does not exist) — `ubloxd`/`pigeond` never start
+- GPS hardware is a **Quectel EC25 LTE modem** (USB, `lsusb: 2c7c:0125`) with built-in GPS
+- GPS is accessed via AT commands through `mmcli`: `mmcli -m any --command='AT+QGPSLOC=2'`
+- **`qcomgpsd`** (original openpilot process) uses the modem's diag interface which is broken on this device — the diag recv loop blocks forever after setup. Commented out.
+- **`system/clearpilot/gpsd.py`** is the replacement — polls GPS via AT commands at 1Hz, publishes `gpsLocation` cereal messages
+- GPS data flows: `gpsd` → `gpsLocation` → `locationd` → `liveLocationKalman` → `timed` (sets system clock)
+- `locationd` checks `UbloxAvailable` param (false on this device) to subscribe to `gpsLocation` instead of `gpsLocationExternal`
+- `mmcli` returns `response: '...'` wrapper — `at_cmd()` strips it before parsing (fixed)
+- GPS antenna power must be enabled via GPIO: `gpio_set(GPIO.GNSS_PWR_EN, True)`
+- System `/usr/sbin/gpsd` daemon may respawn and interfere — should be disabled or killed
+
+### Telemetry
+
+- **Client**: `selfdrive/clearpilot/telemetry.py` — `tlog(group, data)` sends JSON over ZMQ PUSH
+- **Collector**: `selfdrive/clearpilot/telemetryd.py` — diffs against previous state, writes changed values to CSV
+- **Toggle**: `TelemetryEnabled` param, controlled from Debug panel in ClearPilot menu
+- **Auto-disable**: disabled on every manager start; disabled if `/data` free < 5GB
+- **Hyundai CAN-FD data**: logged from `selfdrive/car/hyundai/carstate.py` `update_canfd()` — groups: `car`, `cruise`, `speed_limit`, `buttons`
+- **CSV location**: `/data/log2/current/telemetry.csv` (or session directory)
 
 ### Key Dependencies
 

build_only.sh

@@ -7,11 +7,14 @@
 
 BASEDIR="/data/openpilot"
 
-# Kill stale error displays and any running manager/launch processes
-pkill -f "selfdrive/ui/text" 2>/dev/null
-pkill -f 'launch_openpilot.sh' 2>/dev/null
-pkill -f 'launch_chffrplus.sh' 2>/dev/null
-pkill -f 'python.*manager.py' 2>/dev/null
+# Kill stale error displays and any running manager/launch/managed processes
+pkill -9 -f "selfdrive/ui/text" 2>/dev/null
+pkill -9 -f 'launch_openpilot.sh' 2>/dev/null
+pkill -9 -f 'launch_chffrplus.sh' 2>/dev/null
+pkill -9 -f 'python.*manager.py' 2>/dev/null
+pkill -9 -f 'selfdrive\.' 2>/dev/null
+pkill -9 -f 'system\.' 2>/dev/null
+pkill -9 -f './ui' 2>/dev/null
 sleep 1
 
 source "$BASEDIR/launch_env.sh"

common/params.cc

@@ -159,6 +159,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LastUpdateTime", PERSISTENT},
     {"LiveParameters", PERSISTENT},
     {"LiveTorqueParameters", PERSISTENT | DONT_LOG},
+    {"LogDirInitialized", CLEAR_ON_MANAGER_START},
     {"LongitudinalPersonality", PERSISTENT},
     {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
     {"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},

launch_openpilot.sh

@@ -1,13 +1,18 @@
 #!/usr/bin/bash
 
-# Kill other instances of this script and any running manager
+# Kill other instances of this script, launch chain, and all managed processes
 for pid in $(pgrep -f 'launch_openpilot.sh' | grep -v $$); do
-  kill "$pid" 2>/dev/null
+  kill -9 "$pid" 2>/dev/null
 done
 for pid in $(pgrep -f 'launch_chffrplus.sh' | grep -v $$); do
-  kill "$pid" 2>/dev/null
+  kill -9 "$pid" 2>/dev/null
 done
-pkill -f 'python.*manager.py' 2>/dev/null
+pkill -9 -f 'python.*manager.py' 2>/dev/null
+# Kill all processes started by the manager (run as comma user, in openpilot tree)
+pkill -9 -f 'selfdrive\.' 2>/dev/null
+pkill -9 -f 'system\.' 2>/dev/null
+pkill -9 -f './ui' 2>/dev/null
+pkill -9 -f 'selfdrive/ui/text' 2>/dev/null
 sleep 1
 
 bash /data/openpilot/system/clearpilot/on_start.sh

selfdrive/manager/manager.py

@@ -16,7 +16,7 @@ from openpilot.common.text_window import TextWindow
 from openpilot.common.time import system_time_valid
 from openpilot.system.hardware import HARDWARE, PC
 from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog
-from openpilot.selfdrive.manager.process import ensure_running, update_log_dir_timestamp, session_log
+from openpilot.selfdrive.manager.process import ensure_running, init_log_dir, update_log_dir_timestamp, session_log
 from openpilot.selfdrive.manager.process_config import managed_processes
 from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID
 from openpilot.common.swaglog import cloudlog, add_file_handler
@@ -59,6 +59,8 @@ def cleanup_old_logs(max_age_days=30):
     return
   cutoff = time.time() - (max_age_days * 86400)
   for entry in os.listdir(log_base):
+    if entry == "current":
+      continue
     path = os.path.join(log_base, entry)
     if os.path.isdir(path):
       if os.path.getmtime(path) < cutoff:
@@ -69,6 +71,7 @@ def cleanup_old_logs(max_age_days=30):
 
 
 def manager_init(frogpilot_functions) -> None:
+  init_log_dir()
   cleanup_old_logs()
 
   frogpilot_boot = threading.Thread(target=frogpilot_boot_functions, args=(frogpilot_functions,))

selfdrive/manager/process.py

@@ -23,44 +23,98 @@ from openpilot.common.time import system_time_valid
 WATCHDOG_FN = "/dev/shm/wd_"
 ENABLE_WATCHDOG = os.getenv("NO_WATCHDOG") is None
 
-# CLEARPILOT: time-safe log directory — use temporary name if clock is invalid (1970),
-# rename to real timestamp once GPS/NTP resolves the time
-if system_time_valid():
-  _log_dir = f"/data/log2/{datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')}"
-  _time_resolved = True
-else:
-  _log_dir = f"/data/log2/boot-{int(time.monotonic())}"
-  _time_resolved = False
-os.makedirs(_log_dir, exist_ok=True)
-
-# CLEARPILOT: aggregate session log for major events
+# CLEARPILOT: logging directory and session log
+# init_log_dir() must be called once from manager_init() before any process starts.
+# Until then, _log_dir and session_log are usable but write to a NullHandler.
 import logging
+
+_log_dir = "/data/log2/current"
+_time_resolved = False
+_session_handler = None
+
 session_log = logging.getLogger("clearpilot.session")
 session_log.setLevel(logging.DEBUG)
-_session_handler = logging.FileHandler(os.path.join(_log_dir, "session.log"))
-_session_handler.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(message)s"))
-session_log.addHandler(_session_handler)
+session_log.addHandler(logging.NullHandler())
+
+
+def init_log_dir():
+  """Create /data/log2/current as a real directory for this session.
+  Called once from manager_init(). Previous current (if a real dir) is
+  renamed to a timestamp or boot-monotonic name before we create a fresh one."""
+  global _log_dir, _time_resolved, _session_handler
+
+  log_base = "/data/log2"
+  current = os.path.join(log_base, "current")
+  os.makedirs(log_base, exist_ok=True)
+
+  # If 'current' is a symlink, just remove the symlink
+  if os.path.islink(current):
+    os.unlink(current)
+  # If 'current' is a real directory (leftover from previous session that
+  # never got time-resolved), rename it out of the way
+  elif os.path.isdir(current):
+    # Use mtime of session.log (or the dir itself) for the rename
+    session_file = os.path.join(current, "session.log")
+    mtime = os.path.getmtime(session_file) if os.path.exists(session_file) else os.path.getmtime(current)
+    ts = datetime.datetime.fromtimestamp(mtime).strftime('%Y-%m-%d-%H-%M-%S')
+    dest = os.path.join(log_base, ts)
+    # Avoid collision
+    if os.path.exists(dest):
+      dest = dest + f"-{int(time.monotonic())}"
+    try:
+      os.rename(current, dest)
+    except OSError:
+      pass
+
+  os.makedirs(current, exist_ok=True)
+  _log_dir = current
+  _time_resolved = False
+
+  # Set up session log file handler
+  _session_handler = logging.FileHandler(os.path.join(_log_dir, "session.log"))
+  _session_handler.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(message)s"))
+  # Remove NullHandler and add file handler
+  session_log.handlers.clear()
+  session_log.addHandler(_session_handler)
+
+  session_log.info("session started, log dir: %s", _log_dir)
 
 
 def update_log_dir_timestamp():
-  """Rename boot-xxx log dir to real timestamp once system time is valid."""
+  """Rename /data/log2/current to a real timestamp and replace with a symlink
+  once system time is valid."""
   global _log_dir, _time_resolved, _session_handler
   if _time_resolved:
     return
   if not system_time_valid():
     return
-  new_dir = f"/data/log2/{datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')}"
+
+  log_base = "/data/log2"
+  current = os.path.join(log_base, "current")
+  ts_name = datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
+  new_dir = os.path.join(log_base, ts_name)
+
   try:
-    os.rename(_log_dir, new_dir)
+    os.rename(current, new_dir)
+    # Create symlink: current -> YYYY-MM-DD-HH-MM-SS
+    os.symlink(ts_name, current)
     _log_dir = new_dir
     _time_resolved = True
-    # Re-point session log handler to renamed directory
+    # Re-point session log handler (open files follow the inode, but
+    # new opens should go through the symlink — update handler for clarity)
     session_log.removeHandler(_session_handler)
     _session_handler.close()
     _session_handler = logging.FileHandler(os.path.join(_log_dir, "session.log"))
     _session_handler.setFormatter(logging.Formatter("%(asctime)s %(levelname)s %(message)s"))
     session_log.addHandler(_session_handler)
     session_log.info("log directory renamed to %s", _log_dir)
+
+    # Signal via param that the directory has been time-resolved
+    try:
+      from openpilot.common.params import Params
+      Params().put("LogDirInitialized", "1")
+    except Exception:
+      pass
   except OSError:
     pass
 
@@ -71,8 +125,9 @@ def launcher(proc: str, name: str, log_path: str) -> None:
   try:
     log_file = open(log_path, 'a')
     os.dup2(log_file.fileno(), sys.stderr.fileno())
-  except Exception:
-    pass
+    os.dup2(log_file.fileno(), sys.stdout.fileno())
+  except Exception as e:
+    print(f"CLEARPILOT: stderr redirect failed for {name}: {e}", file=sys.stderr)
 
   try:
     # import the process
@@ -102,12 +157,13 @@ def launcher(proc: str, name: str, log_path: str) -> None:
 def nativelauncher(pargs: list[str], cwd: str, name: str, log_path: str) -> None:
   os.environ['MANAGER_DAEMON'] = name
 
-  # CLEARPILOT: redirect stderr to per-process log file
+  # CLEARPILOT: redirect stderr and stdout to per-process log file
   try:
     log_file = open(log_path, 'a')
     os.dup2(log_file.fileno(), sys.stderr.fileno())
-  except Exception:
-    pass
+    os.dup2(log_file.fileno(), sys.stdout.fileno())
+  except Exception as e:
+    print(f"CLEARPILOT: stderr redirect failed for {name}: {e}", file=sys.stderr)
 
   # exec the process
   os.chdir(cwd)
@@ -256,6 +312,9 @@ class NativeProcess(ManagerProcess):
     global _log_dir
     log_path = _log_dir+"/"+self.name+".log"
 
+    # CLEARPILOT: ensure log file exists even if child redirect fails
+    open(log_path, 'a').close()
+
     cwd = os.path.join(BASEDIR, self.cwd)
     cloudlog.info(f"starting process {self.name}")
     session_log.info("starting %s", self.name)
@@ -290,6 +349,9 @@ class PythonProcess(ManagerProcess):
 
     global _log_dir
     log_path = _log_dir+"/"+self.name+".log"
+    # CLEARPILOT: ensure log file exists even if child redirect fails
+    open(log_path, 'a').close()
+
     cloudlog.info(f"starting python {self.module}")
     session_log.info("starting %s", self.name)
     self.proc = Process(name=self.name, target=self.launcher, args=(self.module, self.name, log_path))

selfdrive/manager/process_config.py

@@ -84,7 +84,8 @@ procs = [
   PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
   PythonProcess("deleter", "system.loggerd.deleter", always_run),
   PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
-  PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),
+  # PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),
+  PythonProcess("gpsd", "system.clearpilot.gpsd", qcomgps, enabled=TICI),
   # PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI),
   #PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
   PythonProcess("pandad", "selfdrive.boardd.pandad", always_run),

system/clearpilot/gpsd.py

@@ -0,0 +1,168 @@
+#!/usr/bin/env python3
+"""
+ClearPilot GPS daemon — reads GPS from Quectel EC25 modem via AT commands
+and publishes gpsLocation messages for locationd/timed.
+
+Replaces qcomgpsd which uses the diag interface (broken on this device).
+"""
+import math
+import os
+import subprocess
+import time
+import datetime
+
+from cereal import log
+import cereal.messaging as messaging
+from openpilot.common.gpio import gpio_init, gpio_set
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.time import system_time_valid
+from openpilot.system.hardware.tici.pins import GPIO
+
+
+def at_cmd(cmd: str) -> str:
+  try:
+    result = subprocess.check_output(
+      f"mmcli -m any --timeout 10 --command='{cmd}'",
+      shell=True, encoding='utf8', stderr=subprocess.DEVNULL
+    ).strip()
+    # mmcli wraps AT responses: response: '+QGPSLOC: ...'
+    # Strip the wrapper to get just the AT response
+    if result.startswith("response: '") and result.endswith("'"):
+      result = result[len("response: '"):-1]
+    return result
+  except subprocess.CalledProcessError:
+    return ""
+
+
+def wait_for_modem():
+  cloudlog.warning("gpsd: waiting for modem")
+  while True:
+    ret = subprocess.call(
+      "mmcli -m any --timeout 10 --command='AT+QGPS?'",
+      stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True
+    )
+    if ret == 0:
+      return
+    time.sleep(0.5)
+
+
+def parse_qgpsloc(response: str):
+  """Parse AT+QGPSLOC=2 response into a dict.
+  Format: +QGPSLOC: UTC,lat,lon,hdop,alt,fix,cog,spkm,spkn,date,nsat
+  """
+  if "+QGPSLOC:" not in response:
+    return None
+  try:
+    data = response.split("+QGPSLOC:")[1].strip()
+    fields = data.split(",")
+    if len(fields) < 11:
+      return None
+
+    utc = fields[0]       # HHMMSS.S
+    lat = float(fields[1])
+    lon = float(fields[2])
+    hdop = float(fields[3])
+    alt = float(fields[4])
+    fix = int(fields[5])  # 2=2D, 3=3D
+    cog = float(fields[6])  # course over ground
+    spkm = float(fields[7])  # speed km/h
+    spkn = float(fields[8])  # speed knots
+    date = fields[9]      # DDMMYY
+    nsat = int(fields[10])
+
+    # Build unix timestamp from UTC + date
+    # utc: "HHMMSS.S", date: "DDMMYY"
+    hh, mm = int(utc[0:2]), int(utc[2:4])
+    ss = float(utc[4:])
+    dd, mo, yy = int(date[0:2]), int(date[2:4]), 2000 + int(date[4:6])
+    dt = datetime.datetime(yy, mo, dd, hh, mm, int(ss),
+                           int((ss % 1) * 1e6), datetime.timezone.utc)
+
+    return {
+      "latitude": lat,
+      "longitude": lon,
+      "altitude": alt,
+      "speed": spkm / 3.6,  # convert km/h to m/s
+      "bearing": cog,
+      "accuracy": hdop * 5,  # rough conversion from HDOP to meters
+      "timestamp_ms": dt.timestamp() * 1e3,
+      "satellites": nsat,
+      "fix": fix,
+    }
+  except (ValueError, IndexError) as e:
+    cloudlog.error(f"gpsd: parse error: {e}")
+    return None
+
+
+def main():
+  import sys
+  print("gpsd: starting", file=sys.stderr, flush=True)
+
+  # Kill system gpsd which may respawn and interfere with modem access
+  subprocess.run("pkill -f /usr/sbin/gpsd", shell=True,
+                 stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+
+  wait_for_modem()
+  print("gpsd: modem ready", file=sys.stderr, flush=True)
+
+  # Enable GPS antenna power
+  gpio_init(GPIO.GNSS_PWR_EN, True)
+  gpio_set(GPIO.GNSS_PWR_EN, True)
+  print("gpsd: GPIO power enabled", file=sys.stderr, flush=True)
+
+  # Don't restart GPS if already running (preserve existing fix)
+  resp = at_cmd("AT+QGPS?")
+  print(f"gpsd: QGPS status: {resp}", file=sys.stderr, flush=True)
+  if "QGPS: 1" not in resp:
+    at_cmd('AT+QGPSCFG="dpoenable",0')
+    at_cmd('AT+QGPSCFG="outport","none"')
+    at_cmd("AT+QGPS=1")
+    print("gpsd: GPS started fresh", file=sys.stderr, flush=True)
+  else:
+    print("gpsd: GPS already running, keeping fix", file=sys.stderr, flush=True)
+
+  pm = messaging.PubMaster(["gpsLocation"])
+  clock_set = system_time_valid()
+  print("gpsd: entering main loop", file=sys.stderr, flush=True)
+
+  while True:
+    resp = at_cmd("AT+QGPSLOC=2")
+    fix = parse_qgpsloc(resp)
+
+    if fix:
+      # Set system clock from GPS on first valid fix if clock is invalid
+      if not clock_set:
+        gps_dt = datetime.datetime.utcfromtimestamp(fix["timestamp_ms"] / 1000)
+        ret = subprocess.run(["date", "-s", gps_dt.strftime("%Y-%m-%d %H:%M:%S")],
+                             env={**os.environ, "TZ": "UTC"},
+                             capture_output=True)
+        if ret.returncode == 0:
+          clock_set = True
+          cloudlog.warning("gpsd: system clock set from GPS: %s", gps_dt)
+          print(f"gpsd: system clock set from GPS: {gps_dt}", file=sys.stderr, flush=True)
+        else:
+          cloudlog.error("gpsd: failed to set clock: %s", ret.stderr.decode().strip())
+
+      msg = messaging.new_message("gpsLocation", valid=True)
+      gps = msg.gpsLocation
+      gps.latitude = fix["latitude"]
+      gps.longitude = fix["longitude"]
+      gps.altitude = fix["altitude"]
+      gps.speed = fix["speed"]
+      gps.bearingDeg = fix["bearing"]
+      gps.horizontalAccuracy = fix["accuracy"]
+      gps.unixTimestampMillis = int(fix["timestamp_ms"])
+      gps.source = log.GpsLocationData.SensorSource.qcomdiag
+      gps.hasFix = fix["fix"] >= 2
+      gps.flags = 1
+      gps.vNED = [0.0, 0.0, 0.0]
+      gps.verticalAccuracy = fix["accuracy"]
+      gps.bearingAccuracyDeg = 10.0
+      gps.speedAccuracy = 1.0
+      pm.send("gpsLocation", msg)
+
+    time.sleep(1.0)  # 1 Hz polling
+
+
+if __name__ == "__main__":
+  main()