From 485ac322500107daba8e33f7c29c8186e06f31d2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 19 Jan 2025 15:23:51 -0500 Subject: [PATCH] Longitudinal: Distance button hold to toggle Chill/Experimental Mode (#576) * Longitudinal: Distance button hold to toggle Chill/Experimental Mode * unused * fix * no need * Refactor: Introduce ButtonHoldTracker to manage button hold durations (#593) Add ButtonHoldTracker for button hold logic and tests Introduce a new `ButtonHoldTracker` class to manage button hold durations, replacing manual timer handling in `ExperimentalSwitcher`. Updated `ExperimentalSwitcher` to leverage this implementation for cleaner and more modular code. Added comprehensive unit tests for both `ButtonHoldTracker` and `ExperimentalSwitcher` to ensure functionality and edge case coverage. * Revert "Refactor: Introduce ButtonHoldTracker to manage button hold durations (#593)" This reverts commit 7ee7e73ce708f9b52aabed23e398ccf94b1834cf. * less in selfdrived * pass carparams into child * tests for cruisehelper * rename these bad bois (happy now @devtekve? xD) * Apply suggestions from code review Co-authored-by: DevTekVE --------- Co-authored-by: DevTekVE --- cereal/log.capnp | 1 + selfdrive/selfdrived/events.py | 4 ++ selfdrive/selfdrived/selfdrived.py | 15 +++-- sunnypilot/selfdrive/car/__init__.py | 0 sunnypilot/selfdrive/car/cruise_helpers.py | 50 ++++++++++++++ sunnypilot/selfdrive/car/tests/__init__.py | 0 .../selfdrive/car/tests/test_cruise_mode.py | 66 +++++++++++++++++++ 7 files changed, 132 insertions(+), 4 deletions(-) create mode 100644 sunnypilot/selfdrive/car/__init__.py create mode 100644 sunnypilot/selfdrive/car/cruise_helpers.py create mode 100644 sunnypilot/selfdrive/car/tests/__init__.py create mode 100644 sunnypilot/selfdrive/car/tests/test_cruise_mode.py diff --git a/cereal/log.capnp b/cereal/log.capnp index 7d1d2ac5f8..512b4f67b4 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -199,6 +199,7 @@ struct OnroadEvent @0xc4fa6047f024e718 { silentParkBrake @162; controlsMismatchLateral @163; hyundaiRadarTracksConfirmed @164; + experimentalModeSwitched @165; soundsUnavailableDEPRECATED @47; } diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index aab9bc433b..ca068c0b15 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -1066,6 +1066,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { EventName.hyundaiRadarTracksConfirmed: { ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize") + }, + + EventName.experimentalModeSwitched: { + ET.WARNING: NormalPermanentAlert("Experimental Mode Switched", duration=1.5) } } diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index ce92c381d9..b3148c5317 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -25,6 +25,7 @@ from openpilot.system.version import get_build_metadata from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP +from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ @@ -44,7 +45,7 @@ SafetyModel = car.CarParams.SafetyModel IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) -class SelfdriveD: +class SelfdriveD(CruiseHelper): def __init__(self, CP=None): self.params = Params() @@ -140,6 +141,8 @@ class SelfdriveD: self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) + CruiseHelper.__init__(self, self.CP) + def update_events(self, CS): """Compute onroadEvents from carState""" @@ -367,12 +370,16 @@ class SelfdriveD: if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) + CruiseHelper.update(self, CS, self.events, 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): - self.personality = (self.personality - 1) % 3 - self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) - self.events.add(EventName.personalityChanged) + if not self.experimental_mode_switched: + self.personality = (self.personality - 1) % 3 + self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) + self.events.add(EventName.personalityChanged) + self.experimental_mode_switched = False def data_sample(self): car_state = messaging.recv_one(self.car_state_sock) diff --git a/sunnypilot/selfdrive/car/__init__.py b/sunnypilot/selfdrive/car/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/cruise_helpers.py b/sunnypilot/selfdrive/car/cruise_helpers.py new file mode 100644 index 0000000000..24ad6638de --- /dev/null +++ b/sunnypilot/selfdrive/car/cruise_helpers.py @@ -0,0 +1,50 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +from cereal import car, log +from opendbc.car import structs +from openpilot.common.params import Params + +ButtonType = car.CarState.ButtonEvent.Type +EventName = log.OnroadEvent.EventName + +DISTANCE_LONG_PRESS = 50 + + +class CruiseHelper: + def __init__(self, CP: structs.CarParams): + self.CP = CP + self.params = Params() + + self.button_frame_counts = {ButtonType.gapAdjustCruise: 0} + self._experimental_mode = False + self.experimental_mode_switched = False + + def update(self, CS, events, experimental_mode) -> None: + if self.CP.openpilotLongitudinalControl: + if CS.cruiseState.available: + self.update_button_frame_counts(CS) + + # toggle experimental mode once on distance button hold + self.update_experimental_mode(events, experimental_mode) + + def update_button_frame_counts(self, CS) -> None: + for button in self.button_frame_counts: + if self.button_frame_counts[button] > 0: + self.button_frame_counts[button] += 1 + + for button_event in CS.buttonEvents: + button = button_event.type.raw + if button in self.button_frame_counts: + self.button_frame_counts[button] = int(button_event.pressed) + + def update_experimental_mode(self, events, experimental_mode) -> None: + if self.button_frame_counts[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched: + self._experimental_mode = not experimental_mode + self.params.put_bool_nonblocking("ExperimentalMode", self._experimental_mode) + events.add(EventName.experimentalModeSwitched) + self.experimental_mode_switched = True diff --git a/sunnypilot/selfdrive/car/tests/__init__.py b/sunnypilot/selfdrive/car/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/tests/test_cruise_mode.py b/sunnypilot/selfdrive/car/tests/test_cruise_mode.py new file mode 100644 index 0000000000..26338319f5 --- /dev/null +++ b/sunnypilot/selfdrive/car/tests/test_cruise_mode.py @@ -0,0 +1,66 @@ +from parameterized import parameterized_class +from cereal import car +from openpilot.selfdrive.selfdrived.events import Events +from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper, DISTANCE_LONG_PRESS + +ButtonEvent = car.CarState.ButtonEvent +ButtonType = car.CarState.ButtonEvent.Type + + +@parameterized_class(('openpilot_longitudinal',), [(True,)]) +class TestCruiseHelper: + def setup_method(self): + self.CP = car.CarParams(openpilotLongitudinalControl=self.openpilot_longitudinal) + self.cruise_helper = CruiseHelper(self.CP) + self.cruise_helper.experimental_mode_switched = False + self.events = Events() + + def reset(self): + for _ in range(2): + CS = car.CarState(cruiseState={"available": False}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)] + self.cruise_helper._experimental_mode = False + self.cruise_helper.experimental_mode_switched = False + self.cruise_helper.update(CS, self.events, False) + + + def test_gap_adjust_cruise_long_press_toggle_mode(self) -> None: + for pressed in (True, False): + for experimental_mode in (True, False): + self.reset() + self.cruise_helper._experimental_mode = experimental_mode + toggled_mode = not experimental_mode if pressed else experimental_mode + + for i in range(DISTANCE_LONG_PRESS): + CS = car.CarState(cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else [] + self.cruise_helper.update(CS, self.events, experimental_mode) + + # mode should be toggled + assert self.cruise_helper._experimental_mode == toggled_mode + assert self.cruise_helper.experimental_mode_switched is pressed + + # keep holding button after switching mode + for _ in range(DISTANCE_LONG_PRESS): + CS = car.CarState(cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] + self.cruise_helper.update(CS, self.events, toggled_mode) + + # mode should not be toggled + assert self.cruise_helper._experimental_mode == toggled_mode + assert self.cruise_helper.experimental_mode_switched is pressed + + def test_gap_adjust_cruise_short_press_toggle_mode(self) -> None: + for pressed in (True, False): + for experimental_mode in (True, False): + self.reset() + self.cruise_helper._experimental_mode = experimental_mode + + for i in range(DISTANCE_LONG_PRESS - 1): + CS = car.CarState(cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else [] + self.cruise_helper.update(CS, self.events, experimental_mode) + + # mode should not be toggled + assert self.cruise_helper._experimental_mode == experimental_mode + assert self.cruise_helper.experimental_mode_switched is False