From 56faf20801b5cb00e35eb25ac023fd51056bcffa Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 17:33:29 -0400 Subject: [PATCH 1/8] init --- selfdrive/controls/controlsd.py | 3 ++- selfdrive/controls/lib/drive_helpers.py | 36 +++++++++++++++++++++++-- 2 files changed, 36 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 519fddaf6a..4850d36b56 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -489,7 +489,8 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, self.sm['longitudinalPlanSP']) + self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, + self.sm['longitudinalPlanSP'], self.experimental_mode) # 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 2f191b4a55..99910e1ebf 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -4,6 +4,7 @@ import numpy as np from cereal import car, log, custom from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp +from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL, DT_CTRL from openpilot.selfdrive.modeld.constants import ModelConstants @@ -75,9 +76,10 @@ 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.param_s = Params() self.is_metric_prev = None self.v_cruise_min = V_CRUISE_MIN self.slc_state = SpeedLimitControlState.inactive @@ -88,7 +90,7 @@ class VCruiseHelper: def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp): + def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp, experimental_mode): self.v_cruise_kph_last = self.v_cruise_kph self.slc_state = long_plan_sp.speedLimitControlState @@ -105,6 +107,10 @@ class VCruiseHelper: else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + + if self.CP.openpilotLongitudinalControl: + self._update_experimental_mode(CS, experimental_mode) + self.update_button_timers(CS, enabled) else: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET @@ -140,6 +146,9 @@ class VCruiseHelper: if button_type is None: return + if button_type == ButtonType.gapAdjustCruise: + return + resume_button = ButtonType.accelCruise if not self.CP.pcmCruiseSpeed: if self.CP.carName == "chrysler": @@ -236,6 +245,29 @@ class VCruiseHelper: self.v_cruise_min = VOLKSWAGEN_V_CRUISE_MIN[is_metric] self.is_metric_prev = is_metric + # toggle experimental mode on distance button hold + def _update_experimental_mode(self, CS, experimental_mode): + long_press = False + button_type = None + + for b in CS.buttonEvents: + if b.type == ButtonType.gapAdjustCruise and not b.pressed: + if self.button_timers[ButtonType.gapAdjustCruise] > CRUISE_LONG_PRESS: + return # end long press + button_type = b.type + break + else: + if self.button_timers[ButtonType.gapAdjustCruise] and self.button_timers[ButtonType.gapAdjustCruise] % CRUISE_LONG_PRESS == 0: + button_type = ButtonType.gapAdjustCruise + long_press = True + + if button_type is None: + return + + if long_press and self.button_timers[button_type] == CRUISE_LONG_PRESS: + self.experimental_mode_updated = True + self.param_s.put_bool_nonblocking("ExperimentalMode", not experimental_mode) + def clip_curvature(v_ego, prev_curvature, new_curvature): v_ego = max(MIN_SPEED, v_ego) From 224c0457d8118d87f18396dc3320017c2c445031 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 17:36:27 -0400 Subject: [PATCH 2/8] deprecated --- selfdrive/car/chrysler/interface.py | 2 +- selfdrive/car/ford/interface.py | 2 +- selfdrive/car/gm/interface.py | 5 +---- selfdrive/car/honda/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/car/volkswagen/interface.py | 3 +-- 10 files changed, 10 insertions(+), 30 deletions(-) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index ec1587e4e4..124396f0fe 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -136,7 +136,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) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index a3bbd7ecf5..7ad98f620c 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -106,7 +106,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) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 6ca796755e..99d3056983 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -204,8 +204,6 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback) - distance_button = 0 - # Don't add event if transitioning from INIT, unless it's to an actual button if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT: self.CS.button_events = [ @@ -214,7 +212,6 @@ class CarInterface(CarInterfaceBase): *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) ] - distance_button = self.CS.distance_button self.CS.button_events = [ *self.CS.button_events, @@ -252,7 +249,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) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 1459b301a2..751624fbac 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -301,7 +301,7 @@ class CarInterface(CarInterfaceBase): elif not ret.cruiseState.enabled: self.CS.accEnabled = False - ret, self.CS = self.get_sp_common_state(ret, self.CS, gap_button=(self.CS.cruise_setting == 3)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index a12bd0ed87..b8bf909605 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -248,7 +248,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else 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) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 25e8b40a72..dbddb38c0e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -618,7 +618,7 @@ class CarInterfaceBase(ABC): else: return CS.madsEnabled - def get_sp_common_state(self, cs_out, CS, gear_allowed=True, gap_button=False): + def get_sp_common_state(self, cs_out, CS, gear_allowed=True): cs_out.cruiseState.enabled = CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled if not self.enable_mads: @@ -627,9 +627,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) - lane_change_speed_min = get_min_lateral_speed(self.CS.params_list.pause_lateral_speed, self.CS.params_list.is_metric) cs_out.belowLaneChangeSpeed = cs_out.vEgo < lane_change_speed_min and self.CS.params_list.below_speed_pause @@ -657,19 +654,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.CS.params_list.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 4c26a18a57..d75cd8aff6 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -72,7 +72,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) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 661ac668a5..2b6d56c024 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -50,7 +50,7 @@ class CarInterface(CarInterfaceBase): self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled) ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else 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) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 527964cead..bb737ff5a9 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -238,7 +238,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) ret.buttonEvents = [ *self.CS.button_events, diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 0d57539a1b..b12e66245c 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -141,8 +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=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events)) + ret, self.CS = self.get_sp_common_state(ret, self.CS) ret.buttonEvents = [ *self.CS.button_events, From 0bf65192a2a77b1e04f17bd96b5872d543d17d8e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 17:49:53 -0400 Subject: [PATCH 3/8] in controlsd directly --- selfdrive/controls/controlsd.py | 9 +++++++-- selfdrive/controls/lib/drive_helpers.py | 23 +++++++---------------- 2 files changed, 14 insertions(+), 18 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4850d36b56..a59f734535 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -489,8 +489,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, - self.sm['longitudinalPlanSP'], self.experimental_mode) + self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, self.sm['longitudinalPlanSP']) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -724,6 +723,12 @@ class Controls: self.personality = (self.personality - 1) % 3 self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) + # toggle experimental mode on distance button hold + if self.CP.openpilotLongitudinalControl: + if self.v_cruise_helper.experimental_mode_update and self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == 50: + self.experimental_mode = not self.experimental_mode + self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode) + return CC, lac_log def publish_logs(self, CS, start_time, CC, lac_log): diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 99910e1ebf..f89ebe7081 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -4,7 +4,6 @@ import numpy as np from cereal import car, log, custom from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL, DT_CTRL from openpilot.selfdrive.modeld.constants import ModelConstants @@ -86,11 +85,13 @@ class VCruiseHelper: self.slc_state_prev = SpeedLimitControlState.inactive self.slc_speed_limit_offsetted = 0 + self.experimental_mode_update = False + @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp, experimental_mode): + def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp): self.v_cruise_kph_last = self.v_cruise_kph self.slc_state = long_plan_sp.speedLimitControlState @@ -109,7 +110,7 @@ class VCruiseHelper: self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH if self.CP.openpilotLongitudinalControl: - self._update_experimental_mode(CS, experimental_mode) + self._update_experimental_mode(CS) self.update_button_timers(CS, enabled) else: self.v_cruise_kph = V_CRUISE_UNSET @@ -246,27 +247,17 @@ class VCruiseHelper: self.is_metric_prev = is_metric # toggle experimental mode on distance button hold - def _update_experimental_mode(self, CS, experimental_mode): - long_press = False - button_type = None + def _update_experimental_mode(self, CS): + self.experimental_mode_update = False for b in CS.buttonEvents: if b.type == ButtonType.gapAdjustCruise and not b.pressed: if self.button_timers[ButtonType.gapAdjustCruise] > CRUISE_LONG_PRESS: return # end long press - button_type = b.type break else: if self.button_timers[ButtonType.gapAdjustCruise] and self.button_timers[ButtonType.gapAdjustCruise] % CRUISE_LONG_PRESS == 0: - button_type = ButtonType.gapAdjustCruise - long_press = True - - if button_type is None: - return - - if long_press and self.button_timers[button_type] == CRUISE_LONG_PRESS: - self.experimental_mode_updated = True - self.param_s.put_bool_nonblocking("ExperimentalMode", not experimental_mode) + self.experimental_mode_update = True def clip_curvature(v_ego, prev_curvature, new_curvature): From 47d3d1f8a3941d9dbe8314a2e2902be51508d13a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 17:51:21 -0400 Subject: [PATCH 4/8] update --- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/drive_helpers.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a59f734535..887dc1f80e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -723,7 +723,7 @@ class Controls: self.personality = (self.personality - 1) % 3 self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) - # toggle experimental mode on distance button hold + # toggle experimental mode once on distance button hold if self.CP.openpilotLongitudinalControl: if self.v_cruise_helper.experimental_mode_update and self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == 50: self.experimental_mode = not self.experimental_mode diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f89ebe7081..5365d449ba 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -246,7 +246,6 @@ class VCruiseHelper: self.v_cruise_min = VOLKSWAGEN_V_CRUISE_MIN[is_metric] self.is_metric_prev = is_metric - # toggle experimental mode on distance button hold def _update_experimental_mode(self, CS): self.experimental_mode_update = False From 18974df258d42d901c159938b507fd546cb037a7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 17:52:00 -0400 Subject: [PATCH 5/8] unused --- selfdrive/controls/lib/drive_helpers.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 5365d449ba..c874f39a53 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -78,7 +78,6 @@ class VCruiseHelper: 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.param_s = Params() self.is_metric_prev = None self.v_cruise_min = V_CRUISE_MIN self.slc_state = SpeedLimitControlState.inactive From 1866fff33f1f026e467e11c2455d0a1dcd977749 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 17:53:51 -0400 Subject: [PATCH 6/8] combine --- selfdrive/controls/lib/drive_helpers.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index c874f39a53..cb7d284336 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -143,10 +143,7 @@ class VCruiseHelper: long_press = True break - if button_type is None: - return - - if button_type == ButtonType.gapAdjustCruise: + if button_type is None or button_type == ButtonType.gapAdjustCruise: return resume_button = ButtonType.accelCruise From 4ef2afa413946f73f748f647b479b144c9d432b4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 18:05:30 -0400 Subject: [PATCH 7/8] only once --- selfdrive/controls/lib/drive_helpers.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index cb7d284336..2b03974222 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -98,19 +98,18 @@ class VCruiseHelper: self._update_v_cruise_min(is_metric) if CS.cruiseState.available: + if self.CP.openpilotLongitudinalControl: + self._update_experimental_mode(CS) + if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed: # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_acc) self._update_v_cruise_slc(long_plan_sp) self.v_cruise_cluster_kph = self.v_cruise_kph - self.update_button_timers(CS, enabled) else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH - - if self.CP.openpilotLongitudinalControl: - self._update_experimental_mode(CS) - self.update_button_timers(CS, enabled) + self.update_button_timers(CS, enabled) else: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET From 853fed447da65765a0ccbf9184ac926ecbe69ce7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 12 Aug 2024 18:38:09 -0400 Subject: [PATCH 8/8] fix --- selfdrive/controls/controlsd.py | 18 ++++++++++-------- selfdrive/controls/lib/drive_helpers.py | 5 ++--- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 887dc1f80e..8367cca979 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -20,7 +20,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert -from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature +from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature, CRUISE_LONG_PRESS from openpilot.selfdrive.controls.lib.events import Events, ET from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -717,18 +717,20 @@ class Controls: cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") setattr(actuators, p, 0.0) - # 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): - self.personality = (self.personality - 1) % 3 - self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) - # toggle experimental mode once on distance button hold if self.CP.openpilotLongitudinalControl: - if self.v_cruise_helper.experimental_mode_update and self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == 50: + if self.v_cruise_helper.experimental_mode_update and self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == CRUISE_LONG_PRESS: self.experimental_mode = not self.experimental_mode self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode) + # 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) and \ + not self.v_cruise_helper.experimental_mode_update: + self.personality = (self.personality - 1) % 3 + self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) + self.v_cruise_helper.experimental_mode_update = False + return CC, lac_log def publish_logs(self, CS, start_time, CC, lac_log): diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 2b03974222..cc5d19209c 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -113,6 +113,7 @@ class VCruiseHelper: else: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET + self.experimental_mode_update = False def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, reverse_acc): # handle button presses. TODO: this should be in state_control, but a decelCruise press @@ -242,15 +243,13 @@ class VCruiseHelper: self.is_metric_prev = is_metric def _update_experimental_mode(self, CS): - self.experimental_mode_update = False - for b in CS.buttonEvents: if b.type == ButtonType.gapAdjustCruise and not b.pressed: if self.button_timers[ButtonType.gapAdjustCruise] > CRUISE_LONG_PRESS: return # end long press break else: - if self.button_timers[ButtonType.gapAdjustCruise] and self.button_timers[ButtonType.gapAdjustCruise] % CRUISE_LONG_PRESS == 0: + if self.button_timers[ButtonType.gapAdjustCruise] and self.button_timers[ButtonType.gapAdjustCruise] == CRUISE_LONG_PRESS: self.experimental_mode_update = True