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.
This commit is contained in:
@@ -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()
|
||||
Reference in New Issue
Block a user