diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index d83cdb41ba..6308667fae 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -4,7 +4,7 @@ import time import cereal.messaging as messaging -from cereal import car +from cereal import car, log from panda import ALTERNATIVE_EXPERIENCE @@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.events import Events REPLAY = "REPLAY" in os.environ +State = log.ControlsState.OpenpilotState EventName = car.CarEvent.EventName ButtonType = car.CarState.ButtonEvent.Type @@ -57,6 +58,9 @@ class Car: else: self.CI, self.CP = CI, CI.CP + # read params + self.is_metric = self.params.get_bool("IsMetric") + # set alternative experiences from parameters self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.CP.alternativeExperience = 0 @@ -115,6 +119,10 @@ class Car: if can_rcv_valid and REPLAY: self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + if self.sm['controlsState'].initialized and not self.controlsState_prev.initialized: + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + cloudlog.timestamp("Initialized") + return CS def update_events(self, CS: car.CarState) -> car.CarState: @@ -135,6 +143,15 @@ class Car: CS.events = self.events.to_msg() + def state_transition(self, CS: car.CarState): + self.v_cruise_helper.update_v_cruise(CS, self.sm['controlsState'].enabled, self.is_metric) + + controlsState = self.sm['controlsState'] + if self.controlsState_prev.state == State.disabled: + # TODO: use ENABLED_STATES from controlsd? it includes softDisabling which isn't possible here + if controlsState.state in (State.preEnabled, State.overriding, State.enabled): + self.v_cruise_helper.initialize_v_cruise(CS, controlsState.experimentalMode) + def state_publish(self, CS: car.CarState): """carState and carParams publish loop""" @@ -184,14 +201,13 @@ class Car: self.update_events(CS) + if not self.CP.passive and self.sm['controlsState'].initialized: + self.state_transition(CS) + self.state_publish(CS) cloudlog.timestamp("State published") controlsState = self.sm['controlsState'] - if controlsState.initialized and not self.controlsState_prev.initialized: - self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) - cloudlog.timestamp("Initialized") - if not self.CP.passive and controlsState.initialized: self.controls_update(CS, self.sm['carControl']) cloudlog.timestamp("Controls updated") diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index da83f810b4..4895bf12d8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -96,9 +96,8 @@ class Controls: poll='carState', ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ]) - self.joystick_mode = self.params.get_bool("JoystickDebugMode") - # read params + self.joystick_mode = self.params.get_bool("JoystickDebugMode") self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") @@ -168,10 +167,11 @@ class Controls: def set_initial_state(self): if REPLAY: - controls_state = self.params.get("ReplayControlsState") - if controls_state is not None: - with log.ControlsState.from_bytes(controls_state) as controls_state: - self.card.v_cruise_helper.v_cruise_kph = controls_state.vCruise + # TODO: move this to card + # controls_state = self.params.get("ReplayControlsState") + # if controls_state is not None: + # with log.ControlsState.from_bytes(controls_state) as controls_state: + # self.card.v_cruise_helper.v_cruise_kph = controls_state.vCruise if any(ps.controlsAllowed for ps in self.sm['pandaStates']): self.state = State.enabled @@ -427,8 +427,6 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.card.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric) - # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state self.soft_disable_timer = max(0, self.soft_disable_timer - 1) @@ -502,7 +500,6 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.card.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES