diff --git a/laika_repo b/laika_repo index 2e5c7b8a85..b30afe44d2 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 2e5c7b8a85cbaebda7fa715fe53d5e6ecbd62b0a +Subproject commit b30afe44d283072b6ee0ea17f290cca5d002baf7 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index b8d86c63fd..e85cd0ad42 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -17,7 +17,7 @@ from common.params import Params, put_nonblocking from laika import AstroDog from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed -from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem +from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom @@ -139,17 +139,22 @@ class Laikad: if self.use_qcom: return gnss_msg.which() == 'drSvPoly' else: - return gnss_msg.which() == 'ephemeris' + return gnss_msg.which() in ('ephemeris', 'glonassEphemeris') def read_ephemeris(self, gnss_msg): - # TODO this only works on GLONASS if self.use_qcom: # TODO this is not robust to gps week rollover if self.gps_week is None: return ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) else: - ephem = convert_ublox_ephem(gnss_msg.ephemeris) + if gnss_msg.which() == 'ephemeris': + ephem = convert_ublox_gps_ephem(gnss_msg.ephemeris) + elif gnss_msg.which() == 'glonassEphemeris': + ephem = convert_ublox_glonass_ephem(gnss_msg.glonassEphemeris) + else: + cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}") + return self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris(t=ephem.epoch) @@ -410,8 +415,13 @@ def main(sm=None, pm=None, qc=None): if pm is None: pm = messaging.PubMaster(['gnssMeasurements']) + # disable until set as main gps source, to better analyze startup time + use_internet = False #"LAIKAD_NO_INTERNET" not in os.environ + replay = "REPLAY" in os.environ - use_internet = "LAIKAD_NO_INTERNET" not in os.environ + if replay or "CI" in os.environ: + use_internet = True + laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom) while True: