diff --git a/common/params.cc b/common/params.cc index 11b24fe9c1..b95a28158d 100644 --- a/common/params.cc +++ b/common/params.cc @@ -93,6 +93,7 @@ std::unordered_map keys = { {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CompletedTrainingVersion", PERSISTENT}, {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, + {"ControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"DashcamOverride", PERSISTENT}, {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3b2307f04a..0889fbf249 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -203,6 +203,18 @@ class Controls: self.rk = Ratekeeper(100, print_delay_threshold=None) self.prof = Profiler(False) # off by default + def set_initial_state(self): + if REPLAY: + print('SETTING INITIAL STATE') + controls_state = Params().get("ControlsState") + if controls_state is not None: + controls_state = log.ControlsState.from_bytes(controls_state) + self.v_cruise_kph = controls_state.vCruise + print("V_CRUISE:", self.v_cruise_kph) + + if self.sm['pandaStates'][0].controlsAllowed: + self.state = State.enabled + def update_events(self, CS): """Compute carEvents from carState""" @@ -415,13 +427,12 @@ class Controls: all_valid = CS.canValid and self.sm.all_checks() timed_out = self.sm.frame * DT_CTRL > (6. if REPLAY else 3.5) if all_valid or timed_out or SIMULATION: + self.set_initial_state() + 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 - 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..131b5ddbb9 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("ControlsState", controlsState.as_builder().to_bytes()) + else: + params.delete("ControlsState") + # Regen or python process if CP is not None: if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: 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..dd8e6ead94 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -16,21 +16,21 @@ from tools.lib.filereader import FileReader from tools.lib.logreader import LogReader original_segments = [ - ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.BODY - ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA - ("HYUNDAI", "d824e27e8c60172c|2022-07-08--21-21-15--0"), # HYUNDAI.KIA_EV6 - ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) - ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) - ("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.COROLLA_TSS2 - ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.CIVIC (NIDEC) - ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.ACCORD (BOSCH) - ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.PACIFICA - ("RAM", "2f4452b03ccb98f0|2022-07-07--08-01-56--3"), # CHRYSLER.RAM_1500 - ("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"), # SUBARU.IMPREZA + # ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.BODY + # ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA + # ("HYUNDAI", "d824e27e8c60172c|2022-07-08--21-21-15--0"), # HYUNDAI.KIA_EV6 + # ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) + # ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) + # ("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.COROLLA_TSS2 + # ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.CIVIC (NIDEC) + # ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.ACCORD (BOSCH) + # ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.PACIFICA + # ("RAM", "2f4452b03ccb98f0|2022-07-07--08-01-56--3"), # CHRYSLER.RAM_1500 + # ("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"), # SUBARU.IMPREZA ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.VOLT - ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.XTRAIL - ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF - ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"), # MAZDA.CX9_2021 + # ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.XTRAIL + # ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF + # ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"), # MAZDA.CX9_2021 # Enable when port is tested and dashcamOnly is no longer set #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.AP2_MODELS