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:
0
system/clearpilot/__init__.py
Normal file
0
system/clearpilot/__init__.py
Normal file
168
system/clearpilot/gpsd.py
Normal file
168
system/clearpilot/gpsd.py
Normal 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()
|
||||
Reference in New Issue
Block a user