diff --git a/laika_repo b/laika_repo index 231eafbf65..e816bbe6bd 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 231eafbf659309b85acb5b575b7f898e7a4f196e +Subproject commit e816bbe6bd6e304dfd7669174ad12491b3792801 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 86f214976c..eee07e7805 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -1,4 +1,6 @@ #!/usr/bin/env python3 +import threading +import time from typing import List import numpy as np @@ -8,7 +10,9 @@ from numpy.linalg import linalg from cereal import log, messaging from laika import AstroDog -from laika.ephemeris import convert_ublox_ephem +from laika.constants import SECS_IN_HR, SECS_IN_MIN +from laika.ephemeris import EphemerisType, convert_ublox_ephem +from laika.gps_time import GPSTime from laika.helpers import ConstellationId from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind @@ -22,14 +26,18 @@ MAX_TIME_GAP = 10 class Laikad: - def __init__(self, use_internet): - self.astro_dog = AstroDog(use_internet=use_internet) + 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) 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): if ublox_msg.which == 'measurementReport': report = ublox_msg.measurementReport new_meas = read_raw_ublox(report) + if report.gpsWeek > 0: + self.latest_time_msg = GPSTime(report.gpsWeek, report.rcvTow) measurements = process_measurements(new_meas, self.astro_dog) pos_fix = calc_pos_fix(measurements, min_measurements=4) # To get a position fix a minimum of 5 measurements are needed. @@ -41,6 +49,7 @@ class Laikad: t = ublox_mono_time * 1e-9 self.update_localizer(pos_fix, t, corrected_measurements) localizer_valid = self.localizer_valid(t) + ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() @@ -50,7 +59,6 @@ class Laikad: bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] - dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { @@ -63,7 +71,7 @@ class Laikad: return dat elif ublox_msg.which == 'ephemeris': ephem = convert_ublox_ephem(ublox_msg.ephemeris) - self.astro_dog.add_ephem(ephem, self.astro_dog.orbits) + self.astro_dog.add_ephems([ephem], self.astro_dog.nav) # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. @@ -90,8 +98,7 @@ class Laikad: def localizer_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time - return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and \ - all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) + return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) def init_gnss_localizer(self, est_pos): x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial)) @@ -100,6 +107,22 @@ class Laikad: self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) + 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) + 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 + def create_measurement_msg(meas: GNSSMeasurement): c = log.GnssMeasurements.CorrectedMeasurement.new_message() @@ -144,16 +167,22 @@ def main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) - laikad = Laikad(use_internet=True) - - while True: - sm.update() - - if sm.updated['ubloxGnss']: - ublox_msg = sm['ubloxGnss'] - msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) - if msg is not None: - pm.send('gnssMeasurements', msg) + laikad = Laikad() + + end_event = threading.Event() + threading.Thread(target=laikad.orbit_thread, args=(end_event,)).start() + try: + while not end_event.is_set(): + sm.update() + + if sm.updated['ubloxGnss']: + ublox_msg = sm['ubloxGnss'] + msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss']) + if msg is not None: + pm.send('gnssMeasurements', msg) + except (KeyboardInterrupt, SystemExit): + end_event.set() + raise if __name__ == "__main__": diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index bf984becb8..baa17792bd 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -2,9 +2,10 @@ import unittest from datetime import datetime +from laika.ephemeris import EphemerisType from laika.gps_time import GPSTime from laika.helpers import ConstellationId -from laika.raw_gnss import GNSSMeasurement +from laika.raw_gnss import GNSSMeasurement, read_raw_ublox from selfdrive.locationd.laikad import Laikad, create_measurement_msg from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader @@ -17,11 +18,11 @@ def get_log(segs=range(0)): return [m for m in logs if m.which() == 'ubloxGnss'] -def process_msgs(lr, laikad: Laikad): +def verify_messages(lr, laikad): good_msgs = [] for m in lr: msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime) - if msg is not None: + if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0: good_msgs.append(msg) return good_msgs @@ -42,32 +43,63 @@ class TestLaikad(unittest.TestCase): self.assertEqual(msg.constellationId, 'gps') def test_laika_online(self): - # Set to offline forces to use ephemeris messages - laikad = Laikad(use_internet=True) - msgs = process_msgs(self.logs, laikad) - correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] + laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT) + 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])) + + 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])) def test_laika_offline(self): - # Set to offline forces to use ephemeris messages - laikad = Laikad(use_internet=False) - msgs = process_msgs(self.logs, laikad) - correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] + # Set auto_update to false forces to use ephemeris messages + laikad = Laikad(auto_update=False) + correct_msgs = verify_messages(self.logs, laikad) self.assertEqual(256, len(correct_msgs)) self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) def test_laika_offline_ephem_at_start(self): # Test offline but process ephemeris msgs of segment first - laikad = Laikad(use_internet=False) + laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV) ephemeris_logs = [m for m in self.logs if m.ubloxGnss.which() == 'ephemeris'] - msgs = process_msgs(ephemeris_logs+self.logs, laikad) - correct_msgs = [m for m in msgs if len(m.gnssMeasurements.correctedMeasurements) > 0] + correct_msgs = verify_messages(ephemeris_logs+self.logs, laikad) + self.assertEqual(554, len(correct_msgs)) self.assertGreaterEqual(554, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) + def test_laika_get_orbits(self): + laikad = Laikad(auto_update=False) + first_gps_time = None + for m in self.logs: + if m.ubloxGnss.which == 'measurementReport': + new_meas = read_raw_ublox(m.ubloxGnss.measurementReport) + if len(new_meas) != 0: + first_gps_time = new_meas[0].recv_time + 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())) + 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])) + + @unittest.skip("Use to debug live data") + 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()) + if __name__ == "__main__": unittest.main()