diff --git a/laika_repo b/laika_repo index ba6ed3277c..d871946134 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit ba6ed3277cadfdc8697206784afbd7f9a223798b +Subproject commit d87194613455b42af19ff2b5a3f7d1cae5852885 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index eee07e7805..d8e32005f8 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -6,11 +6,9 @@ from typing import List import numpy as np from collections import defaultdict -from numpy.linalg import linalg - from cereal import log, messaging from laika import AstroDog -from laika.constants import SECS_IN_HR, SECS_IN_MIN +from laika.constants import SECS_IN_MIN from laika.ephemeris import EphemerisType, convert_ublox_ephem from laika.gps_time import GPSTime from laika.helpers import ConstellationId @@ -26,10 +24,9 @@ MAX_TIME_GAP = 10 class Laikad: - def __init__(self, valid_const=("GPS",), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): - self.astro_dog = AstroDog(valid_const=valid_const, use_internet=auto_update, valid_ephem_types=valid_ephem_types) + def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV)): + self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types) self.gnss_kf = GNSSKalman(GENERATED_DIR) - self.latest_epoch_fetched = GPSTime(0, 0) self.latest_time_msg = None def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): @@ -43,7 +40,7 @@ class Laikad: # To get a position fix a minimum of 5 measurements are needed. # Each report can contain less and some measurements can't be processed. corrected_measurements = [] - if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100: + if len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog) t = ublox_mono_time * 1e-9 @@ -110,18 +107,15 @@ class Laikad: def orbit_thread(self, end_event: threading.Event): while not end_event.is_set(): if self.latest_time_msg: - self.fetch_orbits(self.latest_time_msg) + self.fetch_orbits(self.latest_time_msg + SECS_IN_MIN) time.sleep(0.1) def fetch_orbits(self, t: GPSTime): - if self.latest_epoch_fetched < t + SECS_IN_MIN: - cloudlog.info("Start to download/parse orbits") - orbit_ephems = self.astro_dog.download_parse_orbit_data(t, skip_before_epoch=t - 2 * SECS_IN_HR) - if len(orbit_ephems) > 0: - cloudlog.info(f"downloaded and parsed correctly new orbits {len(orbit_ephems)}, Constellations:{set([e.prn[0] for e in orbit_ephems])}") - self.astro_dog.add_ephems(orbit_ephems, self.astro_dog.orbits) - latest_orbit = max(orbit_ephems, key=lambda e: e.epoch) # type: ignore - self.latest_epoch_fetched = latest_orbit.epoch + if t not in self.astro_dog.orbit_fetched_times: + cloudlog.info(f"Start to download/parse orbits for time {t.as_datetime()}") + start_time = time.monotonic() + self.astro_dog.get_orbit_data(t, only_predictions=True) + cloudlog.info(f"Done parsing orbits. Took {time.monotonic() - start_time:.2f}s") def create_measurement_msg(meas: GNSSMeasurement): diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index baa17792bd..8cb0773b37 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -53,7 +53,6 @@ class TestLaikad(unittest.TestCase): def test_laika_online_nav_only(self): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) correct_msgs = verify_messages(self.logs, laikad) - correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(correct_msgs_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) @@ -86,7 +85,7 @@ class TestLaikad(unittest.TestCase): break # Pretend thread has loaded the orbits on startup by using the time of the first gps message. laikad.fetch_orbits(first_gps_time) - self.assertEqual(31, len(laikad.astro_dog.orbits.keys())) + self.assertEqual(29, len(laikad.astro_dog.orbits.keys())) correct_msgs = verify_messages(self.logs, laikad) correct_msgs_expected = 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) @@ -96,10 +95,11 @@ class TestLaikad(unittest.TestCase): def test_laika_get_orbits_now(self): laikad = Laikad(auto_update=False) laikad.fetch_orbits(GPSTime.from_datetime(datetime.utcnow())) - print(laikad.latest_epoch_fetched.as_datetime()) - - print(min(laikad.astro_dog.orbits[list(laikad.astro_dog.orbits.keys())[0]], key=lambda e: e.epoch).epoch.as_datetime()) - + prn = "G01" + self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + prn = "R01" + self.assertLess(0, len(laikad.astro_dog.orbits[prn])) + print(min(laikad.astro_dog.orbits[prn], key=lambda e: e.epoch).epoch.as_datetime()) if __name__ == "__main__": unittest.main()