From 407791113d10d2f9fea9b68c8313271fefa1e36e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 15 May 2024 14:32:52 -0700 Subject: [PATCH] Revert "card: move all car events (#32427)" (#32439) * Revert "card: move all car events (#32427)" This reverts commit 8f46028bd4b71c619bac654d6f2a4f8006b7508d. * keep the event here * oops * Revert "oops" This reverts commit ea99a2768fbeb7a6123dd755585157b530e7a2a1. * Revert "keep the event here" This reverts commit ec089b4e1afdf09b2e6b184de8f23584ef931601. --- selfdrive/car/card.py | 10 +--------- selfdrive/controls/controlsd.py | 22 ++++++++++++++-------- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 4dcd5ad0b4..ed916a6fe1 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -14,13 +14,11 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_one_can from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper from openpilot.selfdrive.controls.lib.events import Events REPLAY = "REPLAY" in os.environ EventName = car.CarEvent.EventName -ButtonType = car.CarState.ButtonEvent.Type class CarD: @@ -35,7 +33,6 @@ class CarD: self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count self.CC_prev = car.CarControl.new_message() - self.CS_prev = car.CarState.new_message() self.last_actuators = None @@ -79,8 +76,8 @@ class CarD: self.params.put_nonblocking("CarParamsCache", cp_bytes) self.params.put_nonblocking("CarParamsPersistent", cp_bytes) + self.CS_prev = car.CarState.new_message() self.events = Events() - self.v_cruise_helper = VCruiseHelper(self.CP) def initialize(self): """Initialize CarInterface, once controls are ready""" @@ -121,11 +118,6 @@ class CarD: self.events.add_from_msg(CS.events) - # Block resume if cruise never previously enabled - resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) - if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: - self.events.add(EventName.resumeBlocked) - # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index eba4142ca8..92c9331121 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,7 +21,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.car.car_helpers import get_startup_event from openpilot.selfdrive.car.card import CarD from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert -from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature +from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature from openpilot.selfdrive.controls.lib.events import Events, ET from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -143,6 +143,7 @@ class Controls: self.desired_curvature = 0.0 self.experimental_mode = False self.personality = self.read_personality_param() + self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False self.can_log_mono_time = 0 @@ -168,7 +169,7 @@ class Controls: 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 + self.v_cruise_helper.v_cruise_kph = controls_state.vCruise if any(ps.controlsAllowed for ps in self.sm['pandaStates']): self.state = State.enabled @@ -197,6 +198,11 @@ class Controls: if self.CP.passive: return + # Block resume if cruise never previously enabled + resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) + if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: + self.events.add(EventName.resumeBlocked) + if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) @@ -425,7 +431,7 @@ 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) + self.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 @@ -500,7 +506,7 @@ 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) + self.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 @@ -556,7 +562,7 @@ class Controls: if not self.joystick_mode: # accel PID loop - pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.card.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) + pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_helper.v_cruise_kph * CV.KPH_TO_MS) t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) @@ -659,7 +665,7 @@ class Controls: CC.cruiseControl.resume = self.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1 hudControl = CC.hudControl - hudControl.setSpeed = float(self.card.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) + hudControl.setSpeed = float(self.v_cruise_helper.v_cruise_cluster_kph * CV.KPH_TO_MS) hudControl.speedVisible = self.enabled hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead @@ -743,8 +749,8 @@ class Controls: controlsState.engageable = not self.events.contains(ET.NO_ENTRY) controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) - controlsState.vCruise = float(self.card.v_cruise_helper.v_cruise_kph) - controlsState.vCruiseCluster = float(self.card.v_cruise_helper.v_cruise_cluster_kph) + controlsState.vCruise = float(self.v_cruise_helper.v_cruise_kph) + controlsState.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f)