mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-06 12:33:51 +08:00
firestar5683
ab81464bd1
Add navigation and map matching providers with the vendored map service sources. Co-authored-by: Daniel Koepping <elkoled@gmail.com> Co-authored-by: Danny <dannyirwin31@yahoo.com> Co-authored-by: Prabhaav Pillai <prabhaav@msu.edu> Co-authored-by: dirwin31 <83434411+dirwin31@users.noreply.github.com>
46 lines
1.9 KiB
Python
46 lines
1.9 KiB
Python
import time
|
|
|
|
from openpilot.cereal import messaging
|
|
from openpilot.starpilot.gps.source import GPS_MAX_AGE_NS
|
|
|
|
FIELDS = ("latitude", "longitude", "altitude", "speed", "bearingDeg", "horizontalAccuracy",
|
|
"verticalAccuracy", "bearingAccuracyDeg", "speedAccuracy", "unixTimestampMillis", "hasFix", "vNED")
|
|
|
|
|
|
def boot_time_ns():
|
|
return time.clock_gettime_ns(getattr(time, "CLOCK_BOOTTIME", time.CLOCK_MONOTONIC))
|
|
|
|
|
|
class CarGpsPublisher:
|
|
def __init__(self, *, mono_clock=time.monotonic_ns, boot_clock=boot_time_ns):
|
|
self.mono_clock, self.boot_clock = mono_clock, boot_clock
|
|
self.last_source_ns = 0
|
|
self.cached_gps = None
|
|
|
|
def update(self, car_state, pm, message=None):
|
|
getter = getattr(car_state, "get_car_gps", None)
|
|
fix = getter() if getter is not None else None
|
|
gps, after = None, 0
|
|
if fix is not None:
|
|
source_ns = int(fix["timestamp_nanos"])
|
|
if source_ns > self.last_source_ns:
|
|
self.last_source_ns = source_ns
|
|
before, boot, after = self.mono_clock(), self.boot_clock(), self.mono_clock()
|
|
# pandad timestamps received CAN in BOOTTIME; Python Events use MONOTONIC.
|
|
age = boot - source_ns
|
|
if 0 <= after - before <= 1_000_000 and 0 <= age <= GPS_MAX_AGE_NS:
|
|
stamp = before - age
|
|
if stamp > 0:
|
|
gps = {key: fix[key] for key in FIELDS}
|
|
gps["vNED"] = list(fix["vNED"])
|
|
gps["sourceMonoTime"] = stamp
|
|
self.cached_gps = gps if fix["hasFix"] else None
|
|
if message is None:
|
|
if gps is None:
|
|
return
|
|
message = messaging.new_message("starpilotCarState", valid=bool(gps["hasFix"]), logMonoTime=after)
|
|
gps = gps if gps is not None else self.cached_gps
|
|
if gps is not None and 0 < gps["sourceMonoTime"] <= int(message.logMonoTime) <= gps["sourceMonoTime"] + GPS_MAX_AGE_NS:
|
|
message.starpilotCarState.gps = gps
|
|
pm.send("starpilotCarState", message)
|