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>
This commit is contained in:
2026-04-13 04:51:33 +00:00
parent f969214a3d
commit 4f16a8953a
11 changed files with 324 additions and 48 deletions

View File

168
system/clearpilot/gpsd.py Normal file
View File

@@ -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()