Merge branch 'dec-new' into dec-new-refactored

The `messaging` module was added to resolve potential issues with undefined references. This change ensures all required imports are present, improving the reliability and maintainability of the code.

cereal/custom.capnp

@@ -8,6 +8,11 @@ $Cxx.namespace("cereal");
 # cereal, so use these if you want custom events in your fork.
 
 # you can rename the struct, but don't change the identifier
+enum MpcSource {
+  acc @0;
+  blended @1;
+}
+
 struct SelfdriveStateSP @0x81c2f05a394cf4af {
   mads @0 :ModularAssistiveDrivingSystem;
 
@@ -82,7 +87,11 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
   }
 }
 
-struct CustomReserved2 @0xf35cc4560bbf6ec2 {
+struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
+  e2eBlended @0 :Text;
+  e2eStatus @1 :Bool;
+  mpcSource @2 :MpcSource;
+  dynamicExperimentalControl @3 :Bool;
 }
 
 struct CustomReserved3 @0xda96579883444c35 {

cereal/log.capnp

@@ -2633,7 +2633,7 @@ struct Event {
     # *********** Custom: reserved for forks ***********
     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., 5),
 
   # debug
   "uiDebug": (True, 0., 1),

common/params.cc

@@ -223,6 +223,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SunnylinkDongleId", PERSISTENT},
     {"SunnylinkdPid", PERSISTENT},
     {"SunnylinkEnabled", PERSISTENT},
+    {"DynamicExperimentalControl", PERSISTENT},
 };
 
 } // namespace

selfdrive/car/card.py

@@ -76,6 +76,8 @@ class Car:
     self.CC_prev = car.CarControl.new_message()
     self.initialized_prev = False
 
+    self.dynamic_experimental_control = False
+
     self.last_actuators_output = structs.CarControl.Actuators()
 
     self.params = Params()
@@ -157,6 +159,7 @@ class Car:
 
     self.is_metric = self.params.get_bool("IsMetric")
     self.experimental_mode = self.params.get_bool("ExperimentalMode")
+    self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl")
 
     # card is driven by can recv, expected at 100Hz
     self.rk = Ratekeeper(100, print_delay_threshold=None)

selfdrive/controls/controlsd.py

@@ -38,7 +38,7 @@ class Controls:
 
     self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
                                    'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
-                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
+                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'longitudinalPlanSP'], poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'])
 
     self.steer_limited = False

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,6 +15,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
+from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import DynamicExperimentalController
+from sunnypilot.selfdrive.controls.dec.helpers import get_mpc_mode, publish_longitudinal_plan_sp
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 A_CRUISE_MIN = -1.2
@@ -83,6 +85,7 @@ class LongitudinalPlanner:
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
     self.solverExecutionTime = 0.0
+    self.dynamic_experimental_controller = DynamicExperimentalController()
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -105,7 +108,7 @@ class LongitudinalPlanner:
     return x, v, a, j, throttle_prob
 
   def update(self, sm):
-    self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
+    self.mpc.mode = get_mpc_mode(sm, self.dynamic_experimental_controller, self.mpc, self.CP)
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -206,3 +209,4 @@ class LongitudinalPlanner:
     longitudinalPlan.allowThrottle = self.allow_throttle
 
     pm.send('longitudinalPlan', plan_send)
+    publish_longitudinal_plan_sp(sm, pm, self.mpc, self.dynamic_experimental_controller)

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/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/model.cc

@@ -106,8 +106,14 @@ void ModelRenderer::drawLaneLines(QPainter &painter) {
 }
 
 void ModelRenderer::drawPath(QPainter &painter, const cereal::ModelDataV2::Reader &model, int height) {
+
+  auto *s = uiState();
+  auto &sm = *(s->sm);
+  const auto long_plan_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+  bool exp_mode_path = (long_plan_sp.getDynamicExperimentalControl() && long_plan_sp.getMpcSource() == cereal::MpcSource::BLENDED) ||
+                       (!long_plan_sp.getDynamicExperimentalControl() && sm["selfdriveState"].getSelfdriveState().getExperimentalMode());
   QLinearGradient bg(0, height, 0, 0);
-  if (experimental_mode) {
+  if (exp_mode_path) {
     // The first half of track_vertices are the points for the right side of the path
     const auto &acceleration = model.getAcceleration().getX();
     const int max_len = std::min<int>(track_vertices.length() / 2, acceleration.size());

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

@@ -1372,6 +1372,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

@@ -1356,6 +1356,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

@@ -1356,6 +1356,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

@@ -1356,6 +1356,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

@@ -1350,6 +1350,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

@@ -1352,6 +1352,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

@@ -1356,6 +1356,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

@@ -1352,6 +1352,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

@@ -1350,6 +1350,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

@@ -1352,6 +1352,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

@@ -1352,6 +1352,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/controls/dec/dynamic_experimental_controller.py

@@ -0,0 +1,380 @@
+# 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 = 2024-7-11
+from openpilot.common.numpy_fast import interp
+from openpilot.common.params import Params
+import numpy as np
+
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+
+LEAD_WINDOW_SIZE = 4
+LEAD_PROB = 0.6
+
+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.]
+
+SLOWNESS_WINDOW_SIZE = 12
+SLOWNESS_PROB = 0.5
+SLOWNESS_CRUISE_OFFSET = 1.05
+
+DANGEROUS_TTC_WINDOW_SIZE = 3
+DANGEROUS_TTC = 2.3
+
+HIGHWAY_CRUISE_KPH = 70
+
+STOP_AND_GO_FRAME = 60
+
+SET_MODE_TIMEOUT = 10
+
+MPC_FCW_WINDOW_SIZE = 10
+MPC_FCW_PROB = 0.5
+
+V_ACC_MIN = 9.72
+
+
+class SNG_State:
+  off = 0
+  stopped = 1
+  going = 2
+
+
+class GenericMovingAverageCalculator:
+  def __init__(self, window_size):
+    self.window_size = window_size
+    self.data = []
+    self.total = 0
+
+  def add_data(self, value):
+    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):
+    return None if len(self.data) == 0 else self.total / len(self.data)
+
+  def reset_data(self):
+    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):
+    if len(self.data) == self.window_size:
+      self.data.pop(0)
+    self.data.append(value)
+
+  def get_weighted_average(self):
+    if len(self.data) == 0:
+      return None
+    weighted_sum = np.dot(self.data, self.weights[-len(self.data):])
+    weight_total = np.sum(self.weights[-len(self.data):])
+    return weighted_sum / weight_total
+
+  def reset_data(self):
+    self.data = []
+
+class DynamicExperimentalController:
+  def __init__(self, params = None):
+    self._params = params or Params()
+    self._is_enabled = self._params.get_bool("DynamicExperimentalControl")
+    self._mode = 'acc'
+    self._mode_prev = 'acc'
+    self._mode_changed = False
+    self._frame = 0
+
+    # Use weighted moving average for filtering leads
+    self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE)
+    self._has_lead_filtered = False
+    self._has_lead_filtered_prev = False
+
+    self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE)
+    self._has_slow_down = False
+
+    self._has_blinkers = False
+
+    self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE)
+    self._has_slowness = False
+
+    self._has_nav_instruction = False
+
+    self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE)
+    self._has_dangerous_ttc = 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=MPC_FCW_WINDOW_SIZE)
+    self._has_mpc_fcw = False
+    self._mpc_fcw_crash_cnt = 0
+
+    self._set_mode_timeout = 0
+
+
+  def _adaptive_slowdown_threshold(self):
+    """
+    Adapts the slow down threshold based on vehicle speed and recent behavior.
+    """
+    return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data)))
+
+  def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True):
+    """
+    Basic anomaly detection using standard deviation.
+    """
+    if len(recent_data) < 5:
+      return False
+    mean = np.mean(recent_data)
+    std_dev = np.std(recent_data)
+    anomaly = 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 _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2):
+    """
+    Smoothing the lead detection to avoid erratic behavior.
+    """
+    self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob
+    return self._has_lead_filtered > LEAD_PROB
+
+  def _adaptive_lead_prob_threshold(self):
+    """
+    Adapts lead probability threshold based on driving conditions.
+    """
+    if self._v_ego_kph > HIGHWAY_CRUISE_KPH:
+      return LEAD_PROB + 0.1  # Increase the threshold on highways
+    return LEAD_PROB
+
+  def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance):
+    self._v_ego_kph = car_state.vEgo * 3.6
+    self._v_cruise_kph = controls_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)
+    self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB
+
+    # 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() > 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)
+    self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB
+
+    # anomaly detection for slow down events
+    if self._anomaly_detection(self._slow_down_gmac.data):
+      # Handle anomaly: potentially log it, adjust behavior, or issue a warning
+      self._has_slow_down = False  # Reset slow down if anomaly detected
+
+    # 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*SLOWNESS_CRUISE_OFFSET))
+      self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB
+
+    # 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)
+
+    self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC
+
+    # keep prev values
+    self._has_standstill_prev = self._has_standstill
+    self._has_lead_filtered_prev = self._has_lead_filtered
+
+  def _radarless_mode(self):
+    # 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):
+    # 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 update(self, radar_unavailable, car_state, lead_one, md, controls_state): #, maneuver_distance):
+    if self._frame % 50 == 0:
+      self._is_enabled = self._params.get_bool("DynamicExperimentalControl")
+
+    if self._is_enabled:
+      self._update(car_state, lead_one, md, controls_state) #, maneuver_distance)
+      if radar_unavailable:
+        self._radarless_mode()
+      else:
+        self._radar_mode()
+    self._mode_changed = self._mode != self._mode_prev
+    self._mode_prev = self._mode
+    self._frame += 1
+
+  def get_mpc_mode(self):
+    return self._mode
+
+  def has_changed(self):
+    return self._mode_changed
+
+  def set_enabled(self, enabled):
+    self._is_enabled = enabled
+
+  def is_enabled(self):
+    return self._is_enabled
+
+  def set_mpc_fcw_crash_cnt(self, crash_cnt):
+    self._mpc_fcw_crash_cnt = crash_cnt
+
+  def _set_mode(self, mode):
+    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

sunnypilot/selfdrive/controls/dec/helpers.py

@@ -0,0 +1,42 @@
+from cereal import custom, messaging
+MpcSource = custom.MpcSource
+
+def get_mpc_mode(sm, dynamic_experimental_controller, mpc, CP):
+  """
+  Determines the appropriate MPC mode based on the experimental state and system
+  configurations. It either returns a default mode or updates the dynamic
+  experimental controller and retrieves the updated mode.
+
+  :param is_experimental: A flag indicating whether to use the experimental mode.
+                          If False, defaults to 'acc' mode.
+  :type is_experimental: bool
+
+  :return: The calculated or retrieved MPC mode.
+  :rtype: str
+  """
+  is_experimental = sm['selfdriveState'].experimentalMode
+  if not dynamic_experimental_controller.is_enabled() or not is_experimental:
+    return 'blended' if is_experimental else 'acc'
+
+  dynamic_experimental_controller.set_mpc_fcw_crash_cnt(mpc.crash_cnt)
+  dynamic_experimental_controller.update(CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'])
+  #, sm['navInstruction'].maneuverDistance)
+  return dynamic_experimental_controller.get_mpc_mode()
+
+
+def publish_longitudinal_plan_sp(sm, pm, mpc, dynamic_experimental_controller):
+  plan_sp_send = messaging.new_message('longitudinalPlanSP')
+
+  plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+
+  longitudinalPlanSP = plan_sp_send.longitudinalPlanSP
+
+  # DEC
+  longitudinalPlanSP.mpcSource = MpcSource.blended if mpc.mode == 'blended' else MpcSource.acc
+  print(f"mpcSource: {longitudinalPlanSP.mpcSource}")
+
+  longitudinalPlanSP.dynamicExperimentalControl = dynamic_experimental_controller.is_enabled()
+  print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}")
+
+
+  pm.send('longitudinalPlanSP', plan_sp_send)

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

@@ -0,0 +1,257 @@
+from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import (
+  DynamicExperimentalController,
+  TRAJECTORY_SIZE,
+  LEAD_WINDOW_SIZE,
+  SLOW_DOWN_WINDOW_SIZE,
+  DANGEROUS_TTC_WINDOW_SIZE,
+  MPC_FCW_WINDOW_SIZE,
+  SNG_State,
+  STOP_AND_GO_FRAME
+)
+
+import pytest
+import numpy as np
+from openpilot.common.params import Params
+
+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)
+  controller = DynamicExperimentalController()
+  return controller
+
+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(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(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(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(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(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(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(LEAD_WINDOW_SIZE, SLOW_DOWN_WINDOW_SIZE,
+                       DANGEROUS_TTC_WINDOW_SIZE, 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(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(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(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(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(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(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

system/manager/manager.py

@@ -48,7 +48,8 @@ def manager_init() -> None:
     ("MadsPauseLateralOnBrake", "0"),
     ("MadsUnifiedEngagementMode", "1"),
     ("ModelManager_LastSyncTime", "0"),
-    ("ModelManager_ModelsCache", "")
+    ("ModelManager_ModelsCache", ""),
+    ("DynamicExperimentalControl", "0"),
   ]
 
   if params.get_bool("RecordFrontLock"):