diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 53fbf3606..5b12c7a16 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -94,6 +94,7 @@ class Laikad: nav_list: List = sum([v for k,v in self.astro_dog.navs.items()], []) ephem_cache = ephemeris_structs.EphemerisCache(**{'glonassEphemerides': [e.data for e in nav_list if e.prn[0]=='R'], 'gpsEphemerides': [e.data for e in nav_list if e.prn[0]=='G']}) + put_nonblocking(EPHEMERIS_CACHE, ephem_cache.to_bytes()) cloudlog.debug("Cache saved") self.last_cached_t = self.last_report_time @@ -157,6 +158,8 @@ class Laikad: if self.gps_week is None: return ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) + self.astro_dog.add_orbits({ephem.prn: [ephem]}) + else: if gnss_msg.which() == 'ephemeris': data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict()) @@ -167,7 +170,7 @@ class Laikad: else: cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}") return - self.astro_dog.add_navs({ephem.prn: [ephem]}) + self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris() def process_report(self, new_meas, t): @@ -421,7 +424,6 @@ def main(sm=None, pm=None, qc=None): if sm.updated[raw_gnss_socket]: gnss_msg = sm[raw_gnss_socket] - msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay) if msg is None: # TODO: beautify this, locationd needs a valid message diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index be79ff16b..b22c9994b 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -15,7 +15,7 @@ from laika.downloader import DownloadFailed from laika.ephemeris import EphemerisType, GPSEphemeris, ephemeris_structs from laika.gps_time import GPSTime from laika.helpers import ConstellationId, TimeRangeHolder -from laika.raw_gnss import GNSSMeasurement, read_raw_ublox +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.test.openpilotci import get_url from tools.lib.logreader import LogReader @@ -70,14 +70,23 @@ def get_log(segs=range(0)): low_gnss.append(mb.as_reader()) else: low_gnss.append(m) - return all_logs, low_gnss +def get_log_qcom(segs=range(0)): + logs = [] + for i in segs: + logs.extend(LogReader(get_url("b0b3cba7abf862d1|2023-03-11--09-40-33", i))) + all_logs = [m for m in logs if m.which() == 'qcomGnss'] + return all_logs def verify_messages(lr, laikad, return_one_success=False): good_msgs = [] for m in lr: - msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True) + if m.which() == 'ubloxGnss': + gnss_msg = m.ubloxGnss + else: + gnss_msg = m.qcomGnss + msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=True) if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: good_msgs.append(msg) if return_one_success: @@ -87,10 +96,16 @@ def verify_messages(lr, laikad, return_one_success=False): def get_first_gps_time(logs): for m in logs: - if m.ubloxGnss.which == 'measurementReport': - new_meas = read_raw_ublox(m.ubloxGnss.measurementReport) - if len(new_meas) > 0: - return new_meas[0].recv_time + if m.which() == 'ubloxGnss': + if m.ubloxGnss.which == 'measurementReport': + new_meas = read_raw_ublox(m.ubloxGnss.measurementReport) + if len(new_meas) > 0: + return new_meas[0].recv_time + else: + if m.qcomGnss.which == 'measurementReport': + new_meas = read_raw_qcom(m.qcomGnss.measurementReport) + if len(new_meas) > 0: + return new_meas[0].recv_time def get_measurement_mock(gpstime, sat_ephemeris): @@ -111,6 +126,7 @@ class TestLaikad(unittest.TestCase): logs, low_gnss = get_log(range(1)) cls.logs = logs cls.low_gnss = low_gnss + cls.logs_qcom = get_log_qcom(range(1)) first_gps_time = get_first_gps_time(logs) cls.first_gps_time = first_gps_time @@ -209,12 +225,13 @@ class TestLaikad(unittest.TestCase): self.assertTrue(kf_valid) def test_laika_online_nav_only(self): - laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV) - # Disable fetch_orbits to test NAV only - correct_msgs = verify_messages(self.logs, laikad) - correct_msgs_expected = 559 - 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])) + for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]): + laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom) + # Disable fetch_orbits to test NAV only + correct_msgs = verify_messages(logs, laikad) + correct_msgs_expected = 42 if use_qcom else 559 + 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])) @mock.patch('laika.downloader.download_and_cache_file') def test_laika_offline(self, downloader_mock): @@ -247,65 +264,73 @@ class TestLaikad(unittest.TestCase): print(min(laikad.astro_dog.navs[prn], key=lambda e: e.epoch).epoch.as_datetime()) def test_get_navs_in_process(self): - laikad = Laikad(auto_update=False) - has_navs = False - for m in self.logs: - laikad.process_gnss_msg(m.ubloxGnss, 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 - if has_navs: - break - self.assertTrue(has_navs) - self.assertGreater(len(laikad.astro_dog.navs_fetched_times._ranges), 0) - self.assertEqual(None, laikad.orbit_fetch_future) + for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]): + laikad = Laikad(auto_update=False, use_qcom=use_qcom) + 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.orbits.values() + has_polys = len(vals) > 0 and max([len(v) for v in vals]) > 0 + if out_msg is not None: + has_fix = has_fix or out_msg.gnssMeasurements.positionECEF.valid + + self.assertTrue(has_navs or has_polys) + self.assertTrue(has_fix) + self.assertGreater(len(laikad.astro_dog.navs_fetched_times._ranges), 0) + self.assertEqual(None, laikad.orbit_fetch_future) def test_cache(self): - laikad = Laikad(auto_update=True, save_ephemeris=True) - - def wait_for_cache(): - max_time = 2 - while Params().get(EPHEMERIS_CACHE) is None: - time.sleep(0.1) - max_time -= 0.1 - if max_time < 0: - self.fail("Cache has not been written after 2 seconds") - - # Test cache with no ephemeris - laikad.last_report_time = GPSTime(1,0) - laikad.cache_ephemeris() - wait_for_cache() - 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 - wait_for_cache() - - # Check both nav and orbits separate - laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True) - # 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(self.logs, laikad, return_one_success=True) - self.assertIsNotNone(msg) - - with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method: - # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently - laikad.astro_dog.orbit_fetched_times = TimeRangeHolder() - laikad.fetch_navs(self.first_gps_time, block=False) - mock_method.assert_not_called() - - # Verify cache is working for only orbits by running a segment - laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT, save_ephemeris=True) - msg = verify_messages(self.logs, laikad, return_one_success=True) + use_qcom = True + for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]): + laikad = Laikad(auto_update=True, save_ephemeris=True, use_qcom=use_qcom) + def wait_for_cache(): + max_time = 2 + while Params().get(EPHEMERIS_CACHE) is None: + time.sleep(0.1) + max_time -= 0.1 + if max_time < 0: + self.fail("Cache has not been written after 2 seconds") + + # Test cache with no ephemeris + laikad.last_report_time = GPSTime(1,0) + laikad.cache_ephemeris() + wait_for_cache() + 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 + 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) + # 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.assertIsNotNone(msg) - # Verify orbit data is not downloaded - mock_method.assert_not_called() + with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method: + # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently + laikad.astro_dog.orbit_fetched_times = TimeRangeHolder() + laikad.fetch_navs(self.first_gps_time, block=False) + mock_method.assert_not_called() + + # Verify cache is working for only orbits by running a segment + laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT, save_ephemeris=True) + msg = verify_messages(self.logs, laikad, return_one_success=True) + self.assertIsNotNone(msg) + # Verify orbit data is not downloaded + mock_method.assert_not_called() + break def test_low_gnss_meas(self): cnt = 0 @@ -323,6 +348,5 @@ class TestLaikad(unittest.TestCase): self.assertGreater(len(dct), 0) self.assertGreater(min([len(v) for v in dct.values()]), 0) - if __name__ == "__main__": unittest.main()