From 04a7808590ac55a5e3dea6080b6a24aae78d9277 Mon Sep 17 00:00:00 2001
From: Kurt Nistelberger <kurt.nistelberger@hotmail.com>
Date: Thu, 2 Feb 2023 21:16:28 -0700
Subject: [PATCH] glonass support laikad (#27173)

* add glonass kaitai parsing

* add kaita generated files

* remove glonass from build

* add string non immediate type

* fix kaitai bug

* cleanUp

* add patch file

* fix scons order

* make lookup const

* remove comments

* add to release files

* init

* add laika

* remove printf

* laikad add glonass ephem parsing

* add signs

* final ublox parsing

* rev gps

* bump subs

* address comments

* disable internet for laikad

---------

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
---
 laika_repo                    |  2 +-
 selfdrive/locationd/laikad.py | 20 +++++++++++++++-----
 2 files changed, 16 insertions(+), 6 deletions(-)

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: