diff --git a/cereal b/cereal index 9888e0476c..c579889f39 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 9888e0476c05069d46f273569467e4371b2d8690 +Subproject commit c579889f396cd754048c7b1a51c6f33b1988762a diff --git a/laika_repo b/laika_repo index b740b71c82..b896cdbbd1 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit b740b71c82a748e3520b1599487d9a7aaf728670 +Subproject commit b896cdbbd1e8f85df25a1afa0c9a2ec150b72f92 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 2b18dcf152..53fbf36066 100755 --- a/selfdrive/locationd/laikad.py +++ b/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 diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 10ac7790b6..be79ff16bc 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/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() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a8896884e1..dae7ceab1a 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -3c5ebb007f76ba0de710ff7a8cf5910ad2edf22f \ No newline at end of file +b79fa775682401c25aa9ce38b5542b4206d0c56b diff --git a/system/ubloxd/ublox_msg.cc b/system/ubloxd/ublox_msg.cc index 83e64b7ddc..811492ba9b 100644 --- a/system/ubloxd/ublox_msg.cc +++ b/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 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 UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m gps_t subframe(&stream); gps_t::subframe_1_t* subframe_1 = static_cast(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 UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m gps_t subframe(&stream); gps_t::subframe_2_t* subframe_2 = static_cast(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 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 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 UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ glonass_t::string_1_t* data = static_cast(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 UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ glonass_t::string_4_t* data = static_cast(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 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(); - } - - 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(); diff --git a/system/ubloxd/ublox_msg.h b/system/ubloxd/ublox_msg.h index 06877cc50d..a6e0623aa0 100644 --- a/system/ubloxd/ublox_msg.h +++ b/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 parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);