Use structs in laika (#27585)

* 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

cereal

@@ -1 +1 @@
-Subproject commit 9888e0476c05069d46f273569467e4371b2d8690
+Subproject commit c579889f396cd754048c7b1a51c6f33b1988762a

laika_repo

@@ -1 +1 @@
-Subproject commit b740b71c82a748e3520b1599487d9a7aaf728670
+Subproject commit b896cdbbd1e8f85df25a1afa0c9a2ec150b72f92

selfdrive/locationd/laikad.py

@@ -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]}")
-
-  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))
+      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):
+
+    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

@@ -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()

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-3c5ebb007f76ba0de710ff7a8cf5910ad2edf22f
+b79fa775682401c25aa9ce38b5542b4206d0c56b

system/ubloxd/ublox_msg.cc

@@ -65,22 +65,6 @@ inline bool UbloxMsgParser::valid_so_far() {
   return true;
 }
 
-inline uint16_t UbloxMsgParser::get_glonass_year(uint8_t N4, uint16_t Nt) {
-  // convert time to year (conversion from A3.1.3)
-  int J = 0;
-  if (1 <= Nt && Nt <= 366) {
-    J = 1;
-  } else if (367 <= Nt && Nt <= 731) {
-    J = 2;
-  } else if (732 <= Nt && Nt <= 1096) {
-    J = 3;
-  } else if (1097 <= Nt && Nt <= 1461) {
-    J = 4;
-  }
-  uint16_t year = 1996 + 4*(N4 -1) + (J - 1);
-  return year;
-}
-
 bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
   last_log_time = log_time;
   int needed = needed_bytes();
@@ -203,6 +187,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
     int iode_s2 = 0;
     int iode_s3 = 0;
     int iodc_lsb = 0;
+    int week;
 
     // Subframe 1
     {
@@ -210,7 +195,14 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
       gps_t subframe(&stream);
       gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
 
-      eph.setGpsWeek(subframe_1->week_no());
+      // Each message is incremented to be greater or equal than week 1877 (2015-12-27).
+      //  To skip this use the current_time argument
+      week = subframe_1->week_no();
+      week += 1024;
+      if (week < 1877) {
+        week += 1024;
+      }
+      //eph.setGpsWeek(subframe_1->week_no());
       eph.setTgd(subframe_1->t_gd() * pow(2, -31));
       eph.setToc(subframe_1->t_oc() * pow(2, 4));
       eph.setAf2(subframe_1->af_2() * pow(2, -55));
@@ -227,6 +219,12 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
       gps_t subframe(&stream);
       gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
 
+      // GPS week refers to current week, the ephemeris can be valid for the next
+      // if toe equals 0, this can be verified by the TOW count if it is within the
+      // last 2 hours of the week (gps ephemeris valid for 4hours)
+      if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){
+        week += 1;
+      }
       eph.setCrs(subframe_2->c_rs() * pow(2, -5));
       eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
       eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
@@ -256,6 +254,9 @@ kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m
       iode_s3 = subframe_3->iode();
     }
 
+    eph.setToeWeek(week);
+    eph.setTocWeek(week);
+
     gps_subframes[msg->sv_id()].clear();
     if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) {
       // data set cutover, reject ephemeris
@@ -329,7 +330,10 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
   MessageBuilder msg_builder;
   auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
   eph.setSvId(msg->sv_id());
+  eph.setFreqNum(msg->freq_id() - 7);
+
   uint16_t current_day = 0;
+  uint16_t tk = 0;
 
   // string number 1
   {
@@ -338,7 +342,8 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
     glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
 
     eph.setP1(data->p1());
-    eph.setTk(data->t_k());
+    tk = data->t_k();
+    eph.setTkDEPRECATED(tk);
     eph.setXVel(data->x_vel() * pow(2, -20));
     eph.setXAccel(data->x_accel() * pow(2, -30));
     eph.setX(data->x() * pow(2, -11));
@@ -379,6 +384,7 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
     glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
 
     current_day = data->n_t();
+    eph.setNt(current_day);
     eph.setTauN(data->tau_n() * pow(2, -30));
     eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30));
     eph.setAge(data->e_n());
@@ -398,27 +404,9 @@ kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_
 
     // string5 parsing is only needed to get the year, this can be removed and
     // the year can be fetched later in laika (note rollovers and leap year)
-    uint8_t n_4 = data->n_4();
-    uint16_t year = get_glonass_year(n_4, current_day);
-    if (current_day > 1461) {
-      // impossible day within last 4 year, reject ephemeris
-      // TODO: check if this can be detected via hamming code
-      LOGE("INVALID DATA: current day out of range: %d, %d", current_day, n_4);
-      glonass_strings[msg->sv_id()].clear();
-      return kj::Array<capnp::word>();
-    }
-
-    uint16_t last_leap_year = 1996 + 4*(n_4-1);
-    uint16_t days_till_this_year = (year - last_leap_year)*365;
-    if (days_till_this_year != 0) {
-      days_till_this_year++;
-    }
-
-    eph.setYear(year);
-    eph.setDayInYear(current_day - days_till_this_year);
-    eph.setHour((eph.getTk()>>7) & 0x1F);
-    eph.setMinute((eph.getTk()>>1) & 0x3F);
-    eph.setSecond((eph.getTk() & 0x1) * 30);
+    eph.setN4(data->n_4());
+    int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30;
+    eph.setTkSeconds(tk_seconds);
   }
 
   glonass_strings[msg->freq_id()].clear();

system/ubloxd/ublox_msg.h

@@ -15,6 +15,11 @@
 
 using namespace std::string_literals;
 
+const int SECS_IN_MIN = 60;
+const int SECS_IN_HR = 60 * SECS_IN_MIN;
+const int SECS_IN_DAY = 24 * SECS_IN_HR;
+const int SECS_IN_WEEK = 7 * SECS_IN_DAY;
+
 // protocol constants
 namespace ublox {
   const uint8_t PREAMBLE1 = 0xb5;
@@ -102,7 +107,6 @@ class UbloxMsgParser {
     inline bool valid_cheksum();
     inline bool valid();
     inline bool valid_so_far();
-    inline uint16_t get_glonass_year(uint8_t N4, uint16_t Nt);
 
     kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
     kj::Array<capnp::word> parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);