Refactor: Address PR #1573 review feedback

- Add ACCEL_PERSONALITY_OPTIONS global variable
- Move accel_controller and dynamic_follow logic out of stock
- Add get_accel_clip(), get_cruise_min_accel(), get_t_follow() methods to LongitudinalPlannerSP
- Stock long_mpc.py now accepts optional a_cruise_min and t_follow

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -10,8 +10,6 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
-from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_follow import FollowDistanceController
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
@@ -230,8 +228,6 @@ class LongitudinalMpc:
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
     self.source = SOURCES[2]
-    self.accel_controller = AccelPersonalityController()
-    self.dynamic_follow = FollowDistanceController()
 
   def reset(self):
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -331,28 +327,13 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, a_cruise_min_override=None, t_follow_override=None):
+    t_follow = t_follow_override if t_follow_override is not None else get_T_FOLLOW(personality)
+    a_cruise_min = a_cruise_min_override if a_cruise_min_override is not None else CRUISE_MIN_ACCEL
+
     v_ego = self.x0[1]
-
-    if self.dynamic_follow.is_enabled():
-      t_follow = self.dynamic_follow.get_follow_distance_multiplier(v_ego)
-      #print(f"DEBUG: dynamic_follow enabled, t_follow={t_follow:.3f}, v_ego={v_ego:.2f}, v_cruise={v_cruise:.2f}")
-    else:
-      t_follow = get_T_FOLLOW(personality)
-      #print(f"DEBUG: dynamic_follow disabled, using personality t_follow={t_follow:.3f}, personality={personality}")
-
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
-    # Get acceleration limits
-    if self.accel_controller.is_enabled():
-      min_accel = self.accel_controller.get_min_accel(v_ego)
-      #print(f"DEBUG: accel_enabled=True, min_accel={min_accel:.3f}")
-    else:
-      min_accel = CRUISE_MIN_ACCEL
-      #print(f"DEBUG: accel_enabled=False, using stock min_accel={min_accel}")
-
-    a_cruise_min = min_accel
-
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -124,10 +124,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
     if mode == 'acc':
-      if self.accel_controller.is_enabled():
-        max_accel = self.accel_controller.get_max_accel(v_ego)
-        #print(f"Vibe personality active - max accel: {max_accel:.3f}")
-        accel_clip = [ACCEL_MIN, max_accel]
+      if sp_accel_clip := LongitudinalPlannerSP.get_accel_clip(self, v_ego, mode):
+        accel_clip = sp_accel_clip
       else:
         accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
 
@@ -160,7 +158,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
+    a_cruise_min_override = LongitudinalPlannerSP.get_cruise_min_accel(self, v_ego)
+    t_follow_override = LongitudinalPlannerSP.get_t_follow(self, v_ego)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality, a_cruise_min_override=a_cruise_min_override, t_follow_override=t_follow_override)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -11,6 +11,7 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.common.params import Params
 
 AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+ACCEL_PERSONALITY_OPTIONS = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
 
 # Acceleration Profiles
 MAX_ACCEL_PROFILES = {
@@ -41,43 +42,33 @@ class AccelPersonalityController:
   def __init__(self):
     self.params = Params()
     self.frame = 0
-    self.accel_personality = AccelPersonality.normal
     self.last_max_accel = 2.0
     self.last_min_accel = -0.01
     self.first_run = True
-    self.param_keys = {'personality': 'AccelPersonality', 'enabled': 'AccelPersonalityEnabled'}
-    self._load_personality_from_params()
-
-  def _load_personality_from_params(self):
-    try:
-      saved = self.params.get(self.param_keys['personality'])
-      if saved is not None:
-        val = int(saved)
-        if val in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
-          self.accel_personality = val
-    except (ValueError, TypeError):
-      pass
-
-  def _update_from_params(self):
-    if self.frame % int(1. / DT_MDL) == 0:
-      self._load_personality_from_params()
+    self._accel_personality = self.params.get('AccelPersonality') or AccelPersonality.normal
+    self._enabled = self.params.get_bool('AccelPersonalityEnabled')
 
   def update(self, sm=None):
     self.frame += 1
-    self._update_from_params()
+    if self.frame % int(1.0 / DT_MDL) == 0:
+      self._accel_personality = self.params.get('AccelPersonality') or AccelPersonality.normal
+      self._enabled = self.params.get_bool('AccelPersonalityEnabled')
+
+  @property
+  def accel_personality(self) -> int:
+    return self._accel_personality
 
   def get_accel_personality(self) -> int:
-    self._update_from_params()
-    return int(self.accel_personality)
+    return int(self._accel_personality)
 
   def set_accel_personality(self, personality: int):
-    if personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
-      self.accel_personality = personality
-      self.params.put(self.param_keys['personality'], str(personality))
+    if personality in ACCEL_PERSONALITY_OPTIONS:
+      self._accel_personality = personality
+      self.params.put('AccelPersonality', personality)
 
   def cycle_accel_personality(self) -> int:
-    personality = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
-    next_personality = personality[(personality.index(self.accel_personality) + 1) % len(personality)]
+    current = self._accel_personality
+    next_personality = ACCEL_PERSONALITY_OPTIONS[(ACCEL_PERSONALITY_OPTIONS.index(current) + 1) % len(ACCEL_PERSONALITY_OPTIONS)]
     self.set_accel_personality(next_personality)
     return int(next_personality)
 
@@ -92,8 +83,8 @@ class AccelPersonalityController:
       return float(target_min), float(target_max)
 
     # Smoothing
-    self.last_max_accel = (ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel)
-    smoothed_decel = (DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel)
+    self.last_max_accel = ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel
+    smoothed_decel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel
 
     # Rate Limiting (Asymmetric)
     raw_change = smoothed_decel - self.last_min_accel
@@ -124,18 +115,20 @@ class AccelPersonalityController:
     return self.get_accel_limits(v_ego)[1]
 
   def is_enabled(self) -> bool:
-    return self.params.get_bool(self.param_keys['enabled'])
+    return self._enabled
 
   def set_enabled(self, enabled: bool):
-    self.params.put_bool(self.param_keys['enabled'], enabled)
+    self._enabled = enabled
+    self.params.put_bool('AccelPersonalityEnabled', enabled)
 
   def toggle_enabled(self) -> bool:
-    current = self.is_enabled()
+    current = self._enabled
     self.set_enabled(not current)
     return not current
 
   def reset(self):
-    self.accel_personality = AccelPersonality.normal
+    self._accel_personality = AccelPersonality.normal
+    self.params.put('AccelPersonality', AccelPersonality.normal)
     self.frame = 0
     self.last_max_accel = 2.0
     self.last_min_accel = -0.01

sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py

@@ -31,93 +31,67 @@ class FollowDistanceController:
   def __init__(self):
     self.params = Params()
     self.frame = 0
-    self.personality = LongPersonality.standard
     self.current_multiplier = None
     self.first_run = True
     self.personality_change_cooldown = 0
     self.personality_cooldown_frames = int(PERSONALITY_CHANGE_COOLDOWN_S / DT_MDL)
-    self._load_personality()
-
-  def _load_personality(self):
-    try:
-      saved = self.params.get('LongitudinalPersonality')
-      if saved is not None:
-        val = int(saved)
-        if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
-          self.personality = val
-    except (ValueError, TypeError):
-      pass
-
-  def _update_from_params(self):
-    if self.frame % int(1. / DT_MDL) != 0:
-      return
-
-    if self.personality_change_cooldown > 0:
-      self.personality_change_cooldown -= 1
-      return
-
-    try:
-      param = self.params.get('LongitudinalPersonality')
-      if param is not None:
-        val = int(param)
-        if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
-          if val != self.personality:
-            self.personality = val
-            self.personality_change_cooldown = self.personality_cooldown_frames
-    except (ValueError, TypeError):
-      pass
+    self._personality = self.params.get('LongitudinalPersonality') or LongPersonality.standard
+    self._enabled = self.params.get_bool('DynamicFollow')
 
   def _get_smoothing_factor(self, v_ego: float) -> float:
     speed_factor = np.clip(v_ego / SMOOTHING_SPEED_THRESHOLD, 0.3, 1.0)
     return SMOOTHING_BASE + (SMOOTHING_RANGE * speed_factor)
 
   def is_enabled(self) -> bool:
-    return self.params.get_bool('DynamicFollow')
+    return self._enabled
 
   def set_enabled(self, enabled: bool):
+    self._enabled = enabled
     self.params.put_bool('DynamicFollow', enabled)
 
   def toggle(self) -> bool:
-    enabled = self.is_enabled()
-    self.set_enabled(not enabled)
-    return not enabled
+    current = self._enabled
+    self.set_enabled(not current)
+    return not current
+
+  @property
+  def personality(self) -> int:
+    return self._personality
 
   def get_personality(self) -> int:
-    self._update_from_params()
-    return int(self.personality)
+    return int(self._personality)
 
   def set_personality(self, personality: int):
     if personality not in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]:
       return
-
-    self.personality = personality
-    self.params.put('LongitudinalPersonality', str(personality))
+    self._personality = personality
+    self.params.put('LongitudinalPersonality', personality)
     self.personality_change_cooldown = self.personality_cooldown_frames
 
   def cycle_personality(self) -> int:
     personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
-    current_idx = personalities.index(self.personality)
+    current_idx = personalities.index(self._personality)
     next_personality = personalities[(current_idx + 1) % len(personalities)]
     self.set_personality(next_personality)
     return int(next_personality)
 
   def get_follow_distance_multiplier(self, v_ego: float) -> float:
-    self._update_from_params()
     v_ego = max(0.0, v_ego)
-    target = float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.personality]))
+    target = float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self._personality]))
 
     if self.first_run:
       self.current_multiplier = target
       self.first_run = False
       return self.current_multiplier
 
-    #exponential smoothing with speedadaptive factor
+    # exponential smoothing with speedadaptive factor
     alpha = self._get_smoothing_factor(v_ego)
     self.current_multiplier = alpha * self.current_multiplier + (1.0 - alpha) * target
     return self.current_multiplier
 
   def reset(self):
-    self.personality = LongPersonality.standard
+    self._personality = LongPersonality.standard
+    self.params.put('LongitudinalPersonality', LongPersonality.standard)
     self.frame = 0
     self.current_multiplier = None
     self.first_run = True
@@ -125,4 +99,8 @@ class FollowDistanceController:
 
   def update(self):
     self.frame += 1
-    self._update_from_params()
+    if self.personality_change_cooldown > 0:
+      self.personality_change_cooldown -= 1
+    if self.frame % int(1.0 / DT_MDL) == 0:
+      self._personality = self.params.get('LongitudinalPersonality') or LongPersonality.standard
+      self._enabled = self.params.get_bool('DynamicFollow')

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -18,6 +18,9 @@ from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
+from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_follow import FollowDistanceController
+from opendbc.car.interfaces import ACCEL_MIN
+
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
@@ -28,6 +31,7 @@ class LongitudinalPlannerSP:
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.accel_controller = AccelPersonalityController()
+    self.dynamic_follow = FollowDistanceController()
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -35,8 +39,8 @@ class LongitudinalPlannerSP:
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
 
-    self.output_v_target = 0.
-    self.output_a_target = 0.
+    self.output_v_target = 0.0
+    self.output_a_target = 0.0
 
   @property
   def mlsim(self) -> bool:
@@ -49,6 +53,21 @@ class LongitudinalPlannerSP:
 
     return self.dec.mode()
 
+  def get_accel_clip(self, v_ego: float, mode: str) -> list[float] | None:
+    if mode == 'acc' and self.accel_controller.is_enabled():
+      return [ACCEL_MIN, self.accel_controller.get_max_accel(v_ego)]
+    return None
+
+  def get_cruise_min_accel(self, v_ego: float) -> float | None:
+    if self.accel_controller.is_enabled():
+      return self.accel_controller.get_min_accel(v_ego)
+    return None
+
+  def get_t_follow(self, v_ego: float) -> float | None:
+    if self.dynamic_follow.is_enabled():
+      return self.dynamic_follow.get_follow_distance_multiplier(v_ego)
+    return None
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)