From 5ed587ebeff24bacf70c44879616c51756730bba Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 17 Aug 2022 18:03:33 -0700 Subject: [PATCH] controls: block resume if cruise not previously engaged (#25402) * see if this works at all * can revert this * think adding a no entry conditionally is nicer * then we can revert this * 0 makes more sense * Revert "0 makes more sense" This reverts commit efc89e8a2389ef58fbc0cec0a2872d24db524867. * gm CC uses > 70 * bump cereal * comment * test on Honda * whoops * works * add exception with todo * moved button enable events to controlsd * get rid of that get rid of that * different values for now * car interfaces add enable event, controlsd can block it * Regen and update refs * delete if not set * One place one place * regen routes are uninitialized first few frames * Trim start of segment so it's like original segment * stash * regen * not working * clean up * more cleanup * revert * bump ceral * actually check resume button * whoops * pcmCruise cars don't use setSpeed, so we're good * engage correctly in sim * Update ref_commit * Update refs --- cereal | 2 +- selfdrive/controls/controlsd.py | 32 ++++++++++++++--------- selfdrive/controls/lib/drive_helpers.py | 1 + selfdrive/controls/lib/events.py | 4 +++ selfdrive/test/process_replay/ref_commit | 2 +- tools/sim/tests/test_carla_integration.py | 2 +- 6 files changed, 27 insertions(+), 16 deletions(-) diff --git a/cereal b/cereal index 2d648b0dc4..f60f0ef200 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 2d648b0dc4654fc7088ffb81fd301352707de749 +Subproject commit f60f0ef200afa9c7da932454b592ad5b0fb58493 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 80d9bccb05..1f394d487a 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 @@ -50,6 +50,7 @@ LaneChangeState = log.LateralPlan.LaneChangeState LaneChangeDirection = log.LateralPlan.LaneChangeDirection EventName = car.CarEvent.EventName ButtonEvent = car.CarState.ButtonEvent +ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) @@ -162,8 +163,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 @@ -228,6 +229,11 @@ class Controls: self.events.add(EventName.controlsInitializing) 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 self.v_cruise_kph == V_CRUISE_INITIAL 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)): @@ -460,18 +466,18 @@ class Controls: self.v_cruise_kph_last = self.v_cruise_kph - # 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 - else: - if CS.cruiseState.available: + if CS.cruiseState.available: + # 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 + else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH - else: - self.v_cruise_kph = 0 - self.v_cruise_cluster_kph = 0 + else: + self.v_cruise_kph = V_CRUISE_INITIAL + self.v_cruise_cluster_kph = V_CRUISE_INITIAL # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state 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 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index de666fd507..5139ead84b 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -635,6 +635,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: wrong_car_mode_alert, }, + EventName.resumeBlocked: { + ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"), + }, + EventName.wrongCruiseMode: { ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"), diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 57d80369f1..da083eeee1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -29e406826b1d7b0cc7e05153b623fbedcd8fd9e9 \ No newline at end of file +35899d5137e298220e91063f3078109227cc8715 \ No newline at end of file diff --git a/tools/sim/tests/test_carla_integration.py b/tools/sim/tests/test_carla_integration.py index 43db783f4e..f467391967 100755 --- a/tools/sim/tests/test_carla_integration.py +++ b/tools/sim/tests/test_carla_integration.py @@ -77,7 +77,7 @@ class TestCarlaIntegration(unittest.TestCase): while time.monotonic() < start_time + max_time_per_step: sm.update() - q.put("cruise_up") # Try engaging + q.put("cruise_down") # Try engaging if sm.all_alive() and sm['controlsState'].active: control_active += 1