use nav and correct

selfdrive/locationd/laikad.py

@@ -34,12 +34,12 @@ POS_FIX_RESIDUAL_THRESHOLD = 100.0
 
 
 class Laikad:
-  def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False,
-               valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV),
+  def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False,
+               valid_ephem_types=(EphemerisType.NAV,),
                save_ephemeris=False, use_qcom=False):
     """
     valid_const: GNSS constellation which can be used
-    auto_fetch_orbits: If true fetch orbits from internet when needed
+    auto_fetch_navs: If true fetch navs from internet when needed
     auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere.
     valid_ephem_types: Valid ephemeris types to be used by AstroDog
     save_ephemeris: If true saves and loads nav and orbit ephemeris to cache.
@@ -47,11 +47,11 @@ class Laikad:
     self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER)
     self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom)
 
-    self.auto_fetch_orbits = auto_fetch_orbits
+    self.auto_fetch_navs = auto_fetch_navs
     self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
     self.orbit_fetch_future: Optional[Future] = None
 
-    self.last_fetch_orbits_t = None
+    self.last_fetch_navs_t = None
     self.got_first_gnss_msg = False
     self.last_cached_t = None
     self.save_ephemeris = save_ephemeris
@@ -74,20 +74,19 @@ class Laikad:
 
     try:
       cache = json.loads(cache, object_hook=deserialize_hook)
-      self.astro_dog.add_orbits(cache['orbits'])
-      self.astro_dog.add_navs(cache['nav'])
-      self.last_fetch_orbits_t = cache['last_fetch_orbits_t']
+      self.astro_dog.add_navs(cache['navs'])
+      self.last_fetch_navs_t = cache['last_fetch_navs_t']
     except json.decoder.JSONDecodeError:
       cloudlog.exception("Error parsing cache")
-    timestamp = self.last_fetch_orbits_t.as_datetime() if self.last_fetch_orbits_t is not None else 'Nan'
+    timestamp = self.last_fetch_navs_t.as_datetime() if self.last_fetch_navs_t is not None else 'Nan'
     cloudlog.debug(
-      f"Loaded nav ({sum([len(v) for v in cache['nav']])}) and orbits ({sum([len(v) for v in cache['orbits']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['orbits'].keys())} {list(cache['nav'].keys())} " +
-      f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.orbit_fetched_times._ranges]}")
+      f"Loaded nav ({sum([len(v) for v in cache['nav']])}) and navs ({sum([len(v) for v in cache['navs']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['navs'].keys())} {list(cache['nav'].keys())} " +
+      f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.navs_fetched_times._ranges]}")
 
   def cache_ephemeris(self, t: GPSTime):
     if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN):
       put_nonblocking(EPHEMERIS_CACHE, json.dumps(
-        {'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav},
+        {'version': CACHE_VERSION, 'last_fetch_navs_t': self.last_fetch_navs_t, 'navs': self.astro_dog.navs,},
         cls=CacheSerializer))
       cloudlog.debug("Cache saved")
       self.last_cached_t = t
@@ -155,8 +154,14 @@ class Laikad:
     # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
     new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
     processed_measurements = process_measurements(new_meas, self.astro_dog)
-
+    if self.last_fix_pos is not None:
+      corrected_measurements = correct_measurements(processed_measurements, self.last_fix_pos, self.astro_dog)
+      instant_fix = self.get_lsq_fix(t, corrected_measurements)
+      #instant_fix = self.get_lsq_fix(t, processed_measurements)
+    else:
+      corrected_measurements = []
       instant_fix = self.get_lsq_fix(t, processed_measurements)
+      print(len(processed_measurements))
     if instant_fix is None:
       return None
     else:
@@ -164,8 +169,6 @@ class Laikad:
       self.last_fix_t = t
       self.last_fix_pos = position_estimate
       self.lat_fix_pos_std = position_std
-
-    corrected_measurements = correct_measurements(processed_measurements, position_estimate, self.astro_dog)
     if (t*1e9) % 10 == 0:
       cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}")
     return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements
@@ -185,8 +188,8 @@ class Laikad:
       if week > 0:
         self.got_first_gnss_msg = True
         latest_msg_t = GPSTime(week, tow)
-        if self.auto_fetch_orbits:
-          self.fetch_orbits(latest_msg_t, block)
+        if self.auto_fetch_navs:
+          self.fetch_navs(latest_msg_t, block)
 
       output = self.process_report(new_meas, t)
       if output is None:
@@ -254,9 +257,9 @@ class Laikad:
     p_initial_diag[GStates.ECEF_POS] = 1000 ** 2
     self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag)
 
-  def fetch_orbits(self, t: GPSTime, block):
-    # Download new orbits if 1 hour of orbits data left
-    if t + SECS_IN_HR not in self.astro_dog.orbit_fetched_times and (self.last_fetch_orbits_t is None or abs(t - self.last_fetch_orbits_t) > SECS_IN_MIN):
+  def fetch_navs(self, t: GPSTime, block):
+    # Download new navs if 1 hour of navs data left
+    if t + SECS_IN_HR not in self.astro_dog.orbit_fetched_times and (self.last_fetch_navs_t is None or abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
       astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir
       ret = None
 
@@ -271,22 +274,22 @@ class Laikad:
 
       if ret is not None:
         if ret[0] is None:
-          self.last_fetch_orbits_t = ret[2]
+          self.last_fetch_navs_t = ret[2]
         else:
-          self.astro_dog.orbits, self.astro_dog.orbit_fetched_times, self.last_fetch_orbits_t = ret
+          self.astro_dog.navs, self.astro_dog.orbit_fetched_times, self.last_fetch_navs_t = ret
           self.cache_ephemeris(t=t)
 
 
 def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir):
   astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, cache_dir=cache_dir)
-  cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}")
+  cloudlog.info(f"Start to download/parse navs for time {t.as_datetime()}")
   start_time = time.monotonic()
   try:
-    astro_dog.get_orbit_data(t, only_predictions=True)
-    cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.1f}s")
-    cloudlog.debug(f"Downloaded orbits ({sum([len(v) for v in astro_dog.orbits])}): {list(astro_dog.orbits.keys())}" +
+    astro_dog.get_navs(t)
+    cloudlog.info(f"Done parsing navs. Took {time.monotonic() - start_time:.1f}s")
+    cloudlog.debug(f"Downloaded navs ({sum([len(v) for v in astro_dog.navs])}): {list(astro_dog.navs.keys())}" +
                    f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in astro_dog.orbit_fetched_times._ranges]}")
-    return astro_dog.orbits, astro_dog.orbit_fetched_times, t
+    return astro_dog.navs, astro_dog.navs_fetched_times, t
   except (DownloadFailed, RuntimeError, ValueError, IOError) as e:
     cloudlog.warning(f"No orbit data found or parsing failure: {e}")
   return None, None, t
@@ -413,7 +416,7 @@ def main(sm=None, pm=None, qc=None):
 
   replay = "REPLAY" in os.environ
   use_internet = "LAIKAD_NO_INTERNET" not in os.environ
-  laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet, use_qcom=use_qcom)
+  laikad = Laikad(save_ephemeris=not replay, auto_fetch_navs=use_internet, use_qcom=use_qcom)
 
   log_done = False
 
@@ -432,8 +435,8 @@ def main(sm=None, pm=None, qc=None):
     if not laikad.got_first_gnss_msg and sm.updated['clocks']:
       clocks_msg = sm['clocks']
       t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
-      if laikad.auto_fetch_orbits:
-        laikad.fetch_orbits(t, block=replay)
+      if laikad.auto_fetch_navs:
+        laikad.fetch_navs(t, block=replay)
 
     if not log_done:
       debug_log_cache()