diff --git a/cereal b/cereal index 4b4d9aab03..3166178611 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 4b4d9aab03937f5a45677ff15efb275abf9c958b +Subproject commit 3166178611989ca555d7b254130375d1d8a55cc8 diff --git a/laika_repo b/laika_repo index a3a80dc4f7..951ab080b9 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit a3a80dc4f7977b2232946e56a16770e413190818 +Subproject commit 951ab080b998ee3edde6229654d1a4cb63cda6a9 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 8eb81345ff..0e2838da6d 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -2,13 +2,13 @@ import json import time from concurrent.futures import Future, ProcessPoolExecutor +from enum import IntEnum from typing import List, Optional import numpy as np from collections import defaultdict import sympy -from numpy.linalg import linalg from cereal import log, messaging from common.params import Params, put_nonblocking @@ -30,7 +30,7 @@ CACHE_VERSION = 0.1 class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), - save_ephemeris=False): + save_ephemeris=False, last_known_position=None): self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True) self.gnss_kf = GNSSKalman(GENERATED_DIR) self.orbit_fetch_executor = ProcessPoolExecutor() @@ -40,6 +40,7 @@ class Laikad: self.save_ephemeris = save_ephemeris self.load_cache() self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} + self.last_pos_fix = last_known_position def load_cache(self): cache = Params().get(EPHEMERIS_CACHE) @@ -70,27 +71,16 @@ class Laikad: processed_measurements = process_measurements(new_meas, self.astro_dog) min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4 - pos_fix = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) + pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) + if len(pos_fix) > 0: + self.last_pos_fix = pos_fix[:3] + est_pos = self.last_pos_fix - t = ublox_mono_time * 1e-9 - kf_pos_std = None - if all(self.kf_valid(t)): - self.gnss_kf.predict(t) - kf_pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())) - # If localizer is valid use its position to correct measurements - if kf_pos_std is not None and linalg.norm(kf_pos_std) < 100: - est_pos = self.gnss_kf.x[GStates.ECEF_POS] - elif len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: - est_pos = pos_fix[0][:3] - else: - est_pos = None - corrected_measurements = [] - if est_pos is not None: - corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) + corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if est_pos is not None else [] + t = ublox_mono_time * 1e-9 self.update_localizer(est_pos, t, corrected_measurements) kf_valid = all(self.kf_valid(t)) - ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() @@ -103,6 +93,7 @@ class Laikad: dat.gnssMeasurements = { "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), + "positionFixECEF": measurement_msg(value=pos_fix, std=pos_fix_residual, valid=len(pos_fix) > 0), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -124,13 +115,7 @@ class Laikad: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") - else: - cloudlog.error("Gnss kalman std too far") - - if est_pos is None: - cloudlog.info("Position fix not available when resetting kalman filter") - return - self.init_gnss_localizer(est_pos.tolist()) + self.init_gnss_localizer(est_pos) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) else: @@ -141,14 +126,13 @@ class Laikad: filter_time = self.gnss_kf.filter.filter_time return [filter_time is not None, filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP, - all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])), - linalg.norm(self.gnss_kf.P[GStates.ECEF_POS]) < 1e5] + 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)) - x_initial[GStates.ECEF_POS] = est_pos + if est_pos is not None: + x_initial[GStates.ECEF_POS] = est_pos 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): @@ -192,6 +176,27 @@ def create_measurement_msg(meas: GNSSMeasurement): c.pseudorangeRateStd = float(meas.observables_std['D1C']) 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 + if ephem.eph_type == EphemerisType.NAV: + source_type = EphemerisSourceType.nav + week, time_of_week = -1, -1 + 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 = time_of_week return c @@ -242,12 +247,12 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C x0 = [0, 0, 0, 0, 0] n = len(measurements) if n < min_measurements: - return [] + return [], [] Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) x = gauss_newton(Fx_pos, x0) residual, _ = Fx_pos(x, weight=1.0) - return x, residual + return x.tolist(), residual.tolist() def pr_residual(measurements, posfix_functions, signal='C1C'): @@ -314,10 +319,16 @@ def get_posfix_sympy_fun(constellation): return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res) +class EphemerisSourceType(IntEnum): + nav = 0 + nasaUltraRapid = 1 + glonassIacUltraRapid = 2 + + def main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) - + # todo get last_known_position laikad = Laikad(save_ephemeris=True) while True: sm.update() diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 01ea8fee27..3a72303b0c 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -1,16 +1,17 @@ #!/usr/bin/env python3 import time import unittest +from collections import defaultdict from datetime import datetime from unittest import mock from unittest.mock import Mock, patch from common.params import Params -from laika.ephemeris import EphemerisType +from laika.ephemeris import EphemerisType, GPSEphemeris from laika.gps_time import GPSTime from laika.helpers import ConstellationId, TimeRangeHolder from laika.raw_gnss import GNSSMeasurement, read_raw_ublox -from selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad, create_measurement_msg +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 @@ -33,23 +34,62 @@ def verify_messages(lr, laikad, return_one_success=False): return good_msgs +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 + + +def get_measurement_mock(gpstime, sat_ephemeris): + meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}) + # Fake measurement being processed + meas.observables_final = meas.observables + meas.sat_ephemeris = sat_ephemeris + return meas + + class TestLaikad(unittest.TestCase): @classmethod def setUpClass(cls): - cls.logs = get_log(range(1)) + logs = get_log(range(1)) + cls.logs = logs + first_gps_time = get_first_gps_time(logs) + cls.first_gps_time = first_gps_time def setUp(self): Params().delete(EPHEMERIS_CACHE) - def test_create_msg_without_errors(self): - gpstime = GPSTime.from_datetime(datetime.now()) - meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}) - # Fake observables_final to be correct - meas.observables_final = meas.observables + def test_ephemeris_source_in_msg(self): + data_mock = defaultdict(str) + data_mock['sv_id'] = 1 + + gpstime = GPSTime.from_datetime(datetime(2022, month=3, day=1)) + laikad = Laikad() + laikad.fetch_orbits(gpstime, block=True) + meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0]) + msg = create_measurement_msg(meas) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid) + # Verify gps satellite returns same source + meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0]) + msg = create_measurement_msg(meas) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid) + + # Test nasa source by using older date + gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1)) + laikad = Laikad() + laikad.fetch_orbits(gpstime, block=True) + meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['G01'][0]) msg = create_measurement_msg(meas) + self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nasaUltraRapid) - self.assertEqual(msg.constellationId, 'gps') + # Test nav source type + ephem = GPSEphemeris(data_mock, gpstime) + 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) @@ -76,18 +116,10 @@ class TestLaikad(unittest.TestCase): self.assertEqual(256, len(correct_msgs)) self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) - def get_first_gps_time(self): - for m in self.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 - def test_laika_get_orbits(self): laikad = Laikad(auto_update=False) - first_gps_time = self.get_first_gps_time() # Pretend process has loaded the orbits on startup by using the time of the first gps message. - laikad.fetch_orbits(first_gps_time, block=True) + laikad.fetch_orbits(self.first_gps_time, block=True) self.dict_has_values(laikad.astro_dog.orbits) @unittest.skip("Use to debug live data") @@ -117,7 +149,6 @@ class TestLaikad(unittest.TestCase): def test_cache(self): laikad = Laikad(auto_update=True, save_ephemeris=True) - first_gps_time = self.get_first_gps_time() def wait_for_cache(): max_time = 2 @@ -126,13 +157,14 @@ class TestLaikad(unittest.TestCase): max_time -= 0.1 if max_time == 0: self.fail("Cache has not been written after 2 seconds") + # Test cache with no ephemeris laikad.cache_ephemeris(t=GPSTime(0, 0)) wait_for_cache() Params().delete(EPHEMERIS_CACHE) - laikad.astro_dog.get_navs(first_gps_time) - laikad.fetch_orbits(first_gps_time, block=True) + laikad.astro_dog.get_navs(self.first_gps_time) + laikad.fetch_orbits(self.first_gps_time, block=True) # Wait for cache to save wait_for_cache() @@ -149,7 +181,7 @@ class TestLaikad(unittest.TestCase): 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_orbits(first_gps_time, block=False) + laikad.fetch_orbits(self.first_gps_time, block=False) mock_method.assert_not_called() # Verify cache is working for only orbits by running a segment