diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 0274b6e..df4a48c 100755 --- a/selfdrive/locationd/locationd.cc +++ b/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()) { - 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); + // CLEARPILOT: GPS branches removed — locationd no longer subscribes to + // gpsLocation/gpsLocationExternal, so these would be dead code regardless. + // Self-driving treats GPS as not present: gpsOK stays false (last_gps_msg + // 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; - LocalizerGnssSource source; - const char* gps_location_socket; - if (params.getBool("UbloxAvailable")) { - source = LocalizerGnssSource::UBLOX; - gps_location_socket = "gpsLocationExternal"; - } else { - source = LocalizerGnssSource::QCOM; - gps_location_socket = "gpsLocation"; - } - - this->configure_gnss_source(source); - const std::initializer_list service_list = {gps_location_socket, "cameraOdometry", "liveCalibration", + // CLEARPILOT: do not subscribe to GPS. Our gpsd publishes gpsLocation for + // UI/clock/dashcam, but feeding it to the kalman filter screws up the + // self-driving math. liveLocationKalman.gpsOK stays false; downstream + // self-driving consumers (controlsd, paramsd, torqued, frogpilot_planner) + // already handle that case. + this->configure_gnss_source(LocalizerGnssSource::QCOM); + const std::initializer_list service_list = {"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 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 - VectorXd posGeo = this->get_position_geodetic(); - std::string lastGPSPosJSON = util::string_format( - "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); - params.putNonBlocking("LastGPSPosition", lastGPSPosJSON); - } + // CLEARPILOT: dead code now that gpsOK is permanently false (we don't + // subscribe to gpsLocation). Was: write LastGPSPosition once a minute. + // if (cnt % 1200 == 0 && gpsOK) { + // VectorXd posGeo = this->get_position_geodetic(); + // std::string lastGPSPosJSON = util::string_format( + // "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); + // params.putNonBlocking("LastGPSPosition", lastGPSPosJSON); + // } cnt++; } } diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 287403c..cc638c0 100755 --- a/selfdrive/manager/process_config.py +++ b/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), diff --git a/system/clearpilot/__init__.py b/system/clearpilot/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/system/clearpilot/gpsd.py b/system/clearpilot/gpsd.py new file mode 100644 index 0000000..76273e3 --- /dev/null +++ b/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()