diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 8ca6668b30..7b221595c9 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) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) # 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 f24d819d2a..6b1ab8432e 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -116,7 +116,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) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) 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 7787fabb02..465109eb4c 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) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button)) # 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 d099009904..002284fe75 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -246,7 +246,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) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_buttons[-1] == 3)) # MADS BUTTON if self.CS.out.madsEnabled != self.CS.madsEnabled: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 036e7ba2d5..fc9715cb82 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): + def get_sp_common_state(self, cs_out, CS, min_enable_speed_pcm=False, gear_allowed=True, gap_button=False): 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,6 +603,9 @@ 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 \ @@ -628,6 +631,19 @@ 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 2d6b68f9e1..8d49218b02 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) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) # 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 4416299230..878d8f14c0 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) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(self.CS.distance_button)) # 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 51d76dece0..38f4753c47 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) + ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=bool(distance_button)) # 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 7763c36f33..00c9e37858 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -761,12 +761,9 @@ class Controls: # decrement personality on distance button press if self.CP.openpilotLongitudinalControl: - if self.v_cruise_helper.update_personality: + if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): 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 0c80d186e5..61edf7aade 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, ButtonType.gapAdjustCruise: 0} + self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} self.is_metric_prev = None @@ -84,9 +84,6 @@ 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 @@ -123,8 +120,6 @@ 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 @@ -145,15 +140,6 @@ 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": @@ -168,9 +154,6 @@ 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