Refactor and modularize DynamicExperimentalController logic

Moved DynamicExperimentalController logic and helper functions to a dedicated module for better readability and maintainability. Simplified longitudinal planner logic by introducing reusable methods to manage MPC mode and longitudinal plan publishing. Adjusted file structure for dynamic controller-related components and updated relevant imports.

selfdrive/controls/lib/longitudinal_planner.py

@@ -2,8 +2,6 @@
 import math
 import numpy as np
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.params import Params
-from cereal import custom
 
 import cereal.messaging as messaging
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
@@ -17,9 +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 openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController
-
+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
@@ -33,7 +30,6 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
 
-MpcSource = custom.MpcSource
 
 def get_max_accel(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
@@ -89,19 +85,8 @@ class LongitudinalPlanner:
     self.a_desired_trajectory = np.zeros(CONTROL_N)
     self.j_desired_trajectory = np.zeros(CONTROL_N)
     self.solverExecutionTime = 0.0
-
-    self.params = Params()
-    self.param_read_counter = 0
-    self.read_param()
-
     self.dynamic_experimental_controller = DynamicExperimentalController()
 
-  def read_param(self):
-    try:
-      self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl"))
-    except AttributeError:
-      self.dynamic_experimental_controller = DynamicExperimentalController()
-
   @staticmethod
   def parse_model(model_msg, model_error):
     if (len(model_msg.position.x) == ModelConstants.IDX_N and
@@ -123,16 +108,7 @@ class LongitudinalPlanner:
     return x, v, a, j, throttle_prob
 
   def update(self, sm):
-    if self.param_read_counter % 50 == 0:
-      self.read_param()
-    self.param_read_counter += 1
-    if self.dynamic_experimental_controller.is_enabled() and sm['selfdriveState'].experimentalMode:
-      self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
-      self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'])
-      #, sm['navInstruction'].maneuverDistance)
-      self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode()
-    else:
-      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])
@@ -233,20 +209,4 @@ class LongitudinalPlanner:
     longitudinalPlan.allowThrottle = self.allow_throttle
 
     pm.send('longitudinalPlan', plan_send)
-
-    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 self.mpc.mode == 'blended' else MpcSource.acc
-    print(f"mpcSource: {longitudinalPlanSP.mpcSource}")
-
-    longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled()
-    print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}")
-
-
-    pm.send('longitudinalPlanSP', plan_sp_send)
-
+    publish_longitudinal_plan_sp(sm, pm, self.mpc, self.dynamic_experimental_controller)

sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py

@@ -22,6 +22,7 @@
 #
 # 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
@@ -102,8 +103,9 @@ class WeightedMovingAverageCalculator:
     self.data = []
 
 class DynamicExperimentalController:
-  def __init__(self):
-    self._is_enabled = False
+  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
@@ -181,7 +183,7 @@ class DynamicExperimentalController:
       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):
+  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
@@ -246,7 +248,6 @@ class DynamicExperimentalController:
     # keep prev values
     self._has_standstill_prev = self._has_standstill
     self._has_lead_filtered_prev = self._has_lead_filtered
-    self._frame += 1
 
   def _radarless_mode(self):
     # when mpc fcw crash prob is high
@@ -341,6 +342,9 @@ class DynamicExperimentalController:
     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:
@@ -349,6 +353,7 @@ class DynamicExperimentalController:
         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

sunnypilot/selfdrive/controls/dec/helpers.py

@@ -0,0 +1,42 @@
+from cereal import custom
+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

@@ -1,4 +1,6 @@
-from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import (
+from openpilot.common.params import Params
+
+from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import (
   DynamicExperimentalController,
   TRAJECTORY_SIZE,
   LEAD_WINDOW_SIZE,
@@ -45,8 +47,9 @@ def interp(monkeypatch):
 
 @pytest.fixture
 def controller(interp):
+  params = Params()
+  params.put_bool("DynamicExperimentalControl", True)
   controller = DynamicExperimentalController()
-  controller.set_enabled(True)
   return controller
 
 def test_initial_state(controller):