diff --git a/cereal b/cereal index 80a8eb8459..7492dc3f45 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 80a8eb84593feb08dfc0bd47e5c04e586ea4b7a6 +Subproject commit 7492dc3f45702dfdbe0f353f38844e7fbf39ca71 diff --git a/laika_repo b/laika_repo index 8463fe8fed..719c79a156 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 8463fe8fed272f58093406a990532084bc060359 +Subproject commit 719c79a156cd254fa8ebde101c2f48583f91afed diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index fe5126522c..695e9adc03 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -18,7 +18,7 @@ from laika.constants import SECS_IN_HR, SECS_IN_MIN from laika.downloader import DownloadFailed from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem from laika.gps_time import GPSTime -from laika.helpers import ConstellationId +from laika.helpers import ConstellationId, get_sv_id from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind @@ -33,6 +33,44 @@ CACHE_VERSION = 0.2 POS_FIX_RESIDUAL_THRESHOLD = 100.0 +class LogEphemerisType(IntEnum): + nav = 0 + nasaUltraRapid = 1 + glonassIacUltraRapid = 2 + qcom = 3 + +class EphemerisSource(IntEnum): + gnssChip = 0 + internet = 1 + cache = 2 + unknown = 3 + +def get_log_eph_type(ephem): + if ephem.eph_type == EphemerisType.NAV: + source_type = LogEphemerisType.nav + elif ephem.eph_type == EphemerisType.QCOM_POLY: + source_type = LogEphemerisType.qcom + else: + assert ephem.file_epoch is not None + file_src = ephem.file_source + if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z' + source_type = LogEphemerisType.nasaUltraRapid + elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3' + source_type = LogEphemerisType.glonassIacUltraRapid + else: + raise Exception(f"Didn't expect file source {file_src}") + return source_type + +def get_log_eph_source(ephem): + if ephem.file_name == 'qcom' or ephem.file_name == 'ublox': + source = EphemerisSource.gnssChip + elif ephem.file_name == EPHEMERIS_CACHE: + source = EphemerisSource.cache + else: + source = EphemerisSource.internet + return source + + class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False, valid_ephem_types=(EphemerisType.NAV, EphemerisType.QCOM_POLY), @@ -64,6 +102,8 @@ class Laikad: self.last_fix_t = None self.gps_week = None self.use_qcom = use_qcom + self.first_log_time = None + self.ttff = -1 def load_cache(self): if not self.save_ephemeris: @@ -76,8 +116,8 @@ class Laikad: nav_dict = {} try: ephem_cache = ephemeris_structs.EphemerisCache.from_bytes(cache_bytes) - glonass_navs = [GLONASSEphemeris(data_struct) for data_struct in ephem_cache.glonassEphemerides] - gps_navs = [GPSEphemeris(data_struct) for data_struct in ephem_cache.gpsEphemerides] + glonass_navs = [GLONASSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.glonassEphemerides] + gps_navs = [GPSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.gpsEphemerides] for e in sum([glonass_navs, gps_navs], []): if e.prn not in nav_dict: nav_dict[e.prn] = [] @@ -99,6 +139,21 @@ class Laikad: cloudlog.debug("Cache saved") self.last_cached_t = self.last_report_time + def create_ephem_statuses(self): + ephemeris_statuses = [] + prns_to_check = self.astro_dog.get_all_ephem_prns() + for prn in prns_to_check: + eph = self.astro_dog.get_eph(prn, self.last_report_time) + if eph is not None: + status = log.GnssMeasurements.EphemerisStatus.new_message() + status.constellationId = ConstellationId.from_rinex_char(prn[0]).value + status.svId = get_sv_id(prn) + status.type = get_log_eph_type(eph).value + status.source = get_log_eph_source(eph).value + ephemeris_statuses.append(status) + return ephemeris_statuses + + def get_lsq_fix(self, t, measurements): if self.last_fix_t is None or abs(self.last_fix_t - t) > 0: min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4 @@ -170,10 +225,10 @@ class Laikad: else: if gnss_msg.which() == 'ephemeris': data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict()) - ephem = GPSEphemeris(data_struct) + ephem = GPSEphemeris(data_struct, file_name='ublox') elif gnss_msg.which() == 'glonassEphemeris': data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict()) - ephem = GLONASSEphemeris(data_struct) + ephem = GLONASSEphemeris(data_struct, file_name='ublox') else: cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}") return @@ -204,6 +259,12 @@ class Laikad: def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): out_msg = messaging.new_message("gnssMeasurements") + out_msg.gnssMeasurements = { + "timeToFirstFix": self.ttff, + "ephemerisStatuses" : self.create_ephem_statuses(), + } + if self.first_log_time is None: + self.first_log_time = 1e-9 * gnss_mono_time if self.is_ephemeris(gnss_msg): self.read_ephemeris(gnss_msg) return out_msg @@ -223,6 +284,8 @@ class Laikad: output = self.process_report(new_meas, t) if output is None: return out_msg + if self.ttff <= 0: + self.ttff = max(1e-3, t - self.first_log_time) position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output self.update_localizer(position_estimate, t, corrected_measurements) @@ -244,14 +307,12 @@ class Laikad: "velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)), "measTime": gnss_mono_time, - "correctedMeasurements": meas_msgs + "correctedMeasurements": meas_msgs, + "timeToFirstFix": self.ttff, + "ephemerisStatuses" : self.create_ephem_statuses(), } return out_msg - #elif gnss_msg.which() == 'ionoData': - # TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them. - - def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid valid = self.kf_valid(t) @@ -335,31 +396,8 @@ def create_measurement_msg(meas: GNSSMeasurement): c.satPos = meas.sat_pos_final.tolist() c.satVel = meas.sat_vel.tolist() c.satVel = meas.sat_vel.tolist() - ephem = meas.sat_ephemeris - assert ephem is not None - week, time_of_week = -1, -1 - if ephem.eph_type == EphemerisType.NAV: - source_type = EphemerisSourceType.nav - elif ephem.eph_type == EphemerisType.QCOM_POLY: - source_type = EphemerisSourceType.qcom - else: - assert ephem.file_epoch is not None - week = ephem.file_epoch.week - time_of_week = ephem.file_epoch.tow - file_src = ephem.file_source - if file_src == 'igu': # example nasa: '2214/igu22144_00.sp3.Z' - source_type = EphemerisSourceType.nasaUltraRapid - elif file_src == 'Sta': # example nasa: '22166/ultra/Stark_1D_22061518.sp3' - source_type = EphemerisSourceType.glonassIacUltraRapid - else: - raise Exception(f"Didn't expect file source {file_src}") - - c.ephemerisSource.type = source_type.value - c.ephemerisSource.gpsWeek = week - c.ephemerisSource.gpsTimeOfWeek = int(time_of_week) return c - def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]): ekf_data = defaultdict(list) for m in measurements: @@ -375,18 +413,6 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe gnss_kf.predict_and_observe(t, kind, data) -class EphemerisSourceType(IntEnum): - nav = 0 - nasaUltraRapid = 1 - glonassIacUltraRapid = 2 - qcom = 3 - - -def process_msg(laikad, gnss_msg, mono_time, block=False): - - return laikad.process_gnss_msg(gnss_msg, mono_time, block=block) - - def clear_tmp_cache(): if os.path.exists(DOWNLOADS_CACHE_FOLDER): shutil.rmtree(DOWNLOADS_CACHE_FOLDER) diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 2e03551f6e..265db50c6d 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -12,11 +12,11 @@ from tqdm import tqdm from laika.constants import SECS_IN_DAY from laika.downloader import DownloadFailed -from laika.ephemeris import EphemerisType, GPSEphemeris, ephemeris_structs +from laika.ephemeris import EphemerisType from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, read_raw_ublox, read_raw_qcom -from selfdrive.locationd.laikad import EPHEMERIS_CACHE, EphemerisSourceType, Laikad, create_measurement_msg +from selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader from selfdrive.manager.process_config import managed_processes @@ -138,9 +138,9 @@ class TestLaikad(unittest.TestCase): laikad = Laikad() laikad.fetch_navs(gpstime, block=False) laikad.orbit_fetch_future.result(30) + # Get results and save orbits to laikad: laikad.fetch_navs(gpstime, block=False) - ephem = laikad.astro_dog.navs['G01'][0] self.assertIsNotNone(ephem) @@ -153,6 +153,7 @@ class TestLaikad(unittest.TestCase): self.assertIsNotNone(ephem) self.assertNotEqual(ephem, ephem2) + def test_fetch_navs_with_wrong_clocks(self): laikad = Laikad() @@ -175,35 +176,6 @@ class TestLaikad(unittest.TestCase): check_has_navs() self.assertEqual(laikad.last_fetch_navs_t, real_current_time) - def test_ephemeris_source_in_msg(self): - dicto = {'svId': 1} - data_mock = ephemeris_structs.Ephemeris.new_message(**dicto) - - gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC - laikad = Laikad() - laikad.fetch_navs(gpstime, block=True) - meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0]) - msg = create_measurement_msg(meas) - self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) - # Verify gps satellite returns same source - meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0]) - msg = create_measurement_msg(meas) - self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) - - # Test nasa source by using older date - gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) - laikad = Laikad() - laikad.fetch_navs(gpstime, block=True) - meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['G01'][0]) - msg = create_measurement_msg(meas) - self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) - - # Test nav source type - ephem = GPSEphemeris(data_mock) - meas = get_measurement_mock(gpstime, ephem) - msg = create_measurement_msg(meas) - self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav) - def test_laika_online(self): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT) correct_msgs = verify_messages(self.logs, laikad) @@ -263,26 +235,34 @@ class TestLaikad(unittest.TestCase): self.assertGreater(len(laikad.astro_dog.navs[prn]), 0) def test_get_navs_in_process(self): - for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]): - laikad = Laikad(auto_update=False, use_qcom=use_qcom, auto_fetch_navs=False) - has_navs = False - has_fix = False - for m in logs: - gnss_msg = m.qcomGnss if use_qcom else m.ubloxGnss - out_msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=False) - if laikad.orbit_fetch_future is not None: - laikad.orbit_fetch_future.result() - vals = laikad.astro_dog.navs.values() - has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0 - vals = laikad.astro_dog.qcom_polys.values() - has_polys = len(vals) > 0 and max([len(v) for v in vals]) > 0 - if out_msg is not None: + for auto_fetch_navs in [True, False]: + for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]): + laikad = Laikad(auto_update=False, use_qcom=use_qcom, auto_fetch_navs=auto_fetch_navs) + has_navs = False + has_fix = False + seen_chip_eph = False + seen_internet_eph = False + + for m in logs: + gnss_msg = m.qcomGnss if use_qcom else m.ubloxGnss + out_msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=False) + if laikad.orbit_fetch_future is not None: + laikad.orbit_fetch_future.result() + vals = laikad.astro_dog.navs.values() + has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0 + vals = laikad.astro_dog.qcom_polys.values() + has_polys = len(vals) > 0 and max([len(v) for v in vals]) > 0 has_fix = has_fix or out_msg.gnssMeasurements.positionECEF.valid - - self.assertTrue(has_navs or has_polys) - self.assertTrue(has_fix) - self.assertEqual(len(laikad.astro_dog.navs_fetched_times._ranges), 0) - self.assertEqual(None, laikad.orbit_fetch_future) + if len(out_msg.gnssMeasurements.ephemerisStatuses): + seen_chip_eph = seen_chip_eph or any([x.source == 'gnssChip' for x in out_msg.gnssMeasurements.ephemerisStatuses]) + seen_internet_eph = seen_internet_eph or any([x.source == 'internet' for x in out_msg.gnssMeasurements.ephemerisStatuses]) + + self.assertTrue(has_navs or has_polys) + self.assertTrue(has_fix) + self.assertTrue(seen_chip_eph or auto_fetch_navs) + self.assertTrue(seen_internet_eph or not auto_fetch_navs) + self.assertEqual(len(laikad.astro_dog.navs_fetched_times._ranges), 0) + self.assertEqual(None, laikad.orbit_fetch_future) def test_cache(self): use_qcom = True @@ -303,18 +283,19 @@ class TestLaikad(unittest.TestCase): Params().remove(EPHEMERIS_CACHE) #laikad.astro_dog.get_navs(self.first_gps_time) - laikad.last_report_time = GPSTime(2,0) - laikad.fetch_navs(self.first_gps_time, block=True) - - # Wait for cache to save + msg = verify_messages(logs, laikad, return_one_success=True) + laikad.cache_ephemeris() wait_for_cache() # Check both nav and orbits separate - laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True, use_qcom=use_qcom) + laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, + save_ephemeris=True, use_qcom=use_qcom, auto_fetch_navs=False) # Verify navs are loaded from cache self.dict_has_values(laikad.astro_dog.navs) # Verify cache is working for only nav by running a segment msg = verify_messages(logs, laikad, return_one_success=True) + self.assertTrue(len(msg.gnssMeasurements.ephemerisStatuses)) + self.assertTrue(any([x.source=='cache' for x in msg.gnssMeasurements.ephemerisStatuses])) self.assertIsNotNone(msg) @@ -349,7 +330,5 @@ class TestLaikad(unittest.TestCase): def dict_has_values(self, dct): self.assertGreater(len(dct), 0) self.assertGreater(min([len(v) for v in dct.values()]), 0) - - if __name__ == "__main__": unittest.main() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 50288bfd03..fbe02bda3a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c8ba9450af16f9efc70eb76a5edc205aed53e779 +3b127e8aa63d3e5d260e130631ebc2dd67347556