From deafc23ea39051bb4661e26cd895274a545b5c12 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 15 Sep 2024 02:10:56 -0400 Subject: [PATCH] must check manual presses this way --- .../controllers.py | 10 +++++++--- .../helpers.py | 15 +++++++++++++++ .../controller.py | 2 +- 3 files changed, 23 insertions(+), 4 deletions(-) diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py index 3994f2ff03..0b3c6461ba 100644 --- a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/controllers.py @@ -7,8 +7,9 @@ from openpilot.selfdrive.controls.lib.sunnypilot.speed_limit_controller import A from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.states import InactiveState, \ AcceleratingState, DeceleratingState, HoldingState, ResettingState, LoadingState from openpilot.sunnypilot.controls.lib.custom_stock_longitudinal_controller.helpers import get_set_point, \ - speed_hysteresis + speed_hysteresis, update_manual_button_timers +ButtonType = car.CarState.ButtonEvent.Type ButtonControlState = custom.CarControlSP.CustomStockLongitudinalControl.ButtonControlState SpeedLimitControlState = custom.LongitudinalPlanSP.SpeedLimitControlState TurnSpeedControlState = custom.LongitudinalPlanSP.SpeedLimitControlState @@ -46,7 +47,7 @@ class CustomStockLongitudinalControllerBase(ABC): self.accel_button = None self.decel_button = None - self.set_speed_buttons = None + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} self.button_states = { ButtonControlState.inactive: InactiveState(self, car_state), @@ -99,8 +100,11 @@ class CustomStockLongitudinalControllerBase(ABC): self.v_cruise = round(CS.cruiseState.speed * (CV.MS_TO_MPH if not self.car_state.params_list.is_metric else CV.MS_TO_KPH)) def update_state(self, CS: car.CarState, CC: car.CarControl) -> None: + update_manual_button_timers(CS, self.cruise_buttons) + ready = CS.cruiseState.enabled and not CC.cruiseControl.cancel and not CC.cruiseControl.resume - button_pressed = any(be.type in self.set_speed_buttons for be in CS.buttonEvents) + button_pressed = any(self.cruise_buttons[k] > 0 for k in CS.buttonEvents) + self.is_ready = ready and not button_pressed self.cruise_button = self.button_states[self.button_state]() diff --git a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py index 2992d6465f..a236454a59 100644 --- a/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py +++ b/sunnypilot/controls/lib/custom_stock_longitudinal_controller/helpers.py @@ -1,5 +1,8 @@ +from cereal import car from openpilot.common.conversions import Conversions as CV +ButtonType = car.CarState.ButtonEvent.Type + def get_set_point(is_metric: bool) -> float: return 30 if is_metric else int(20 * CV.MPH_TO_KPH) @@ -11,3 +14,15 @@ def speed_hysteresis(speed: float, speed_steady: float, hyst: float) -> float: elif speed < speed_steady - hyst: speed_steady = speed + hyst return speed_steady + + +def update_manual_button_timers(CS: car.CarState, button_timers: dict[ButtonType, int]) -> None: + # increment timer for buttons still pressed + for k in button_timers: + if button_timers[k] > 0: + button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in button_timers: + # Start/end timer and store current state on change of button pressed + button_timers[b.type.raw] = 1 if b.pressed else 0 diff --git a/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py b/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py index 4839c6ae7f..80951b79b3 100644 --- a/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py +++ b/sunnypilot/selfdrive/car/hyundai/custom_stock_longitudinal_controller/controller.py @@ -13,7 +13,7 @@ class CustomStockLongitudinalController(CustomStockLongitudinalControllerBase): super().__init__(car, car_controller, car_state, CP) self.accel_button = Buttons.RES_ACCEL self.decel_button = Buttons.SET_DECEL - self.set_speed_buttons = (ButtonType.accelCruise, ButtonType.decelCruise, ButtonType.cancel) + self.cruise_buttons = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0} def create_can_mock_button_messages(self) -> list[SendCan]: can_sends = []