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.

cereal/custom.capnp

@@ -84,7 +84,19 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
   }
 }
 
-struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
+  dec @0 :DynamicExperimentalControl;
+
+  struct DynamicExperimentalControl {
+    state @0 :DynamicExperimentalControlState;
+    enabled @1 :Bool;
+    active @2 :Bool;
+
+    enum DynamicExperimentalControlState {
+      acc @0;
+      blended @1;
+    }
+  }
 }
 
 struct CustomReserved3 @0xda96579883444c35 {

cereal/log.capnp

@@ -199,6 +199,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     silentParkBrake @162;
     controlsMismatchLateral @163;
     hyundaiRadarTracksConfirmed @164;
+    experimentalModeSwitched @165;
 
     soundsUnavailableDEPRECATED @47;
   }
@@ -2641,7 +2642,7 @@ struct Event {
     # DON'T change which struct it points to
     selfdriveStateSP @107 :Custom.SelfdriveStateSP;
     modelManagerSP @108 :Custom.ModelManagerSP;
-    customReserved2 @109 :Custom.CustomReserved2;
+    longitudinalPlanSP @109 :Custom.LongitudinalPlanSP;
     customReserved3 @110 :Custom.CustomReserved3;
     customReserved4 @111 :Custom.CustomReserved4;
     customReserved5 @112 :Custom.CustomReserved5;

cereal/services.py

@@ -77,6 +77,7 @@ _services: dict[str, tuple] = {
   # sunnypilot
   "modelManagerSP": (False, 1., 1),
   "selfdriveStateSP": (True, 100., 10),
+  "longitudinalPlanSP": (True, 20., 10),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params.cc

@@ -229,6 +229,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"HyundaiRadarTracksConfirmed", PERSISTENT},
     {"HyundaiRadarTracksPersistent", PERSISTENT},
     {"HyundaiRadarTracksToggle", PERSISTENT},
+
+    {"DynamicExperimentalControl", PERSISTENT},
 };
 
 } // namespace

selfdrive/car/card.py

@@ -122,6 +122,9 @@ class Car:
     MadsParams().set_alternative_experience(self.CP)
     MadsParams().set_car_specific_params(self.CP)
 
+    # Dynamic Experimental Control
+    self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
+
     openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle")
 
     controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly
@@ -204,7 +207,7 @@ class Car:
     self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
     if self.sm['carControl'].enabled and not self.CC_prev.enabled:
       # Use CarState w/ buttons from the step selfdrived enables on
-      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode)
+      self.v_cruise_helper.initialize_v_cruise(self.CS_prev, self.experimental_mode, self.dynamic_experimental_control)
 
     # TODO: mirror the carState.cruiseState struct?
     CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
@@ -278,6 +281,10 @@ class Car:
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+
+      # sunnypilot
+      self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
+
       time.sleep(0.1)
 
   def card_thread(self):

selfdrive/car/cruise.py

@@ -120,12 +120,13 @@ class VCruiseHelper:
         self.button_timers[b.type.raw] = 1 if b.pressed else 0
         self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
 
-  def initialize_v_cruise(self, CS, experimental_mode: bool) -> None:
+  def initialize_v_cruise(self, CS, experimental_mode: bool, dynamic_experimental_control: bool) -> None:
     # initializing is handled by the PCM
     if self.CP.pcmCruise:
       return
 
-    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL
+    initial_experimental_mode = experimental_mode and not dynamic_experimental_control
+    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_experimental_mode else V_CRUISE_INITIAL
 
     if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
       self.v_cruise_kph = self.v_cruise_kph_last

selfdrive/car/tests/test_cruise_speed.py

@@ -57,16 +57,16 @@ class TestVCruiseHelper:
     for _ in range(2):
       self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
 
-  def enable(self, v_ego, experimental_mode):
+  def enable(self, v_ego, experimental_mode, dynamic_experimental_control):
     # Simulates user pressing set with a current speed
-    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
+    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode, dynamic_experimental_control)
 
   def test_adjust_speed(self):
     """
     Asserts speed changes on falling edges of buttons.
     """
 
-    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
 
     for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
       for pressed in (True, False):
@@ -90,7 +90,7 @@ class TestVCruiseHelper:
       CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
       self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
       if pressed:
-        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
 
       # Expected diff on enabling. Speed should not change on falling edge of pressed
       assert not pressed == self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last
@@ -100,7 +100,7 @@ class TestVCruiseHelper:
     Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
     """
 
-    self.enable(0, False)
+    self.enable(0, False, False)
 
     for standstill in (True, False):
       for pressed in (True, False):
@@ -120,7 +120,7 @@ class TestVCruiseHelper:
 
     for v_ego in np.linspace(0, 100, 101):
       self.reset_cruise_speed_state()
-      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
+      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
 
       # first decrement speed, then perform gas pressed logic
       expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
@@ -142,10 +142,11 @@ class TestVCruiseHelper:
     """
 
     for experimental_mode in (True, False):
-      for v_ego in np.linspace(0, 100, 101):
-        self.reset_cruise_speed_state()
-        assert not self.v_cruise_helper.v_cruise_initialized
+      for dynamic_experimental_control in (True, False):
+        for v_ego in np.linspace(0, 100, 101):
+          self.reset_cruise_speed_state()
+          assert not self.v_cruise_helper.v_cruise_initialized
 
-        self.enable(float(v_ego), experimental_mode)
-        assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
-        assert self.v_cruise_helper.v_cruise_initialized
+          self.enable(float(v_ego), experimental_mode, dynamic_experimental_control)
+          assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX
+          assert self.v_cruise_helper.v_cruise_initialized

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,6 +15,8 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
 
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
@@ -66,10 +68,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05):
   return a_target, should_stop
 
 
-class LongitudinalPlanner:
+class LongitudinalPlanner(LongitudinalPlannerSP):
   def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
     self.mpc = LongitudinalMpc(dt=dt)
+    LongitudinalPlannerSP.__init__(self, self.CP, self.mpc)
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -104,7 +107,10 @@ class LongitudinalPlanner:
     return x, v, a, j, throttle_prob
 
   def update(self, sm):
+    LongitudinalPlannerSP.update(self, sm)
     self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
+    if dec_mpc_mode := self.get_mpc_mode():
+      self.mpc.mode = dec_mpc_mode
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -205,3 +211,5 @@ class LongitudinalPlanner:
     longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
     pm.send('longitudinalPlan', plan_send)
+
+    self.publish_longitudinal_plan_sp(sm, pm)

selfdrive/controls/plannerd.py

@@ -18,7 +18,7 @@ def main():
 
   ldw = LaneDepartureWarning()
   longitudinal_planner = LongitudinalPlanner(CP)
-  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance'])
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'],
                            poll='modelV2', ignore_avg_freq=['radarState'])
 

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)
   }
 
 }

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)

selfdrive/ui/qt/offroad/settings.cc

@@ -40,6 +40,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       "",
       "../assets/img_experimental_white.svg",
     },
+    {
+      "DynamicExperimentalControl",
+      tr("Enable Dynamic Experimental Control"),
+      tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."),
+      "../assets/offroad/icon_blank.png",
+    },
     {
       "DisengageOnAccelerator",
       tr("Disengage on Accelerator Pedal"),

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -2,14 +2,16 @@
 
 #include <QVBoxLayout>
 #include <memory>
-#include "selfdrive/ui/qt/onroad/buttons.h"
 #include "selfdrive/ui/qt/onroad/driver_monitoring.h"
 #include "selfdrive/ui/qt/onroad/model.h"
 #include "selfdrive/ui/qt/widgets/cameraview.h"
 
 #ifdef SUNNYPILOT
+#include "selfdrive/ui/sunnypilot/qt/onroad/buttons.h"
 #include "selfdrive/ui/sunnypilot/qt/onroad/hud.h"
+#define ExperimentalButton ExperimentalButtonSP
 #else
+#include "selfdrive/ui/qt/onroad/buttons.h"
 #include "selfdrive/ui/qt/onroad/hud.h"
 #endif
 

selfdrive/ui/qt/onroad/buttons.cc

@@ -44,6 +44,10 @@ void ExperimentalButton::updateState(const UIState &s) {
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
+  drawButton(p);
+}
+
+void ExperimentalButton::drawButton(QPainter &p) {
   QPixmap img = experimental_mode ? experimental_img : engage_img;
   drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
 }

selfdrive/ui/qt/onroad/buttons.h

@@ -16,13 +16,17 @@ class ExperimentalButton : public QPushButton {
 
 public:
   explicit ExperimentalButton(QWidget *parent = 0);
-  void updateState(const UIState &s);
+  virtual void updateState(const UIState &s);
 
 private:
   void paintEvent(QPaintEvent *event) override;
   void changeMode();
 
   Params params;
+
+protected:
+  virtual void drawButton(QPainter &p);
+
   QPixmap engage_img;
   QPixmap experimental_img;
   bool experimental_mode;

selfdrive/ui/sunnypilot/SConscript

@@ -23,6 +23,7 @@ qt_src = [
   "sunnypilot/qt/offroad/settings/sunnypilot_panel.cc",
   "sunnypilot/qt/offroad/settings/trips_panel.cc",
   "sunnypilot/qt/onroad/annotated_camera.cc",
+  "sunnypilot/qt/onroad/buttons.cc",
   "sunnypilot/qt/onroad/hud.cc",
   "sunnypilot/qt/onroad/model.cc",
   "sunnypilot/qt/onroad/onroad_home.cc",

selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.cc

@@ -189,11 +189,16 @@ void SoftwarePanelSP::updateLabels() {
  * @brief Shows dialog prompting user to reset calibration after model download
  */
 void SoftwarePanelSP::showResetParamsDialog() {
-  const auto confirmMsg = tr("Model download has started in the background.") + "\n" +
-                          tr("We STRONGLY suggest you to reset calibration. Would you like to do that now?");
+  const auto confirmMsg = QString("%1<br><br><b>%2</b><br><br><b>%3</b>")
+                          .arg(tr("Model download has started in the background."))
+                          .arg(tr("We STRONGLY suggest you to reset calibration."))
+                          .arg(tr("Would you like to do that now?"));
   const auto button_text = tr("Reset Calibration");
 
-  if (showConfirmationDialog(confirmMsg, button_text, false)) {
+  QString content("<body><h2 style=\"text-align: center;\">" + tr("Driving Model Selector") + "</h2><br>"
+                  "<p style=\"text-align: center; margin: 0 128px; font-size: 50px;\">" + confirmMsg + "</p></body>");
+
+  if (showConfirmationDialog(content, button_text, false)) {
     params.remove("CalibrationParams");
     params.remove("LiveTorqueParameters");
   }

selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h

@@ -53,7 +53,7 @@ private:
     const QString final_message = QString("%1%2").arg(!message.isEmpty() ? message + "\n" : QString(), warning_message);
     const QString final_buttonText = !confirmButtonText.isEmpty() ? confirmButtonText : QString(tr("Continue") + " %1").arg(show_metered_warning ? tr("on Metered") : "");
 
-    return ConfirmationDialog::confirm(final_message, final_buttonText, parent);
+    return ConfirmationDialog(final_message, final_buttonText, tr("Cancel"), true, parent).exec();
   }
 
   bool is_metered{};

selfdrive/ui/sunnypilot/qt/onroad/buttons.cc

@@ -0,0 +1,51 @@
+/**
+ * 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.
+ */
+
+#include "selfdrive/ui/sunnypilot/qt/onroad/buttons.h"
+
+#include <QPainter>
+
+ExperimentalButtonSP::ExperimentalButtonSP(QWidget *parent) : ExperimentalButton(parent) {
+  QObject::disconnect(uiState(), &UIState::uiUpdate, this, &ExperimentalButton::updateState);
+  QObject::connect(uiState(), &UIState::uiUpdate, this, &ExperimentalButtonSP::updateState);
+}
+
+void ExperimentalButtonSP::updateState(const UIState &s) {
+  ExperimentalButton::updateState(s);
+  const auto long_plan_sp = (*s.sm)["longitudinalPlanSP"].getLongitudinalPlanSP();
+
+  int mode = int(long_plan_sp.getDec().getState());
+  if ((long_plan_sp.getDec().getActive() != dynamic_experimental_control) || (mode != dec_mpc_mode)) {
+    dynamic_experimental_control = long_plan_sp.getDec().getActive();
+    dec_mpc_mode = mode;
+    update();
+  }
+}
+
+void ExperimentalButtonSP::drawButton(QPainter &p) {
+  if (dynamic_experimental_control) {
+    QPixmap left_half = engage_img.copy(0, 0, engage_img.width() / 2, engage_img.height());
+    QPixmap right_half = experimental_img.copy(experimental_img.width() / 2, 0, experimental_img.width() / 2, experimental_img.height());
+
+    QPixmap combined_img(engage_img.width(), engage_img.height());
+    combined_img.fill(Qt::transparent);
+
+    QPainter combined_painter(&combined_img);
+
+    combined_painter.setOpacity(dec_mpc_mode == 1 ? 0.1 : 1.0);
+    combined_painter.drawPixmap(0, 0, left_half);
+
+    combined_painter.setOpacity(dec_mpc_mode == 1 ? 1.0 : 0.1);
+    combined_painter.drawPixmap(engage_img.width() / 2, 0, right_half);
+
+    combined_painter.end();
+
+    drawIcon(p, QPoint(btn_size / 2, btn_size / 2), combined_img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
+  } else {
+    ExperimentalButton::drawButton(p);
+  }
+}

selfdrive/ui/sunnypilot/qt/onroad/buttons.h

@@ -0,0 +1,24 @@
+/**
+ * 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.
+ */
+
+#pragma once
+
+#include "selfdrive/ui/qt/onroad/buttons.h"
+
+class ExperimentalButtonSP : public ExperimentalButton {
+  Q_OBJECT
+
+public:
+  explicit ExperimentalButtonSP(QWidget *parent = nullptr);
+  void updateState(const UIState &s) override;
+
+private:
+  void drawButton(QPainter &p) override;
+
+  bool dynamic_experimental_control;
+  int dec_mpc_mode;
+};

selfdrive/ui/sunnypilot/ui.cc

@@ -18,7 +18,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
-    "modelManagerSP", "selfdriveStateSP",
+    "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP",
   });
 
   // update timer

selfdrive/ui/translations/main_ar.ts

@@ -1105,10 +1105,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">اختيار</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">إعادة ضبط المعايرة</translation>
@@ -1193,6 +1189,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">إلغاء</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1423,6 +1435,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>تمكين مراقبة السائق حتى عندما لا يكون نظام OpenPilot مُفعّلاً.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_de.ts

@@ -1089,10 +1089,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">AUSWÄHLEN</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Neu kalibrieren</translation>
@@ -1177,6 +1173,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Abbrechen</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1419,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_es.ts

@@ -1089,10 +1089,6 @@ Esto puede tardar un minuto.</translation>
         <source>SELECT</source>
         <translation type="unfinished">SELECCIONAR</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Formatear Calibración</translation>
@@ -1177,6 +1173,22 @@ Esto puede tardar un minuto.</translation>
         <source>Default</source>
         <translation>Por Defecto</translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Cancelar</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1419,14 @@ Esto puede tardar un minuto.</translation>
         <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
         <translation>Activar el control longitudinal (fase experimental) para permitir el modo Experimental.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_fr.ts

@@ -1089,10 +1089,6 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>SELECT</source>
         <translation type="unfinished">SÉLECTIONNER</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Réinitialiser la calibration</translation>
@@ -1177,6 +1173,22 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Annuler</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1419,14 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_ja.ts

@@ -1083,10 +1083,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">選択</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">ｷｬﾘﾌﾞﾚｰｼｮﾝをﾘｾｯﾄ</translation>
@@ -1171,6 +1167,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">キャンセル</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1401,6 +1413,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_ko.ts

@@ -1085,10 +1085,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">선택</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">캘리브레이션 초기화</translation>
@@ -1173,6 +1169,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">취소</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1415,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>Openpilot이 활성화되지 않은 경우에도 드라이버 모니터링을 활성화합니다.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_pt-BR.ts

@@ -1089,10 +1089,6 @@ Isso pode levar até um minuto.</translation>
         <source>SELECT</source>
         <translation type="unfinished">SELECIONE</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Reinicializar Calibragem</translation>
@@ -1177,6 +1173,22 @@ Isso pode levar até um minuto.</translation>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">Cancelar</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1407,6 +1419,14 @@ Isso pode levar até um minuto.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado.</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_th.ts

@@ -1085,10 +1085,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">เลือก</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">รีเซ็ตการคาลิเบรท</translation>
@@ -1173,6 +1169,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">ยกเลิก</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1415,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_tr.ts

@@ -1083,10 +1083,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished"></translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">Kalibrasyonu sıfırla</translation>
@@ -1171,6 +1167,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1401,6 +1413,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_zh-CHS.ts

@@ -1085,10 +1085,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">选择</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">重置设备校准</translation>
@@ -1173,6 +1169,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">取消</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1415,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活时也启用驾驶员监控。</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

selfdrive/ui/translations/main_zh-CHT.ts

@@ -1085,10 +1085,6 @@ This may take up to a minute.</source>
         <source>SELECT</source>
         <translation type="unfinished">選取</translation>
     </message>
-    <message>
-        <source>We STRONGLY suggest you to reset calibration. Would you like to do that now?</source>
-        <translation type="unfinished"></translation>
-    </message>
     <message>
         <source>Reset Calibration</source>
         <translation type="unfinished">重設校準</translation>
@@ -1173,6 +1169,22 @@ This may take up to a minute.</source>
         <source>Default</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Cancel</source>
+        <translation type="unfinished">取消</translation>
+    </message>
+    <message>
+        <source>Driving Model Selector</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>We STRONGLY suggest you to reset calibration.</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Would you like to do that now?</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>SshControl</name>
@@ -1403,6 +1415,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活時也啟用駕駛監控。</translation>
     </message>
+    <message>
+        <source>Enable Dynamic Experimental Control</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

sunnypilot/selfdrive/car/button_hold_tracker.py

@@ -0,0 +1,32 @@
+"""
+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
+
+
+class ButtonHoldTracker:
+  def __init__(self):
+    self.button_frame_counts = {}
+
+  def update(self, CS) -> None:
+    if CS.cruiseState.available:
+      self.update_button_timers(CS)
+
+  def update_button_timers(self, CS) -> None:
+    for button_type, held_frames in self.button_frame_counts.items():
+      if held_frames > 0:
+        self.button_frame_counts[button_type] += 1
+
+    for event in CS.buttonEvents:
+      event_type = event.type.raw
+      if not (event.pressed and self.button_frame_counts.get(event_type, 0) > 0):  # No need to reset if the button is "pulsing" always "pressed"
+        self.button_frame_counts[event_type] = int(event.pressed)
+
+  def get_hold_duration(self, button_type):
+    return self.button_frame_counts.get(button_type, 0)

sunnypilot/selfdrive/car/experimental_switcher.py

@@ -0,0 +1,30 @@
+"""
+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 openpilot.sunnypilot.selfdrive.car.button_hold_tracker import ButtonHoldTracker, ButtonType, EventName
+
+DISTANCE_LONG_PRESS = 50
+
+
+class ExperimentalSwitcher:
+  def __init__(self, params, button_type=ButtonType.gapAdjustCruise):
+    self.params = params
+    self.button_type = button_type
+    self._button_tracker = ButtonHoldTracker()
+    self.experimental_mode_switched = False  # Meant to be toggled off by this class's children.
+
+  def update(self, CS, events, experimental_mode):
+    self._button_tracker.update(CS)
+    if CS.cruiseState.available:
+      self.update_mode(events, experimental_mode)
+
+  def update_mode(self, events, experimental_mode) -> None:
+    if self._button_tracker.get_hold_duration(self.button_type) >= 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

sunnypilot/selfdrive/car/tests/test_button_hold_tracker.py

@@ -0,0 +1,81 @@
+"""
+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.
+"""
+
+import pytest
+
+from cereal import car
+from sunnypilot.selfdrive.car.button_hold_tracker import ButtonHoldTracker, ButtonType
+
+
+@pytest.fixture
+def setup_button_tracker():
+  return ButtonHoldTracker()
+
+
+class TestButtonHoldTracker:
+  @pytest.mark.parametrize(
+    "button_type, button_events, initial_timer, expected_timer",
+    [
+      # Basic button press scenarios
+      (ButtonType.gapAdjustCruise, [car.CarState.ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=True)], 0, 1),
+      (ButtonType.gapAdjustCruise, [car.CarState.ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)], 1, 0),
+
+      # Auto-increment when already held
+      (ButtonType.gapAdjustCruise, [], 5, 6),
+      (ButtonType.cancel, [], 10, 11),
+      (ButtonType.leftBlinker, [], 1, 2),
+
+      # Multiple button events in same frame
+      (ButtonType.gapAdjustCruise,
+       [
+         car.CarState.ButtonEvent(type=ButtonType.cancel, pressed=True),
+         car.CarState.ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)
+       ], 0, 0),
+
+      # Events for different buttons shouldn't affect each other
+      (ButtonType.gapAdjustCruise, [car.CarState.ButtonEvent(type=ButtonType.cancel, pressed=True)], 5, 6),
+
+      # Press while already held (like if it was pulsing the event for some reason)
+      (ButtonType.cancel, [car.CarState.ButtonEvent(type=ButtonType.cancel, pressed=True)], 5, 6),
+
+      # Edge cases
+      (ButtonType.leftBlinker, [], 0, 0),
+      (ButtonType.cancel, [car.CarState.ButtonEvent(type=ButtonType.cancel, pressed=False)], 0, 0),
+    ],
+    ids=[
+      "initial_button_press",
+      "button_release",
+      "increment_held_gap_adjust",
+      "increment_held_cancel",
+      "increment_held_blinker",
+      "multiple_events_same_frame",
+      "different_button_no_effect",
+      "press_while_held",
+      "zero_state_no_events",
+      "release_when_not_held"
+    ]
+  )
+  def test_update_button_timers(self, mocker, setup_button_tracker, button_type, button_events, initial_timer, expected_timer):
+    tracker = setup_button_tracker
+    mock_CS = mocker.Mock(buttonEvents=button_events, cruiseState=mocker.Mock(available=True))
+
+    if initial_timer > 0:
+      tracker.button_frame_counts[button_type] = initial_timer
+
+    tracker.update(mock_CS)
+    timer_value = tracker.get_hold_duration(button_type)
+    assert timer_value == expected_timer, f"Expected timer for {button_type} to be {expected_timer}, but got {timer_value}"
+
+  def test_cruise_state_unavailable(self, mocker, setup_button_tracker):
+    tracker = setup_button_tracker
+    mock_CS = mocker.Mock(
+      buttonEvents=[car.CarState.ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=True)],
+      cruiseState=mocker.Mock(available=False)
+    )
+
+    tracker.update(mock_CS)
+    assert tracker.get_hold_duration(ButtonType.gapAdjustCruise) == 0

sunnypilot/selfdrive/car/tests/test_experimental_switcher.py

@@ -0,0 +1,55 @@
+"""
+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.
+"""
+
+import pytest
+
+from cereal import car
+from sunnypilot.selfdrive.car.experimental_switcher import ExperimentalSwitcher, ButtonType, EventName, DISTANCE_LONG_PRESS
+
+
+@pytest.fixture
+def setup_switcher(mocker):
+  mock_params = mocker.Mock()
+  return ExperimentalSwitcher(mock_params), mock_params
+
+
+class TestExperimentalSwitcher:
+  @pytest.mark.parametrize(
+    "use_experimental_mode, long_press, expected_mode, expected_experimental_mode",
+    [
+      (False, True, True, True),  # Normal -> Experimental via long press
+      (True, True, False, True),  # Experimental -> Normal via long press
+      (False, False, False, False)  # No change without long press
+    ],
+    ids=[
+      "enable_experimental_mode",
+      "disable_experimental_mode",
+      "no_mode_change_without_long_press"
+    ]
+  )
+  def test_update_mode_switches(self, mocker, setup_switcher, use_experimental_mode, long_press, expected_mode, expected_experimental_mode):
+    switcher, params = setup_switcher
+    mock_events = mocker.Mock()
+    mock_CS = mocker.Mock(
+      buttonEvents=[car.CarState.ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=True)],
+      cruiseState=mocker.Mock(available=True)
+    )
+
+    if long_press:
+      for _ in range(DISTANCE_LONG_PRESS + 1):
+        switcher.update(mock_CS, mock_events, use_experimental_mode)
+
+    switcher.update_mode(mock_events, use_experimental_mode)
+
+    if expected_experimental_mode:
+      params.put_bool_nonblocking.assert_called_once_with("ExperimentalMode", expected_mode)
+      mock_events.add.assert_called_once_with(EventName.experimentalModeSwitched)
+      assert switcher.experimental_mode_switched is True
+    else:
+      params.put_bool_nonblocking.assert_not_called()
+      mock_events.add.assert_not_called()
+      assert switcher.experimental_mode_switched is False

sunnypilot/selfdrive/controls/lib/dec/constants.py

@@ -0,0 +1,26 @@
+class WMACConstants:
+  LEAD_WINDOW_SIZE = 5
+  LEAD_PROB = 0.5
+
+  SLOW_DOWN_WINDOW_SIZE = 4
+  SLOW_DOWN_PROB = 0.6
+
+  SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
+  #SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.]
+  SLOW_DOWN_DIST = [30., 45., 60., 80., 100., 120., 135., 150.]
+
+  SLOWNESS_WINDOW_SIZE = 12
+  SLOWNESS_PROB = 0.5
+  SLOWNESS_CRUISE_OFFSET = 1.05
+
+  DANGEROUS_TTC_WINDOW_SIZE = 3
+  DANGEROUS_TTC = 2.3
+
+  MPC_FCW_WINDOW_SIZE = 10
+  MPC_FCW_PROB = 0.5
+
+
+class SNG_State:
+  off = 0
+  stopped = 1
+  going = 2

sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -0,0 +1,390 @@
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+#
+# Version = 2025-1-18
+
+import numpy as np
+
+from cereal import messaging
+from opendbc.car import structs
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State
+
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+
+HIGHWAY_CRUISE_KPH = 70
+
+STOP_AND_GO_FRAME = 60
+
+SET_MODE_TIMEOUT = 10
+
+V_ACC_MIN = 9.72
+
+
+class GenericMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.total = 0
+
+  def add_data(self, value: float) -> None:
+    if len(self.data) == self.window_size:
+      self.total -= self.data.pop(0)
+    self.data.append(value)
+    self.total += value
+
+  def get_moving_average(self) -> float | None:
+    return None if len(self.data) == 0 else self.total / len(self.data)
+
+  def reset_data(self) -> None:
+    self.data = []
+    self.total = 0
+
+
+class WeightedMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.weights = np.linspace(1, 3, window_size)  # Linear weights, adjust as needed
+
+  def add_data(self, value: float) -> None:
+    if len(self.data) == self.window_size:
+      self.data.pop(0)
+    self.data.append(value)
+
+  def get_weighted_average(self) -> float | None:
+    if len(self.data) == 0:
+      return None
+    weighted_sum: float = float(np.dot(self.data, self.weights[-len(self.data):]))
+    weight_total: float = float(np.sum(self.weights[-len(self.data):]))
+    return weighted_sum / weight_total
+
+  def reset_data(self) -> None:
+    self.data = []
+
+
+class DynamicExperimentalController:
+  def __init__(self, CP: structs.CarParams, mpc, params=None):
+    self._CP = CP
+    self._mpc = mpc
+    self._params = params or Params()
+    self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
+    self._active: bool = False
+    self._mode: str = 'acc'
+    self._frame: int = 0
+
+    # Use weighted moving average for filtering leads
+    self._lead_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.LEAD_WINDOW_SIZE)
+    self._has_lead_filtered = False
+    self._has_lead_filtered_prev = False
+
+    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOW_DOWN_WINDOW_SIZE)
+    self._has_slow_down: bool = False
+    self._slow_down_confidence: float = 0.0
+
+    self._has_blinkers = False
+
+    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOWNESS_WINDOW_SIZE)
+    self._has_slowness: bool = False
+
+    self._has_nav_instruction = False
+
+    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.DANGEROUS_TTC_WINDOW_SIZE)
+    self._has_dangerous_ttc: bool = False
+
+    self._v_ego_kph = 0.
+    self._v_cruise_kph = 0.
+
+    self._has_lead = False
+
+    self._has_standstill = False
+    self._has_standstill_prev = False
+
+    self._sng_transit_frame = 0
+    self._sng_state = SNG_State.off
+
+    self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.MPC_FCW_WINDOW_SIZE)
+    self._has_mpc_fcw: bool = False
+    self._mpc_fcw_crash_cnt = 0
+
+    self._set_mode_timeout = 0
+
+  def _read_params(self) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._enabled = self._params.get_bool("DynamicExperimentalControl")
+
+  def mode(self) -> str:
+    return str(self._mode)
+
+  def enabled(self) -> bool:
+    return self._enabled
+
+  def active(self) -> bool:
+    return self._active
+
+  @staticmethod
+  def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool:
+    """
+    Basic anomaly detection using standard deviation.
+    """
+    if len(recent_data) < 5:
+      return False
+    mean: float = float(np.mean(recent_data))
+    std_dev: float = float(np.std(recent_data))
+    anomaly: bool = bool(recent_data[-1] > mean + threshold * std_dev)
+
+    # Context check to ensure repeated anomaly
+    if context_check:
+      return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1
+    return anomaly
+
+  def _adaptive_slowdown_threshold(self) -> float:
+    """
+    Adapts the slow-down threshold based on vehicle speed and recent behavior.
+    """
+    slowdown_scaling_factor: float = (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data)))
+    adaptive_threshold: float = float(
+      interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST) * slowdown_scaling_factor
+    )
+    return adaptive_threshold
+
+  def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2):
+    """
+    Smoothing the lead detection to avoid erratic behavior.
+    """
+    lead_filtering: float = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
+    return lead_filtering > WMACConstants.LEAD_PROB
+
+  def _adaptive_lead_prob_threshold(self) -> float:
+    """
+    Adapts lead probability threshold based on driving conditions.
+    """
+    if self._v_ego_kph > HIGHWAY_CRUISE_KPH:
+      return float(WMACConstants.LEAD_PROB + 0.1)  # Increase the threshold on highways
+    return float(WMACConstants.LEAD_PROB)
+
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+    car_state = sm['carState']
+    lead_one = sm['radarState'].leadOne
+    md = sm['modelV2']
+
+    self._v_ego_kph = car_state.vEgo * 3.6
+    self._v_cruise_kph = car_state.vCruise
+    self._has_lead = lead_one.status
+    self._has_standstill = car_state.standstill
+
+    # fcw detection
+    self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0)
+    if _mpc_fcw_weighted_average := self._mpc_fcw_gmac.get_weighted_average():
+      self._has_mpc_fcw = _mpc_fcw_weighted_average > WMACConstants.MPC_FCW_PROB
+    else:
+      self._has_mpc_fcw = False
+
+    # nav enable detection
+    # self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13
+
+    # lead detection with smoothing
+    self._lead_gmac.add_data(lead_one.status)
+    self._has_lead_filtered = (self._lead_gmac.get_weighted_average() or -1.) > WMACConstants.LEAD_PROB
+    #lead_prob = self._lead_gmac.get_weighted_average() or 0
+    #self._has_lead_filtered = self._smoothed_lead_detection(lead_prob)
+
+    # adaptive slow down detection
+    adaptive_threshold = self._adaptive_slowdown_threshold()
+    slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold
+    self._slow_down_gmac.add_data(slow_down_trigger)
+    if _has_slow_down_weighted_average := self._slow_down_gmac.get_weighted_average():
+      self._has_slow_down = _has_slow_down_weighted_average > WMACConstants.SLOW_DOWN_PROB
+      self._slow_down_confidence = _has_slow_down_weighted_average  # Store confidence level
+    else:
+      self._has_slow_down = False
+      self._slow_down_confidence = 0.0  # No confidence if no slowdown
+
+    # anomaly detection for slow down events
+    if self._anomaly_detection(self._slow_down_gmac.data):
+      self._slow_down_confidence *= 0.85  # Reduce confidence
+      self._has_slow_down = self._slow_down_confidence > WMACConstants.SLOW_DOWN_PROB
+
+    # blinker detection
+    self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker
+
+    # sng detection
+    if self._has_standstill:
+      self._sng_state = SNG_State.stopped
+      self._sng_transit_frame = 0
+    else:
+      if self._sng_transit_frame == 0:
+        if self._sng_state == SNG_State.stopped:
+          self._sng_state = SNG_State.going
+          self._sng_transit_frame = STOP_AND_GO_FRAME
+        elif self._sng_state == SNG_State.going:
+          self._sng_state = SNG_State.off
+      elif self._sng_transit_frame > 0:
+        self._sng_transit_frame -= 1
+
+    # slowness detection
+    if not self._has_standstill:
+      self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
+      if _slowness_weighted_average := self._slowness_gmac.get_weighted_average():
+        self._has_slowness = _slowness_weighted_average > WMACConstants.SLOWNESS_PROB
+      else:
+        self._has_slowness = False
+
+    # dangerous TTC detection
+    if not self._has_lead_filtered and self._has_lead_filtered_prev:
+      self._dangerous_ttc_gmac.reset_data()
+      self._has_dangerous_ttc = False
+
+    if self._has_lead and car_state.vEgo >= 0.01:
+      self._dangerous_ttc_gmac.add_data(lead_one.dRel / car_state.vEgo)
+
+    if _dangerous_ttc_weighted_average := self._dangerous_ttc_gmac.get_weighted_average():
+      self._has_dangerous_ttc = _dangerous_ttc_weighted_average <= WMACConstants.DANGEROUS_TTC
+    else:
+      self._has_dangerous_ttc = False
+
+    # keep prev values
+    self._has_standstill_prev = self._has_standstill
+    self._has_lead_filtered_prev = self._has_lead_filtered
+
+  def _radarless_mode(self) -> None:
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
+    if self._has_mpc_fcw:
+      self._set_mode('blended')
+      return
+
+    # Nav enabled and distance to upcoming turning is 300 or below
+    # if self._has_nav_instruction:
+    #  self._set_mode('blended')
+    #  return
+
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when at highway cruise and SNG: blended
+    # ensuring blended mode is used because acc is bad at catching SNG lead car
+    # especially those who accel very fast and then brake very hard.
+    # if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._set_mode('blended')
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._set_mode('blended')
+      return
+
+    # when detecting lead slow down: blended
+    # use blended for higher braking capability
+    if self._has_dangerous_ttc:
+      self._set_mode('blended')
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
+      return
+
+    self._set_mode('acc')
+
+  def _radar_mode(self) -> None:
+    # when mpc fcw crash prob is high
+    # use blended to slow down quickly
+    if self._has_mpc_fcw:
+      self._set_mode('blended')
+      return
+
+    # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition,
+    if self._has_lead_filtered and not self._has_standstill:
+      self._set_mode('acc')
+      return
+
+    # when blinker is on and speed is driving below V_ACC_MIN: blended
+    # we don't want it to switch mode at higher speed, blended may trigger hard brake
+    # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN:
+    #  self._set_mode('blended')
+    #  return
+
+    # when standstill: blended
+    # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light.
+    if self._has_standstill:
+      self._set_mode('blended')
+      return
+
+    # when detecting slow down scenario: blended
+    # e.g. traffic light, curve, stop sign etc.
+    if self._has_slow_down:
+      self._set_mode('blended')
+      return
+
+    # car driving at speed lower than set speed: acc
+    if self._has_slowness:
+      self._set_mode('acc')
+      return
+
+    # Nav enabled and distance to upcoming turning is 300 or below
+    # if self._has_nav_instruction:
+    #  self._set_mode('blended')
+    #  return
+
+    self._set_mode('acc')
+
+  def set_mpc_fcw_crash_cnt(self) -> None:
+    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+
+  def _set_mode(self, mode: str) -> None:
+    if self._set_mode_timeout == 0:
+      self._mode = mode
+      if mode == 'blended':
+        self._set_mode_timeout = SET_MODE_TIMEOUT
+
+    if self._set_mode_timeout > 0:
+      self._set_mode_timeout -= 1
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self._read_params()
+
+    self.set_mpc_fcw_crash_cnt()
+
+    self._update_calculations(sm)
+
+    if self._CP.radarUnavailable:
+      self._radarless_mode()
+    else:
+      self._radar_mode()
+
+    self._active = sm['selfdriveState'].experimentalMode and self._enabled
+
+    self._frame += 1

sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py

@@ -0,0 +1,248 @@
+import pytest
+import numpy as np
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, TRAJECTORY_SIZE, STOP_AND_GO_FRAME
+
+class MockInterp:
+  def __call__(self, x, xp, fp):
+    return np.interp(x, xp, fp)
+
+class MockCarState:
+  def __init__(self, v_ego=0., standstill=False, left_blinker=False, right_blinker=False):
+    self.vEgo = v_ego
+    self.standstill = standstill
+    self.leftBlinker = left_blinker
+    self.rightBlinker = right_blinker
+
+class MockLeadOne:
+  def __init__(self, status=False, d_rel=0):
+    self.status = status
+    self.dRel = d_rel
+
+class MockModelData:
+  def __init__(self, x_vals=None, positions=None):
+    self.orientation = type('Orientation', (), {'x': x_vals})()
+    self.position = type('Position', (), {'x': positions})()
+
+class MockControlState:
+  def __init__(self, v_cruise=0):
+    self.vCruise = v_cruise
+
+@pytest.fixture
+def interp(monkeypatch):
+  mock_interp = MockInterp()
+  monkeypatch.setattr('openpilot.common.numpy_fast.interp', mock_interp)
+  return mock_interp
+
+@pytest.fixture
+def controller(interp):
+  params = Params()
+  params.put_bool("DynamicExperimentalControl", True)
+  return DynamicExperimentalController()
+
+def test_initial_state(controller):
+  """Test initial state of the controller"""
+  assert controller._mode == 'acc'
+  assert not controller._has_lead
+  assert not controller._has_standstill
+  assert controller._sng_state == SNG_State.off
+  assert not controller._has_lead_filtered
+  assert not controller._has_slow_down
+  assert not controller._has_dangerous_ttc
+  assert not controller._has_mpc_fcw
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_standstill_detection(controller, has_radar):
+  """Test standstill detection and state transitions"""
+  car_state = MockCarState(standstill=True)
+  lead_one = MockLeadOne()
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState()
+
+  # Test transition to standstill
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.stopped
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test transition from standstill to moving
+  car_state.standstill = False
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.going
+
+  # Test complete transition to normal driving
+  for _ in range(STOP_AND_GO_FRAME + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller._sng_state == SNG_State.off
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_lead_detection(controller, has_radar):
+  """Test lead vehicle detection and filtering"""
+  car_state = MockCarState(v_ego=20)  # 72 kph
+  lead_one = MockLeadOne(status=True, d_rel=50)  # Safe distance
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=72)
+
+  # Let moving average stabilize
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode
+
+  # Test lead loss detection
+  lead_one.status = False
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_lead_filtered
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_slow_down_detection(controller, has_radar):
+  """Test slow down detection based on trajectory"""
+  car_state = MockCarState(v_ego=10/3.6)  # 10 kph
+  lead_one = MockLeadOne()
+  x_vals = [0] * TRAJECTORY_SIZE
+  positions = [20] * TRAJECTORY_SIZE  # Position within slow down threshold
+  md = MockModelData(x_vals=x_vals, positions=positions)
+  controls_state = MockControlState(v_cruise=30)
+
+  # Test slow down detection
+  for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_slow_down
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test slow down recovery
+  positions = [200] * TRAJECTORY_SIZE  # Position outside slow down threshold
+  md = MockModelData(x_vals=x_vals, positions=positions)
+  for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_slow_down
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_dangerous_ttc_detection(controller, has_radar):
+  """Test Time-To-Collision detection and handling"""
+  car_state = MockCarState(v_ego=10)  # 36 kph
+  lead_one = MockLeadOne(status=True)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=36)
+
+  # First establish normal conditions with lead
+  lead_one.dRel = 100  # Safe distance
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):  # First establish lead detection
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered  # Verify lead is detected
+
+  # Now test dangerous TTC detection
+  lead_one.dRel = 10  # 10m distance - should trigger dangerous TTC
+  # TTC = dRel/vEgo = 10/10 = 1s (which is less than DANGEROUS_TTC = 2.3s)
+
+  # Need to update multiple times to allow the weighted average to stabilize
+  for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_dangerous_ttc, "TTC of 1s should be considered dangerous"
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_mode_transitions(controller, has_radar):
+  """Test comprehensive mode transitions under different conditions"""
+  # Initialize with normal driving conditions
+  car_state = MockCarState(v_ego=25)  # 90 kph
+  lead_one = MockLeadOne(status=False)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[200] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=100)
+
+  def stabilize_filters():
+    """Helper to let all moving averages stabilize"""
+    for _ in range(max(WMACConstants.LEAD_WINDOW_SIZE, WMACConstants.SLOW_DOWN_WINDOW_SIZE,
+                       WMACConstants.DANGEROUS_TTC_WINDOW_SIZE, WMACConstants.MPC_FCW_WINDOW_SIZE) + 1):
+      controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  # Test 1: Normal driving -> ACC mode
+  stabilize_filters()
+  assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode under normal driving conditions"
+
+  # Test 2: Standstill -> Blended mode
+  car_state.standstill = True
+  controller.update(not has_radar, car_state, lead_one, md, controls_state)
+  assert controller.get_mpc_mode() == 'blended', "Should be in blended mode during standstill"
+
+  # Test 3: Lead car appears -> ACC mode
+  car_state = MockCarState(v_ego=20)  # Reset car state
+  lead_one.status = True
+  lead_one.dRel = 50  # Safe distance
+  stabilize_filters()
+  assert not controller._has_dangerous_ttc, "Should not have dangerous TTC"
+  assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode with safe lead distance"
+
+  # Test 4: Dangerous TTC -> Blended mode
+  car_state = MockCarState(v_ego=20)  # 72 kph
+  lead_one.status = True
+  lead_one.dRel = 50  # First establish normal lead detection
+
+  # First establish lead detection
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_lead_filtered  # Verify lead is detected
+
+  # Now create dangerous TTC condition
+  lead_one.dRel = 20  # This creates a TTC of 1s, well below DANGEROUS_TTC
+
+  for _ in range(WMACConstants.DANGEROUS_TTC_WINDOW_SIZE * 2):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition"
+  expected_mode = 'acc' if has_radar else 'blended'
+  assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC"
+
+@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"])
+def test_mpc_fcw_handling(controller, has_radar):
+  """Test MPC FCW crash count handling and mode transitions"""
+  car_state = MockCarState(v_ego=20)
+  lead_one = MockLeadOne()
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=72)
+
+  # Test FCW activation
+  controller.set_mpc_fcw_crash_cnt(5)
+  for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert controller._has_mpc_fcw
+  assert controller.get_mpc_mode() == 'blended'
+
+  # Test FCW recovery
+  controller.set_mpc_fcw_crash_cnt(0)
+  for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1):
+    controller.update(not has_radar, car_state, lead_one, md, controls_state)
+
+  assert not controller._has_mpc_fcw
+
+def test_radar_unavailable_handling(controller):
+  """Test behavior transitions between radar available and unavailable states"""
+  car_state = MockCarState(v_ego=27.78)  # 100 kph
+  lead_one = MockLeadOne(status=True, d_rel=50)
+  md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE)
+  controls_state = MockControlState(v_cruise=100)
+
+  # Test with radar available
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(False, car_state, lead_one, md, controls_state)
+  radar_mode = controller.get_mpc_mode()
+
+  # Test with radar unavailable
+  for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1):
+    controller.update(True, car_state, lead_one, md, controls_state)
+  radarless_mode = controller.get_mpc_mode()
+
+  assert radar_mode is not None
+  assert radarless_mode is not None

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -0,0 +1,41 @@
+"""
+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 messaging, custom
+from opendbc.car import structs
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+
+DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
+
+
+class LongitudinalPlannerSP:
+  def __init__(self, CP: structs.CarParams, mpc):
+    self.dec = DynamicExperimentalController(CP, mpc)
+
+  def get_mpc_mode(self) -> str | None:
+    if not self.dec.active():
+      return None
+
+    return self.dec.mode()
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self.dec.update(sm)
+
+  def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    plan_sp_send = messaging.new_message('longitudinalPlanSP')
+
+    plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+
+    longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+
+    # Dynamic Experimental Control
+    dec = longitudinalPlanSP.dec
+    dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
+    dec.enabled = self.dec.enabled()
+    dec.active = self.dec.active()
+
+    pm.send('longitudinalPlanSP', plan_sp_send)

system/manager/manager.py

@@ -43,6 +43,7 @@ def manager_init() -> None:
   ]
 
   sunnypilot_default_params: list[tuple[str, str | bytes]] = [
+    ("DynamicExperimentalControl", "0"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),
     ("MadsPauseLateralOnBrake", "0"),