port clearpilot gpsd; decouple self-driving from GPS

Adds the AT-command-based GPS daemon for the Quectel EC25 modem (the
device has no u-blox chip and qcomgpsd's diag interface hangs on this
hardware). Trimmed from broken's version: dropped cloudlog calls and
the IsDaylight / ScreenDisplayMode auto-switching (those belong to the
display-modes feature, port later).

Used solely for system clock initialization, on-screen UI speed, and
per-segment dashcam GPS metadata.

Self-driving must NOT consume gpsLocation — feeding it to locationd's
kalman filter screws up the math. Patch locationd to skip GPS:
- locationd_thread() no longer subscribes to gpsLocation/gpsLocationExternal
- handle_msg's GPS branches commented (dead code without subscription)
- the "save LastGPSPosition once a minute when gpsOK" block commented
  (dead because gpsOK is now permanently false)

Result: liveLocationKalman.gpsOK = false for all self-driving consumers
(controlsd, paramsd, torqued, frogpilot_planner). They already handle
that case. Other liveLocationKalman fields still publish from the
camera-odometry + IMU + calibration kalman state.

system/clearpilot/__init__.py added so system.clearpilot.gpsd is a
valid Python module.

selfdrive/locationd/locationd.cc

@@ -594,10 +594,15 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
     this->handle_sensor(t, log.getAccelerometer());
   } else if (log.isGyroscope()) {
     this->handle_sensor(t, log.getGyroscope());
-  } else if (log.isGpsLocation()) {
+  // CLEARPILOT: GPS branches removed — locationd no longer subscribes to
-    this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
+  // gpsLocation/gpsLocationExternal, so these would be dead code regardless.
-  } else if (log.isGpsLocationExternal()) {
+  // Self-driving treats GPS as not present: gpsOK stays false (last_gps_msg
-    this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
+  // never updates), and other liveLocationKalman fields stay at the kalman
+  // filter's IMU+camera-odometry-only state.
+  // } else if (log.isGpsLocation()) {
+  //   this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
+  // } else if (log.isGpsLocationExternal()) {
+  //   this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
   //} else if (log.isGnssMeasurements()) {
   //  this->handle_gnss(t, log.getGnssMeasurements());
   } else if (log.isCarState()) {
@@ -676,22 +681,16 @@ void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
 }
 
 int Localizer::locationd_thread() {
-  Params params;
+  // CLEARPILOT: do not subscribe to GPS. Our gpsd publishes gpsLocation for
-  LocalizerGnssSource source;
+  // UI/clock/dashcam, but feeding it to the kalman filter screws up the
-  const char* gps_location_socket;
+  // self-driving math. liveLocationKalman.gpsOK stays false; downstream
-  if (params.getBool("UbloxAvailable")) {
+  // self-driving consumers (controlsd, paramsd, torqued, frogpilot_planner)
-    source = LocalizerGnssSource::UBLOX;
+  // already handle that case.
-    gps_location_socket = "gpsLocationExternal";
+  this->configure_gnss_source(LocalizerGnssSource::QCOM);
-  } else {
+  const std::initializer_list<const char *> service_list = {"cameraOdometry", "liveCalibration",
-    source = LocalizerGnssSource::QCOM;
-    gps_location_socket = "gpsLocation";
-  }
-
-  this->configure_gnss_source(source);
-  const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
                                                           "carState", "accelerometer", "gyroscope"};
 
-  SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
+  SubMaster sm(service_list, {}, nullptr, {});
   PubMaster pm({"liveLocationKalman"});
 
   uint64_t cnt = 0;
@@ -730,12 +729,14 @@ int Localizer::locationd_thread() {
       kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
       pm.send("liveLocationKalman", bytes.begin(), bytes.size());
 
-      if (cnt % 1200 == 0 && gpsOK) {  // once a minute
+      // CLEARPILOT: dead code now that gpsOK is permanently false (we don't
-        VectorXd posGeo = this->get_position_geodetic();
+      // subscribe to gpsLocation). Was: write LastGPSPosition once a minute.
-        std::string lastGPSPosJSON = util::string_format(
+      // if (cnt % 1200 == 0 && gpsOK) {
-          "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
+      //   VectorXd posGeo = this->get_position_geodetic();
-        params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
+      //   std::string lastGPSPosJSON = util::string_format(
-      }
+      //     "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
+      //   params.putNonBlocking("LastGPSPosition", lastGPSPosJSON);
+      // }
       cnt++;
     }
   }

selfdrive/manager/process_config.py

@@ -82,6 +82,10 @@ procs = [
   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),  # Fixme
+  # CLEARPILOT: replacement for qcomgpsd (whose diag interface is broken on this device).
+  # Uses Quectel modem AT commands via mmcli. Self-driving does NOT consume this; locationd
+  # is patched to skip gpsLocation. Used only for system clock + UI speed + dashcam metadata.
+  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,167 @@
+#!/usr/bin/env python3
+"""
+ClearPilot GPS daemon — reads GPS from Quectel EC25 modem via AT commands
+and publishes gpsLocation messages.
+
+Replaces qcomgpsd (which uses the diag interface — broken on this device).
+
+Used solely for: setting system clock on first fix, an on-screen UI
+speed indicator, and per-segment GPS metadata for the dashcam. Self-
+driving code does NOT consume this output — locationd is patched to not
+subscribe to gpsLocation, so liveLocationKalman.gpsOK stays false.
+"""
+import os
+import subprocess
+import sys
+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.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
+    if result.startswith("response: '") and result.endswith("'"):
+      result = result[len("response: '"):-1]
+    return result
+  except subprocess.CalledProcessError:
+    return ""
+
+
+def wait_for_modem():
+  print("CLP gpsd: waiting for modem", file=sys.stderr, flush=True)
+  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
+    date = fields[9]      # DDMMYY
+    nsat = int(fields[10])
+
+    # Build unix timestamp from UTC + date
+    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,  # km/h -> m/s
+      "bearing": cog,
+      "accuracy": hdop * 5,  # rough HDOP -> meters
+      "timestamp_ms": dt.timestamp() * 1e3,
+      "satellites": nsat,
+      "fix": fix,
+    }
+  except (ValueError, IndexError) as e:
+    print(f"CLP gpsd: parse error: {e}", file=sys.stderr, flush=True)
+    return None
+
+
+def main():
+  print("CLP 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("CLP 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("CLP 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"CLP 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("CLP gpsd: GPS started fresh", file=sys.stderr, flush=True)
+  else:
+    print("CLP gpsd: GPS already running, keeping fix", file=sys.stderr, flush=True)
+
+  pm = messaging.PubMaster(["gpsLocation"])
+  clock_set = system_time_valid()
+  print("CLP 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
+          print(f"CLP gpsd: system clock set from GPS: {gps_dt}", file=sys.stderr, flush=True)
+        else:
+          print(f"CLP gpsd: failed to set clock: {ret.stderr.decode().strip()}", file=sys.stderr, flush=True)
+
+      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(0.5)  # 2 Hz polling
+
+
+if __name__ == "__main__":
+  main()