From d959c43284eb31a68616afbf4c4e8d5844963bfc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 16 May 2024 00:12:17 -0400 Subject: [PATCH] Longitudinal: Use upstream button timer to toggle between Chill & Experimental Mode --- selfdrive/car/chrysler/interface.py | 2 +- selfdrive/car/ford/interface.py | 2 +- selfdrive/car/gm/interface.py | 2 +- selfdrive/car/hyundai/interface.py | 2 +- selfdrive/car/interfaces.py | 18 +----------------- selfdrive/car/mazda/interface.py | 2 +- selfdrive/car/nissan/interface.py | 2 +- selfdrive/car/toyota/interface.py | 2 +- selfdrive/controls/controlsd.py | 5 ++++- selfdrive/controls/lib/drive_helpers.py | 19 ++++++++++++++++++- 10 files changed, 30 insertions(+), 26 deletions(-) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 7b221595c9..8ca6668b30 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -141,7 +141,7 @@ class CarInterface(CarInterfaceBase): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index ed35d1ba43..5d6fa7dde4 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -104,7 +104,7 @@ class CarInterface(CarInterfaceBase): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) if self.CS.out.madsEnabled != self.CS.madsEnabled: if self.mads_event_lock: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 465109eb4c..7787fabb02 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -251,7 +251,7 @@ class CarInterface(CarInterfaceBase): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c5a83fcbed..d06cdbd209 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -239,7 +239,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) ret.cruiseState.enabled = False if self.CP.pcmCruise else self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index fc9715cb82..036e7ba2d5 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -593,7 +593,7 @@ class CarInterfaceBase(ABC): else: return CS.madsEnabled - def get_sp_common_state(self, cs_out, CS, min_enable_speed_pcm=False, gear_allowed=True, gap_button=False): + def get_sp_common_state(self, cs_out, CS, min_enable_speed_pcm=False, gear_allowed=True): cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed or min_enable_speed_pcm else \ cs_out.cruiseState.enabled @@ -603,9 +603,6 @@ class CarInterfaceBase(ABC): elif not cs_out.cruiseState.enabled and CS.out.cruiseState.enabled: CS.madsEnabled = False - if self.CP.openpilotLongitudinalControl: - self.toggle_exp_mode(gap_button) - cs_out.belowLaneChangeSpeed = cs_out.vEgo < LANE_CHANGE_SPEED_MIN and self.below_speed_pause if cs_out.gearShifter in [GearShifter.park, GearShifter.reverse] or cs_out.doorOpen or \ @@ -631,19 +628,6 @@ class CarInterfaceBase(ABC): return cs_out, CS - # TODO: SP: use upstream's buttonEvents counter checks from controlsd - def toggle_exp_mode(self, gap_pressed): - if gap_pressed: - if not self.experimental_mode_hold: - self.gap_button_counter += 1 - if self.gap_button_counter > 50: - self.gap_button_counter = 0 - self.experimental_mode_hold = True - self.param_s.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) - else: - self.gap_button_counter = 0 - self.experimental_mode_hold = False - def create_sp_events(self, CS, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False, enable_from_brake=False, enable_pressed_long=False, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 8d49218b02..2d6b68f9e1 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -77,7 +77,7 @@ class CarInterface(CarInterfaceBase): self.CS.accEnabled = False self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 878d8f14c0..4416299230 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -51,7 +51,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) ret.cruiseState.enabled = False if self.CP.pcmCruise else self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) # CANCEL if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 38f4753c47..51d76dece0 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -224,7 +224,7 @@ class CarInterface(CarInterfaceBase): if not self.CP.pcmCruise: ret.cruiseState.enabled = self.CS.accEnabled - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) # CANCEL if self.CS.out.cruiseState.enabled and not ret.cruiseState.enabled: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 32407f97a0..ff927f563c 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -760,9 +760,12 @@ class Controls: # decrement personality on distance button press if self.CP.openpilotLongitudinalControl: - if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): + if self.v_cruise_helper.update_personality: self.personality = (self.personality - 1) % 3 self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) + if self.v_cruise_helper.update_experimental_mode: + self.experimental_mode = not self.experimental_mode + self.params.put_bool_nonblocking('ExperimentalMode', self.experimental_mode) return CC, lac_log diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 61edf7aade..0c80d186e5 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -75,7 +75,7 @@ class VCruiseHelper: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET self.v_cruise_kph_last = 0 - self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} + self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} self.is_metric_prev = None @@ -84,6 +84,9 @@ class VCruiseHelper: self.slc_state_prev = SpeedLimitControlState.inactive self.slc_speed_limit_offsetted = 0 + self.update_personality = False + self.update_experimental_mode = False + @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET @@ -120,6 +123,8 @@ class VCruiseHelper: long_press = False button_type = None + update_personality = False + update_experimental_mode = False v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT v_cruise_delta_mltplr = 10 if is_metric else 5 @@ -140,6 +145,15 @@ class VCruiseHelper: if button_type is None: return + if button_type == ButtonType.gapAdjustCruise and self.button_timers[ButtonType.gapAdjustCruise]: + if self.button_timers[ButtonType.gapAdjustCruise] < 50: + update_personality = True + elif self.button_timers[ButtonType.gapAdjustCruise] == 50: + update_experimental_mode = True + + self.update_personality = update_personality + self.update_experimental_mode = update_experimental_mode + resume_button = ButtonType.accelCruise if not self.CP.pcmCruiseSpeed: if self.CP.carName == "chrysler": @@ -154,6 +168,9 @@ class VCruiseHelper: if not self.button_change_states[button_type]["enabled"]: return + if button_type == ButtonType.gapAdjustCruise: + return + pressed_value = (1 if long_press else v_cruise_delta_mltplr) if reverse_acc else (v_cruise_delta_mltplr if long_press else 1) long_press_state = not long_press if reverse_acc else long_press v_cruise_delta = v_cruise_delta * pressed_value