mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
34 lines
1.5 KiB
Python
34 lines
1.5 KiB
Python
import math
|
|
|
|
GPS_SOURCES = {"gpsLocationExternal": "ublox", "gpsLocation": "qcomdiag", "starpilotCarState": "car"}
|
|
GPS_MAX_AGE_NS = 2_500_000_000
|
|
|
|
|
|
def observation(sm, service, now_ns):
|
|
try:
|
|
gps = sm[service].gps if service == "starpilotCarState" else sm[service]
|
|
envelope = int(sm.logMonoTime[service])
|
|
stamp = int(gps.sourceMonoTime) if service == "starpilotCarState" else envelope
|
|
if (not sm.valid[service] or not gps.hasFix or
|
|
not 0 < stamp <= envelope <= now_ns <= stamp + GPS_MAX_AGE_NS or
|
|
not all(math.isfinite(v) for v in (gps.latitude, gps.longitude, gps.horizontalAccuracy)) or
|
|
not (-90 <= gps.latitude <= 90 and -180 <= gps.longitude <= 180 and 0 < gps.horizontalAccuracy <= 25)):
|
|
return None
|
|
if service != "starpilotCarState" and str(gps.source) != GPS_SOURCES[service]:
|
|
return None
|
|
return stamp, gps
|
|
except (AttributeError, KeyError, TypeError, ValueError, OverflowError, RuntimeError):
|
|
return None
|
|
|
|
|
|
def select_location(sm, now_ns):
|
|
native = [fix for service in ("gpsLocationExternal", "gpsLocation")
|
|
if (fix := observation(sm, service, now_ns)) is not None]
|
|
return max(native, key=lambda fix: fix[0]) if native else observation(sm, "starpilotCarState", now_ns)
|
|
|
|
|
|
def bearing(gps):
|
|
value = float(getattr(gps, "bearingDeg", float("nan")))
|
|
accuracy = float(getattr(gps, "bearingAccuracyDeg", 0.))
|
|
return value if math.isfinite(value) and 0 <= value < 360 and math.isfinite(accuracy) and 0 <= accuracy < 180 else None
|