diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index c77e40daaf..518909cb54 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -24,12 +24,12 @@ def create_button_event(cur_but: int, prev_but: int, buttons_dict: Dict[int, cap return be -def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilder, pcm_cruise: bool = False) -> List[int]: +def create_button_enable_events(buttonEvents: capnp.lib.capnp._DynamicListBuilder, pcm_cruise: bool = False, resume_enabled: bool = True) -> List[int]: events = [] for b in buttonEvents: # do enable on both accel and decel buttons if not pcm_cruise: - if b.type in (ButtonType.accelCruise, ButtonType.decelCruise) and not b.pressed: + if (b.type == ButtonType.decelCruise or (b.type == ButtonType.accelCruise and resume_enabled)) and not b.pressed: events.append(EventName.buttonEnable) # do disable on button down if b.type == ButtonType.cancel and b.pressed: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 10ae670ac8..3581e0d665 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -161,9 +161,13 @@ class CarInterface(CarInterfaceBase): if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: be = create_button_event(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, CruiseButtons.UNPRESS) - # Suppress resume button if we're resuming from stop so we don't adjust speed. - if be.type == ButtonType.accelCruise and (ret.cruiseState.enabled and ret.standstill): - be.type = ButtonType.unknown + if be.type == ButtonType.accelCruise: + # Suppress resume button if we're resuming from stop so we don't adjust speed. + if ret.cruiseState.enabled and ret.standstill: + be.type = ButtonType.unknown + # The ECM will fault if resume triggers an enable while speed is unset (unset is greater than 70 m/s) + elif c.hudControl.setSpeed < 70: + be.type = ButtonType.unknown ret.buttonEvents = [be] @@ -179,7 +183,9 @@ class CarInterface(CarInterfaceBase): events.add(car.CarEvent.EventName.belowSteerSpeed) # handle button presses - events.events.extend(create_button_enable_events(ret.buttonEvents, pcm_cruise=self.CP.pcmCruise)) + # The ECM will fault if resume triggers an enable while speed is unset (unset is greater than 70 m/s) + resume_enabled = c.hudControl.setSpeed < 70 + events.events.extend(create_button_enable_events(ret.buttonEvents, pcm_cruise=self.CP.pcmCruise, resume_enabled=resume_enabled)) ret.events = events.to_msg() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2f7c85aa7d..3cb0d9d44a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -16,7 +16,7 @@ from system.version import get_short_branch from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET -from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise +from selfdrive.controls.lib.drive_helpers import V_CRUISE_INITIAL, update_v_cruise, initialize_v_cruise from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.longcontrol import LongControl @@ -162,8 +162,8 @@ class Controls: self.active = False self.can_rcv_error = False self.soft_disable_timer = 0 - self.v_cruise_kph = 255 - self.v_cruise_cluster_kph = 255 + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL self.v_cruise_kph_last = 0 self.mismatch_counter = 0 self.cruise_mismatch_counter = 0 @@ -454,9 +454,13 @@ class Controls: # if stock cruise is completely disabled, then we can use our own set speed logic if not self.CP.pcmCruise: - self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents, - self.button_timers, self.enabled, self.is_metric) - self.v_cruise_cluster_kph = self.v_cruise_kph + if CS.cruiseState.available: + self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents, + self.button_timers, self.enabled, self.is_metric) + self.v_cruise_cluster_kph = self.v_cruise_kph + else: + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL else: if CS.cruiseState.available: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index d79f94bbfd..ffa8373834 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -11,6 +11,7 @@ from selfdrive.modeld.constants import T_IDXS V_CRUISE_MAX = 145 # kph V_CRUISE_MIN = 8 # kph V_CRUISE_ENABLE_MIN = 40 # kph +V_CRUISE_INITIAL = 255 # kph LAT_MPC_N = 16 LON_MPC_N = 32