From da6eb7bdd7e7536574d17aa003c0e912b638b142 Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Fri, 16 Dec 2022 23:18:05 -0800 Subject: [PATCH] add proper qcom replay support --- selfdrive/locationd/laikad.py | 25 +++++++++------ .../test/process_replay/process_replay.py | 32 +++++++++---------- 2 files changed, 30 insertions(+), 27 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index f71383b480..ac4a0b9676 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -171,15 +171,17 @@ class Laikad: return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): + msg = messaging.new_message("gnssMeasurements") + msg.valid = False if self.is_ephemeris(gnss_msg): self.read_ephemeris(gnss_msg) - return None + return msg elif self.is_good_report(gnss_msg): week, tow, new_meas = self.read_report(gnss_msg) if len(new_meas) == 0: - return None + return msg self.gps_week = week t = gnss_mono_time * 1e-9 @@ -191,17 +193,16 @@ class Laikad: output = self.process_report(new_meas, t) if output is None: - return None + return msg position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output self.update_localizer(position_estimate, t, corrected_measurements) meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] - dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message P_diag = self.gnss_kf.P.diagonal() kf_valid = all(self.kf_valid(t)) - dat.gnssMeasurements = { + msg.gnssMeasurements = { "gpsWeek": week, "gpsTimeOfWeek": tow, "kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(), @@ -216,7 +217,8 @@ class Laikad: "measTime": gnss_mono_time, "correctedMeasurements": meas_msgs } - return dat + msg.valid = True + return msg #elif gnss_msg.which() == 'ionoData': # TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them. @@ -386,9 +388,9 @@ def process_msg(laikad, gnss_msg, mono_time, block=False): return laikad.process_gnss_msg(gnss_msg, mono_time, block=block) -def main(sm=None, pm=None): +def main(sm=None, pm=None, qc=None): use_qcom = not Params().get_bool("UbloxAvailable", block=True) - if use_qcom: + if use_qcom or (qc is not None and qc): raw_gnss_socket = "qcomGnss" else: raw_gnss_socket = "ubloxGnss" @@ -409,8 +411,11 @@ def main(sm=None, pm=None): gnss_msg = sm[raw_gnss_socket] msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay) - if msg is not None: - pm.send('gnssMeasurements', msg) + if msg is None: + msg = messaging.new_message("gnssMeasurements") + msg.valid = False + + pm.send('gnssMeasurements', msg) if not laikad.got_first_gnss_msg and sm.updated['clocks']: clocks_msg = sm['clocks'] diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 578101593d..825f69e745 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -25,18 +25,14 @@ from selfdrive.manager.process_config import managed_processes NUMPY_TOLERANCE = 1e-7 CI = "CI" in os.environ TIMEOUT = 15 -# laikad may not return on a gnss message, shorter timeout -TIMEOUT_LAIKAD_RESPONSE = 1 PROC_REPLAY_DIR = os.path.dirname(os.path.abspath(__file__)) FAKEDATA = os.path.join(PROC_REPLAY_DIR, "fakedata/") -ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster', 'submaster_config', 'environ', 'subtest_name', "field_tolerances", "allow_no_response"], defaults=({}, {}, "", {}, False)) +ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster', 'submaster_config', 'environ', 'subtest_name', "field_tolerances"], defaults=({}, {}, "", {})) -def wait_for_event(evt, timeout=TIMEOUT, allow_timeout=False): - if not evt.wait(timeout): - if allow_timeout: - return +def wait_for_event(evt): + if not evt.wait(TIMEOUT): if threading.currentThread().getName() == "MainThread": # tested process likely died. don't let test just hang raise Exception(f"Timeout reached. Tested process {os.environ['PROC_NAME']} likely crashed.") @@ -150,10 +146,10 @@ class FakePubMaster(messaging.PubMaster): wait_for_event(self.get_called) self.get_called.clear() - def wait_for_msg(self, timeout=TIMEOUT, allow_timeout=False): - wait_for_event(self.send_called, timeout, allow_timeout) + def wait_for_msg(self): + wait_for_event(self.send_called) self.send_called.clear() - dat = None if self.last_updated is None else self.data[self.last_updated] + dat = self.data[self.last_updated] self.get_called.set() return dat @@ -255,8 +251,10 @@ def ublox_rcv_callback(msg): def laika_rcv_callback(msg, CP, cfg, fsm): if msg.which() == 'ubloxGnss' and msg.ubloxGnss.which() == "measurementReport": return ["gnssMeasurements"], True + elif msg.which() == 'qcomGnss' and msg.qcomGnss.which() == "drMeasurementReport": + return ["gnssMeasurements"], True else: - return [], True + return [], False CONFIGS = [ @@ -368,6 +366,7 @@ CONFIGS = [ proc_name="laikad", pub_sub={ "ubloxGnss": ["gnssMeasurements"], + "qcomGnss": ["gnssMeasurements"], "clocks": [] }, ignore=["logMonoTime"], @@ -375,7 +374,6 @@ CONFIGS = [ should_recv_callback=laika_rcv_callback, tolerance=NUMPY_TOLERANCE, fake_pubsubmaster=True, - allow_no_response=True, ), ProcessConfig( proc_name="torqued", @@ -465,6 +463,10 @@ def python_replay_process(cfg, lr, fingerprint=None): all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] + # laikad needs decision between submaster ubloxGnss and qcomGnss + if cfg.proc_name == "laikad": + args = (*args, not any(m.which() == "ubloxGnss" for m in pub_msgs)) + controlsState = None initialized = False for msg in lr: @@ -526,11 +528,7 @@ def python_replay_process(cfg, lr, fingerprint=None): recv_cnt = len(recv_socks) while recv_cnt > 0: - timeout = TIMEOUT_LAIKAD_RESPONSE if cfg.allow_no_response else TIMEOUT - m = fpm.wait_for_msg(timeout, cfg.allow_no_response) - if m is None and cfg.allow_no_response: - break - m = m.as_builder() + m = fpm.wait_for_msg().as_builder() m.logMonoTime = msg.logMonoTime m = m.as_reader()