diff --git a/cereal/log.capnp b/cereal/log.capnp index eea8c8ba45..bcfcbbf966 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 481a33079b..1e587caece 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -1063,6 +1063,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 759682f44f..f45e16bce6 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.experimental_switcher import ExperimentalSwitcher 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(ExperimentalSwitcher): def __init__(self, CP=None): self.params = Params() @@ -140,6 +141,8 @@ class SelfdriveD: self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) + ExperimentalSwitcher.__init__(self, self.params) + def update_events(self, CS): """Compute onroadEvents from carState""" @@ -367,12 +370,18 @@ class SelfdriveD: if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) + # toggle experimental mode once on distance button hold + if self.CP.openpilotLongitudinalControl: + ExperimentalSwitcher.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/experimental_switcher.py b/sunnypilot/selfdrive/car/experimental_switcher.py new file mode 100644 index 0000000000..3dd3db1478 --- /dev/null +++ b/sunnypilot/selfdrive/car/experimental_switcher.py @@ -0,0 +1,42 @@ +""" +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 + +ButtonType = car.CarState.ButtonEvent.Type +EventName = log.OnroadEvent.EventName + +DISTANCE_LONG_PRESS = 50 + + +class ExperimentalSwitcher: + def __init__(self, params): + self.params = params + + self.button_timers = {ButtonType.gapAdjustCruise: 0} + self.experimental_mode_switched = False + + def update(self, CS, events, experimental_mode): + if CS.cruiseState.available: + self.update_button_timers(CS) + self.update_mode(events, experimental_mode) + + def update_button_timers(self, CS) -> None: + for k in self.button_timers: + if self.button_timers[k] > 0: + self.button_timers[k] += 1 + + for b in CS.buttonEvents: + if b.type.raw in self.button_timers: + self.button_timers[b.type.raw] = 1 if b.pressed else 0 + + def update_mode(self, events, experimental_mode) -> None: + if self.button_timers[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched: + experimental_mode = not experimental_mode + self.params.put_bool_nonblocking("ExperimentalMode", experimental_mode) + events.add(EventName.experimentalModeSwitched) + self.experimental_mode_switched = True