diff --git a/common/params.cc b/common/params.cc index 11b24fe9c1..f89e7ff002 100644 --- a/common/params.cc +++ b/common/params.cc @@ -151,6 +151,7 @@ std::unordered_map keys = { {"RecordFront", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet {"ReleaseNotes", PERSISTENT}, + {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"ShouldDoUpdate", CLEAR_ON_MANAGER_START}, {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, {"SshEnabled", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3b2307f04a..80d9bccb05 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -203,6 +203,16 @@ class Controls: self.rk = Ratekeeper(100, print_delay_threshold=None) self.prof = Profiler(False) # off by default + def set_initial_state(self): + if REPLAY: + controls_state = Params().get("ReplayControlsState") + if controls_state is not None: + controls_state = log.ControlsState.from_bytes(controls_state) + self.v_cruise_kph = controls_state.vCruise + + if self.sm['pandaStates'][0].controlsAllowed: + self.state = State.enabled + def update_events(self, CS): """Compute carEvents from carState""" @@ -417,11 +427,9 @@ class Controls: if all_valid or timed_out or SIMULATION: if not self.read_only: self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) - self.initialized = True - - if REPLAY and self.sm['pandaStates'][0].controlsAllowed: - self.state = State.enabled + self.initialized = True + self.set_initial_state() Params().put_bool("ControlsReady", True) # Check for CAN timeout diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 32a3c2f3bb..8b9c77625f 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -385,7 +385,7 @@ def replay_process(cfg, lr, fingerprint=None): return cpp_replay_process(cfg, lr, fingerprint) -def setup_env(simulation=False, CP=None, cfg=None): +def setup_env(simulation=False, CP=None, cfg=None, controlsState=None): params = Params() params.clear_all() params.put_bool("OpenpilotEnabledToggle", True) @@ -414,6 +414,12 @@ def setup_env(simulation=False, CP=None, cfg=None): elif "SIMULATION" in os.environ: del os.environ["SIMULATION"] + # Initialize controlsd with a controlsState packet + if controlsState is not None: + params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) + else: + params.delete("ReplayControlsState") + # Regen or python process if CP is not None: if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: @@ -440,13 +446,25 @@ 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())] + controlsState = None + initialized = False + for msg in lr: + if msg.which() == 'controlsState': + controlsState = msg.controlsState + if initialized: + break + elif msg.which() == 'carEvents': + initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.carEvents] + + assert controlsState is not None and initialized, "controlsState never initialized" + if fingerprint is not None: os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = fingerprint - setup_env(cfg=cfg) + setup_env(cfg=cfg, controlsState=controlsState) else: CP = [m for m in lr if m.which() == 'carParams'][0].carParams - setup_env(CP=CP, cfg=cfg) + setup_env(CP=CP, cfg=cfg, controlsState=controlsState) assert(type(managed_processes[cfg.proc_name]) is PythonProcess) managed_processes[cfg.proc_name].prepare() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 8aa80d6b79..400d27f1a7 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -118d78e2040103c00b4bfcc875fcdcd6a15e2211 +375f581030fe8efeb9dacd63875b3f046d3b420f \ No newline at end of file diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 16d7ea8519..4e1cbfd30d 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -203,9 +203,10 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA, disable_tqdm=False): # Get and setup initial state CP = [m for m in lr if m.which() == 'carParams'][0].carParams + controlsState = [m for m in lr if m.which() == 'controlsState'][0].controlsState liveCalibration = [m for m in lr if m.which() == 'liveCalibration'][0] - setup_env(CP=CP) + setup_env(CP=CP, controlsState=controlsState) params.put("CalibrationParams", liveCalibration.as_builder().to_bytes()) vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 608d0c4af3..2a005cf4cc 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -49,7 +49,7 @@ segments = [ ("CHRYSLER", "regen38346FB33D0|2022-07-14--18-05-26--0"), ("RAM", "2f4452b03ccb98f0|2022-07-07--08-01-56--3"), ("SUBARU", "regen54A1E2BE5AA|2022-07-14--18-07-50--0"), - ("GM", "regen01D09D915B5|2022-07-06--14-36-20--0"), + ("GM", "regen76027B408B7|2022-08-16--19-56-58--0"), ("NISSAN", "regenCA0B0DC946E|2022-07-14--18-10-17--0"), ("VOLKSWAGEN", "regen007098CA0EF|2022-07-06--15-01-26--0"), ("MAZDA", "regen61BA413D53B|2022-07-06--14-39-42--0"), @@ -248,12 +248,8 @@ if __name__ == "__main__": print("TEST SUCCEEDED") else: - if failed: - print(diff1) - print("FAILED TO UPDATE REFS") - else: - with open(REF_COMMIT_FN, "w") as f: - f.write(cur_commit) - print(f"\n\nUpdated reference logs for commit: {cur_commit}") + with open(REF_COMMIT_FN, "w") as f: + f.write(cur_commit) + print(f"\n\nUpdated reference logs for commit: {cur_commit}") sys.exit(int(failed))