sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ttff logs (#27610)

Browse Source 
* ttff in logs

* More meta info

* cleanup

* passess tests

* unused import

* fix linting

* ttff of 0 is invalid

* ref commit

* bump cereal

* Update ref_commit

* sort ephem status list

* sort ephem status list

* update ref
old-commit-hash: 204f7d8623
beeps
Harald Schäfer 2 years ago committed by GitHub
parent 10553c0288
commit 6d0116ae80
5 changed files with 112 additions and 106 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      cereal
  2. 2
      laika_repo
  3. 117
      selfdrive/locationd/laikad.py
  4. 61
      selfdrive/locationd/test/test_laikad.py
  5. 2
      selfdrive/test/process_replay/ref_commit

2
cereal

@ -1 +1 @@
Subproject commit 80a8eb84593feb08dfc0bd47e5c04e586ea4b7a6
Subproject commit 7492dc3f45702dfdbe0f353f38844e7fbf39ca71

2
laika_repo

@ -1 +1 @@
Subproject commit 8463fe8fed272f58093406a990532084bc060359
Subproject commit 719c79a156cd254fa8ebde101c2f48583f91afed

117
selfdrive/locationd/laikad.py
Unescape View File

@ -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,22 @@ class Laikad:
cloudlog.debug("Cache saved")
self.last_cached_t = self.last_report_time
def create_ephem_statuses(self):
ephemeris_statuses = []
prns_to_check = list(self.astro_dog.get_all_ephem_prns())
prns_to_check.sort()
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 +226,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 +260,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 +285,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 +308,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 +397,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 +414,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)

61
selfdrive/locationd/test/test_laikad.py
Unescape View File

@ -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,10 +235,14 @@ class TestLaikad(unittest.TestCase):
self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
def test_get_navs_in_process(self):
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=False)
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)
@ -276,11 +252,15 @@ class TestLaikad(unittest.TestCase):
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:
has_fix = has_fix or out_msg.gnssMeasurements.positionECEF.valid
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)
@ -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()

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
c8ba9450af16f9efc70eb76a5edc205aed53e779
5775220ec2e62dcdedb92d96270f1380bbd88c39

Write Preview
Loading…
Cancel
Save