From db93603e57c9266c505c069fb1d86c2c6a9b5a31 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Mon, 9 Jan 2023 21:58:58 -0800 Subject: [PATCH] use nav and correct --- selfdrive/locationd/laikad.py | 65 ++++++++++++++++++----------------- 1 file changed, 34 insertions(+), 31 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index d03dffa6f4..0366e59de8 100755 --- a/selfdrive/locationd/laikad.py +++ b/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) - - instant_fix = self.get_lsq_fix(t, processed_measurements) + 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()