diff --git a/CLAUDE.md b/CLAUDE.md index 4721d27..edb7331 100644 --- a/CLAUDE.md +++ b/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 diff --git a/build_only.sh b/build_only.sh index 25b0ebb..a0113e0 100755 --- a/build_only.sh +++ b/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" diff --git a/common/params.cc b/common/params.cc index 166179b..dee6809 100755 --- a/common/params.cc +++ b/common/params.cc @@ -159,6 +159,7 @@ std::unordered_map 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}, diff --git a/launch_openpilot.sh b/launch_openpilot.sh index 0c400fb..9634937 100755 --- a/launch_openpilot.sh +++ b/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 diff --git a/selfdrive/clearpilot/dashcamd b/selfdrive/clearpilot/dashcamd new file mode 100755 index 0000000..571ea51 Binary files /dev/null and b/selfdrive/clearpilot/dashcamd differ diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index b282a91..4764617 100755 --- a/selfdrive/manager/manager.py +++ b/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,)) diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 59dc61a..e888031 100755 --- a/selfdrive/manager/process.py +++ b/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)) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 001a77f..78b4872 100755 --- a/selfdrive/manager/process_config.py +++ b/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), diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner index 04e4b9f..4cd6e4a 100755 Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ 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..e8c2f76 --- /dev/null +++ b/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()