mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-22 00:33:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use structs in laika (#27585)

Browse Source 
* doesnt crash

* New cacher

* unused import

* help linter

* Annotate list

* print error

* fix caching bugs

* wrong name

* small fixes

* fix sum

* wrong brackets

* fix tests

* update ref

* bump submodules
old-commit-hash: 5c704827619e774c9877b4598af06d1c6bca80d2
This commit is contained in:
Harald Schäfer
2023-03-15 00:02:56 -07:00
committed by GitHub
parent 7bd3d642aa
commit 20dfe96afa
7 changed files with 115 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/laikad.py
+34 -48
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env python3
import json
import math
import os
import time
@@ -17,7 +16,7 @@ from common.params import Params, put_nonblocking
from laika import AstroDog
from laika.constants import SECS_IN_HR, SECS_IN_MIN
from laika.downloader import DownloadFailed
from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_gps_ephem, convert_ublox_glonass_ephem, parse_qcom_ephem
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.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
@@ -51,10 +50,11 @@ class Laikad:
self.auto_fetch_navs = auto_fetch_navs
self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
self.orbit_fetch_future: Optional[Future] = None
self.last_fetch_navs_t = None
self.got_first_gnss_msg = False
self.last_cached_t = None
self.last_report_time = GPSTime(0, 0)
self.last_fetch_navs_t = GPSTime(0, 0)
self.last_cached_t = GPSTime(0, 0)
self.save_ephemeris = save_ephemeris
self.load_cache()
@@ -69,31 +69,34 @@ class Laikad:
if not self.save_ephemeris:
return
cache = Params().get(EPHEMERIS_CACHE)
if not cache:
cache_bytes = Params().get(EPHEMERIS_CACHE)
if not cache_bytes:
return
nav_dict = {}
try:
cache = json.loads(cache, object_hook=deserialize_hook)
if cache['version'] == CACHE_VERSION:
self.astro_dog.add_navs(cache['navs'])
self.last_fetch_navs_t = cache['last_fetch_navs_t']
else:
cache['navs'] = {}
except json.decoder.JSONDecodeError:
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]
for e in sum([glonass_navs, gps_navs], []):
if e.prn not in nav_dict:
nav_dict[e.prn] = []
nav_dict[e.prn].append(e)
self.astro_dog.add_navs(nav_dict)
except Exception:
cloudlog.exception("Error parsing cache")
timestamp = self.last_fetch_navs_t.as_datetime() if self.last_fetch_navs_t is not None else 'Nan'
cloudlog.debug(
f"Loaded navs ({sum([len(v) for v in cache['navs']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['navs'].keys())} " +
f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.navs_fetched_times._ranges]}")
f"Loaded navs ({sum([len(nav_dict[prn]) for prn in nav_dict.keys()])}). Unique orbit and nav sats: {list(nav_dict.keys())} ")
def cache_ephemeris(self, t: GPSTime):
if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN):
put_nonblocking(EPHEMERIS_CACHE, json.dumps(
{'version': CACHE_VERSION, 'last_fetch_navs_t': self.last_fetch_navs_t, 'navs': self.astro_dog.navs},
cls=CacheSerializer))
def cache_ephemeris(self):
if self.save_ephemeris and (self.last_report_time - self.last_cached_t > SECS_IN_MIN):
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 = t
self.last_cached_t = self.last_report_time
def get_lsq_fix(self, t, measurements):
if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
@@ -139,6 +142,7 @@ class Laikad:
week = report.gpsWeek
tow = report.rcvTow
new_meas = read_raw_ublox(report)
self.last_report_time = GPSTime(week, tow)
return week, tow, new_meas
def is_ephemeris(self, gnss_msg):
@@ -155,14 +159,16 @@ class Laikad:
ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week)
else:
if gnss_msg.which() == 'ephemeris':
ephem = convert_ublox_gps_ephem(gnss_msg.ephemeris)
data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict())
ephem = GPSEphemeris(data_struct)
elif gnss_msg.which() == 'glonassEphemeris':
ephem = convert_ublox_glonass_ephem(gnss_msg.glonassEphemeris)
data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict())
ephem = GLONASSEphemeris(data_struct)
else:
cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}")
return
self.astro_dog.add_navs({ephem.prn: [ephem]})
self.cache_ephemeris(t=ephem.epoch)
self.cache_ephemeris()
def process_report(self, new_meas, t):
# Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
@@ -272,7 +278,7 @@ class Laikad:
def fetch_navs(self, t: GPSTime, block):
# Download new navs if 1 hour of navs data left
if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (self.last_fetch_navs_t is None or abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
if t + SECS_IN_HR not in self.astro_dog.navs_fetched_times and (abs(t - self.last_fetch_navs_t) > SECS_IN_MIN):
astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir
ret = None
@@ -290,7 +296,7 @@ class Laikad:
self.last_fetch_navs_t = ret[2]
else:
self.astro_dog.navs, self.astro_dog.navs_fetched_times, self.last_fetch_navs_t = ret
self.cache_ephemeris(t=t)
self.cache_ephemeris()
def get_orbit_data(t: GPSTime, valid_const, auto_update, valid_ephem_types, cache_dir):
@@ -360,26 +366,6 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe
gnss_kf.predict_and_observe(t, kind, data)
class CacheSerializer(json.JSONEncoder):
def default(self, o):
if isinstance(o, Ephemeris):
return o.to_json()
if isinstance(o, GPSTime):
return o.__dict__
if isinstance(o, np.ndarray):
return o.tolist()
return json.JSONEncoder.default(self, o)
def deserialize_hook(dct):
if 'ephemeris' in dct:
return Ephemeris.from_json(dct)
if 'week' in dct:
return GPSTime(dct['week'], dct['tow'])
return dct
class EphemerisSourceType(IntEnum):
nav = 0
nasaUltraRapid = 1
selfdrive/locationd/test/test_laikad.py
+46 -11
View File
@@ -1,21 +1,51 @@
#!/usr/bin/env python3
import time
import unittest
from collections import defaultdict
from cereal import log
import cereal.messaging as messaging
from common.params import Params
from datetime import datetime
from unittest import mock
from unittest.mock import patch
from tqdm import tqdm
from common.params import Params
from laika.constants import SECS_IN_DAY
from laika.downloader import DownloadFailed
from laika.ephemeris import EphemerisType, GPSEphemeris
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 selfdrive.locationd.laikad import EPHEMERIS_CACHE, EphemerisSourceType, Laikad, create_measurement_msg
from selfdrive.test.openpilotci import get_url
from tools.lib.logreader import LogReader
from selfdrive.manager.process_config import managed_processes
from selfdrive.test.process_replay.helpers import OpenpilotPrefix
def get_ublox_gnss(ubloxraw):
with OpenpilotPrefix():
managed_processes['ubloxd'].start()
timeout_ms = 30
pm = messaging.PubMaster(['ubloxRaw'])
sock = messaging.sub_sock('ubloxGnss', timeout=timeout_ms)
log_msgs = []
log_t = []
for x in tqdm(ubloxraw):
pm.send(x.which(), x.as_builder())
ret = messaging.recv_one(sock)
if ret is not None:
msg = log.Event.new_message(ubloxGnss=ret.ubloxGnss.to_dict())
msg.logMonoTime = x.logMonoTime
log_msgs.append(msg)
log_t.append(1e-9 * x.logMonoTime)
assert managed_processes['ubloxd'].get_process_state_msg().running
assert len(log_msgs) > 1 or len(ubloxraw) == 0
managed_processes['ubloxd'].stop()
return log_t, log_msgs
def get_log(segs=range(0)):
@@ -23,7 +53,8 @@ def get_log(segs=range(0)):
for i in segs:
logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i)))
all_logs = [m for m in logs if m.which() == 'ubloxGnss']
raw_logs = [m for m in logs if m.which() == 'ubloxRaw']
all_logs = get_ublox_gnss(raw_logs)[1]
low_gnss = []
for m in all_logs:
if m.ubloxGnss.which() != 'measurementReport':
@@ -31,7 +62,8 @@ def get_log(segs=range(0)):
MAX_MEAS = 7
if m.ubloxGnss.measurementReport.numMeas > MAX_MEAS:
mb = m.as_builder()
mb = log.Event.new_message(ubloxGnss=m.ubloxGnss.to_dict())
mb.logMonoTime = m.logMonoTime
mb.ubloxGnss.measurementReport.numMeas = MAX_MEAS
mb.ubloxGnss.measurementReport.measurements = list(m.ubloxGnss.measurementReport.measurements)[:MAX_MEAS]
mb.ubloxGnss.measurementReport.measurements[0].pseudorange += 1000
@@ -128,8 +160,8 @@ class TestLaikad(unittest.TestCase):
self.assertEqual(laikad.last_fetch_navs_t, real_current_time)
def test_ephemeris_source_in_msg(self):
data_mock = defaultdict(str)
data_mock['sv_id'] = 1
dicto = {'svId': 1}
data_mock = ephemeris_structs.Ephemeris.new_message(**dicto)
gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC
laikad = Laikad()
@@ -151,7 +183,7 @@ class TestLaikad(unittest.TestCase):
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
# Test nav source type
ephem = GPSEphemeris(data_mock, gpstime)
ephem = GPSEphemeris(data_mock)
meas = get_measurement_mock(gpstime, ephem)
msg = create_measurement_msg(meas)
self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
@@ -195,8 +227,8 @@ class TestLaikad(unittest.TestCase):
downloader_mock.side_effect = DownloadFailed
laikad = Laikad(auto_update=False)
correct_msgs = verify_messages(self.logs, laikad)
self.assertEqual(255, len(correct_msgs))
self.assertEqual(255, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
self.assertEqual(375, len(correct_msgs))
self.assertEqual(375, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))
def test_laika_get_orbits(self):
laikad = Laikad(auto_update=False)
@@ -241,11 +273,13 @@ class TestLaikad(unittest.TestCase):
self.fail("Cache has not been written after 2 seconds")
# Test cache with no ephemeris
laikad.cache_ephemeris(t=GPSTime(0, 0))
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
@@ -272,6 +306,7 @@ class TestLaikad(unittest.TestCase):
# Verify orbit data is not downloaded
mock_method.assert_not_called()
def test_low_gnss_meas(self):
cnt = 0
laikad = Laikad()