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