diff --git a/laika_repo b/laika_repo index 400c71d93..3b3d41a88 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 400c71d9313260b137783ecf1fac37ffa18d7f60 +Subproject commit 3b3d41a885d16f4c3739a14ef06bebca858b6e3a diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 3b5609bf6..2e98fd755 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -98,7 +98,6 @@ class Laikad: self.velfix_function = get_velfix_sympy_func() self.last_fix_pos = None self.last_fix_t = None - self.gps_week = None self.use_qcom = use_qcom self.first_log_time = None self.ttff = -1 @@ -245,11 +244,8 @@ class Laikad: def read_ephemeris(self, gnss_msg): if self.use_qcom: - # TODO this is not robust to gps week rollover - if self.gps_week is None: - return try: - ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week) + ephem = parse_qcom_ephem(gnss_msg.drSvPoly) self.astro_dog.add_qcom_polys({ephem.prn: [ephem]}) except Exception: cloudlog.exception("Error parsing qcom svPoly ephemeris from qcom module") @@ -310,7 +306,6 @@ class Laikad: self.read_ephemeris(gnss_msg) elif self.is_good_report(gnss_msg): report_t, new_meas = self.read_report(gnss_msg) - self.gps_week = report_t.week if report_t.week > 0: if self.auto_fetch_navs: self.fetch_navs(report_t, block) diff --git a/selfdrive/locationd/test/test_laikad.py b/selfdrive/locationd/test/test_laikad.py index 79be20a08..ce00bde94 100755 --- a/selfdrive/locationd/test/test_laikad.py +++ b/selfdrive/locationd/test/test_laikad.py @@ -20,12 +20,12 @@ from tools.lib.logreader import LogReader from selfdrive.test.process_replay.process_replay import get_process_config, replay_process GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC = GPSTime.from_datetime(datetime(2022, month=1, day=29, hour=12)) +UBLOX_TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" +QCOM_TEST_ROUTE = "616dc83ca1f7f11e|2023-07-11--10-52-31" -def get_log_ublox(segs=range(0)): - logs = [] - for i in segs: - logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i))) +def get_log_ublox(): + logs = LogReader(get_url(UBLOX_TEST_ROUTE, 0)) ublox_cfg = get_process_config("ubloxd") all_logs = replay_process(ublox_cfg, logs) @@ -47,10 +47,8 @@ def get_log_ublox(segs=range(0)): return all_logs, low_gnss -def get_log_qcom(segs=range(0)): - logs = [] - for i in segs: - logs.extend(LogReader(get_url("b0b3cba7abf862d1|2023-03-11--09-40-33", i))) +def get_log_qcom(): + logs = LogReader(get_url(QCOM_TEST_ROUTE, 0)) all_logs = [m for m in logs if m.which() == 'qcomGnss'] return all_logs @@ -98,10 +96,10 @@ class TestLaikad(unittest.TestCase): @classmethod def setUpClass(cls): - logs, low_gnss = get_log_ublox(range(1)) + logs, low_gnss = get_log_ublox() cls.logs = logs cls.low_gnss = low_gnss - cls.logs_qcom = get_log_qcom(range(1)) + cls.logs_qcom = get_log_qcom() first_gps_time = get_first_gps_time(logs) cls.first_gps_time = first_gps_time @@ -179,8 +177,8 @@ class TestLaikad(unittest.TestCase): laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom) # Disable fetch_orbits to test NAV only correct_msgs = verify_messages(logs, laikad) - correct_msgs_expected = 43 if use_qcom else 560 - valid_fix_expected = 43 if use_qcom else 560 + correct_msgs_expected = 56 if use_qcom else 560 + valid_fix_expected = 56 if use_qcom else 560 self.assertEqual(correct_msgs_expected, len(correct_msgs)) self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid])) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 59c69a321..7a59e296e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -35eb63be5d31ad443da70be29836ce695206d54d +a683d689bd74ba5ba7c10bfad237aacc04b978c3 diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py index a2b971999..78b2fb6f5 100755 --- a/selfdrive/test/update_ci_routes.py +++ b/selfdrive/test/update_ci_routes.py @@ -6,6 +6,7 @@ from tqdm import tqdm from azure.storage.blob import BlockBlobService # pylint: disable=import-error from selfdrive.car.tests.routes import routes as test_car_models_routes +from selfdrive.locationd.test.test_laikad import UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE from selfdrive.test.process_replay.test_processes import source_segments as replay_segments from xx.chffr.lib import azureutil # pylint: disable=import-error from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION # pylint: disable=import-error @@ -81,6 +82,7 @@ if __name__ == "__main__": if not len(to_sync): # sync routes from the car tests routes and process replay + to_sync.extend([UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE]) to_sync.extend([rt.route for rt in test_car_models_routes]) to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments]) diff --git a/system/sensord/rawgps/rawgpsd.py b/system/sensord/rawgps/rawgpsd.py index c6d0adf08..2dd49dd02 100755 --- a/system/sensord/rawgps/rawgpsd.py +++ b/system/sensord/rawgps/rawgpsd.py @@ -14,7 +14,9 @@ from struct import unpack_from, calcsize, pack from cereal import log import cereal.messaging as messaging from common.gpio import gpio_init, gpio_set -from laika.gps_time import GPSTime +from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds +from laika.helpers import get_prn_from_nmea_id +from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK from system.hardware.tici.pins import GPIO from system.swaglog import cloudlog from system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv @@ -248,6 +250,7 @@ def main() -> NoReturn: signal.signal(signal.SIGTERM, cleanup) setup_quectel(diag) + current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow())) cloudlog.warning("quectel setup done") gpio_init(GPIO.UBLOX_PWR_EN, True) gpio_set(GPIO.UBLOX_PWR_EN, True) @@ -316,6 +319,8 @@ def main() -> NoReturn: setattr(sv.measurementStatus, kk, bool(v & (1< NoReturn: pass else: setattr(poly, k, v) + + prn = get_prn_from_nmea_id(poly.svId) + if prn[0] == 'R': + epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time)) + else: + epoch = GPSTime(current_gps_time.week, poly.t0) + + # handle week rollover + if epoch.tow < SECS_IN_DAY and current_gps_time.tow > 6*SECS_IN_DAY: + epoch.week += 1 + elif epoch.tow > 6*SECS_IN_DAY and current_gps_time.tow < SECS_IN_DAY: + epoch.week -= 1 + + poly.gpsWeek = epoch.week + poly.gpsTow = epoch.tow pm.send('qcomGnss', msg) elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: