Kumars Vibe

.gitmodules

@@ -4,7 +4,6 @@
 [submodule "opendbc"]
   path = opendbc_repo
   url = https://github.com/sunnypilot/opendbc.git
-  branch = tn
 [submodule "msgq"]
   path = msgq_repo
   url = https://github.com/sunnypilot/msgq.git

openpilot/cereal/custom.capnp

@@ -203,7 +203,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
-  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -306,35 +305,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
-
-  struct AccelController {
-    enabled @0 :Bool;
-    active @1 :Bool;
-    shadowOnlyDEPRECATED @2 :Bool;
-    profile @3 :Profile;
-    state @4 :State;
-
-    enum Profile {
-      eco @0;
-      normal @1;
-      sport @2;
-    }
-
-    enum State {
-      inactive @0;
-      free @1;
-      restrict @2;
-      hold @3;
-      release @4;
-      stopHold @5;
-    }
-  }
-
-  enum AccelerationPersonality {
-    eco @0;
-    normal @1;
-    sport @2;
-  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

openpilot/common/params_keys.h

@@ -188,12 +188,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // toyota specific params
-    {"ToyotaAutoHold", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}},
-
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -234,15 +228,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"TeslaMadsScreenButton", {PERSISTENT | BACKUP, INT, "0"}},
     {"ToyotaEnforceStockLongitudinal", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ToyotaStopAndGoHack", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"ToyotaVirtualCruiseSpeed", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
-    // Accel Controller profiles (Eco / Normal / Sport)
-    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
-
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

openpilot/common/tests/test_params.py

@@ -112,16 +112,12 @@ class TestParams:
   def test_params_default_value(self):
     self.params.remove("LanguageSetting")
     self.params.remove("LongitudinalPersonality")
-    self.params.remove("AccelPersonalityEnabled")
-    self.params.remove("AccelPersonality")
     self.params.remove("LiveParametersV2")
 
     assert self.params.get("LanguageSetting") is None
     assert self.params.get("LanguageSetting", return_default=False) is None
     assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
     assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
-    assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
-    assert self.params.get("AccelPersonality", return_default=True) == 1
     assert self.params.get("LiveParametersV2") is None
     assert self.params.get("LiveParametersV2", return_default=True) is None
 

openpilot/selfdrive/car/card.py

@@ -11,7 +11,7 @@ from opendbc.car.structs import car
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
 from openpilot.common.swaglog import cloudlog, ForwardingHandler
-from opendbc.safety import ALTERNATIVE_EXPERIENCE
+
 from opendbc.car import DT_CTRL, structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.carlog import carlog
@@ -122,13 +122,7 @@ class Car:
       self.CI, self.CP, self.CP_SP = CI, CI.CP, CI.CP_SP
       self.RI = RI
 
-    # set alternative experiences from parameters
-    sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
     self.CP.alternativeExperience = 0
-    if sp_toyota_auto_brake_hold:
-      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
-
-
     # mads
     set_alternative_experience(self.CP, self.CP_SP, self.params)
     set_car_specific_params(self.CP, self.CP_SP, self.params)

openpilot/selfdrive/car/cruise.py

@@ -19,7 +19,6 @@ IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1)  # round here to avoid rounding err
 ButtonEvent = car.CarState.ButtonEvent
 ButtonType = car.CarState.ButtonEvent.Type
 CRUISE_LONG_PRESS = 50
-TOYOTA_VIRTUAL_CRUISE_LONG_PRESS = 65
 CRUISE_NEAREST_FUNC = {
   ButtonType.accelCruise: math.ceil,
   ButtonType.decelCruise: math.floor,
@@ -44,30 +43,6 @@ class VCruiseHelper(VCruiseHelperSP):
   def v_cruise_initialized(self):
     return self.v_cruise_kph != V_CRUISE_UNSET
 
-  @property
-  def software_pcm_cruise_speed(self) -> bool:
-    return self.CP.brand == "toyota" and self.CP.pcmCruise and self.CP.openpilotLongitudinalControl and not self.CP_SP.pcmCruiseSpeed
-
-  @property
-  def cruise_long_press_frames(self) -> int:
-    return TOYOTA_VIRTUAL_CRUISE_LONG_PRESS if self.software_pcm_cruise_speed else CRUISE_LONG_PRESS
-
-  @property
-  def software_pcm_cruise_initialized(self) -> bool:
-    return 0 < self.v_cruise_kph < V_CRUISE_UNSET and 0 < self.v_cruise_cluster_kph < V_CRUISE_UNSET
-
-  def _apply_software_pcm_cruise_delta(self, delta_kph: float, is_metric: bool) -> None:
-    """Move Toyota's planner/display targets together while respecting both targets' bounds."""
-    cluster_min_kph = self.v_cruise_min if is_metric else self.v_cruise_min * CV.MPH_TO_KPH
-    min_delta = max(V_CRUISE_MIN - self.v_cruise_kph, cluster_min_kph - self.v_cruise_cluster_kph)
-    max_delta = min(V_CRUISE_MAX - self.v_cruise_kph, V_CRUISE_MAX - self.v_cruise_cluster_kph)
-    if delta_kph > 0:
-      applied_delta = min(delta_kph, max(0., max_delta))
-    else:
-      applied_delta = max(delta_kph, min(0., min_delta))
-    self.v_cruise_kph = round(self.v_cruise_kph + applied_delta, 1)
-    self.v_cruise_cluster_kph = round(self.v_cruise_cluster_kph + applied_delta, 1)
-
   def update_v_cruise(self, CS, enabled, is_metric):
     self.v_cruise_kph_last = self.v_cruise_kph
 
@@ -76,21 +51,11 @@ class VCruiseHelper(VCruiseHelperSP):
     _enabled = self.update_enabled_state(CS, enabled)
 
     if CS.cruiseState.available:
-      software_pcm_enabled = not self.CP_SP.pcmCruiseSpeed and _enabled
-      if self.software_pcm_cruise_speed:
-        software_pcm_enabled = software_pcm_enabled and self.software_pcm_cruise_initialized
-
-      if not self.CP.pcmCruise or software_pcm_enabled:
+      if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled):
         # if stock cruise is completely disabled, then we can use our own set speed logic
         self._update_v_cruise_non_pcm(CS, _enabled, is_metric)
-        v_cruise_kph_before_sla = self.v_cruise_kph
         self.update_speed_limit_assist_v_cruise_non_pcm()
-        if self.software_pcm_cruise_speed:
-          sla_delta_kph = self.v_cruise_kph - v_cruise_kph_before_sla
-          self.v_cruise_kph = v_cruise_kph_before_sla
-          self._apply_software_pcm_cruise_delta(sla_delta_kph, is_metric)
-        else:
-          self.v_cruise_cluster_kph = self.v_cruise_kph
+        self.v_cruise_cluster_kph = self.v_cruise_kph
       else:
         self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
         self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
@@ -120,13 +85,13 @@ class VCruiseHelper(VCruiseHelperSP):
 
     for b in CS.buttonEvents:
       if b.type.raw in self.button_timers and not b.pressed:
-        if self.button_timers[b.type.raw] > self.cruise_long_press_frames:
+        if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS:
           return  # end long press
         button_type = b.type.raw
         break
     else:
       for k, timer in self.button_timers.items():
-        if timer and timer % self.cruise_long_press_frames == 0:
+        if timer and timer % CRUISE_LONG_PRESS == 0:
           button_type = k
           long_press = True
           break
@@ -150,26 +115,10 @@ class VCruiseHelper(VCruiseHelperSP):
       return
 
     long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta)
-    # Toyota's canonical PCM set speed and displayed cluster set speed can differ. In
-    # software-owned PCM mode, round the value the driver sees and apply the same delta
-    # to both targets so the planner/cluster calibration offset remains intact.
-    v_cruise_reference = self.v_cruise_cluster_kph if self.software_pcm_cruise_speed else self.v_cruise_kph
-    if long_press and v_cruise_reference % v_cruise_delta != 0:  # partial interval
-      v_cruise_reference_new = CRUISE_NEAREST_FUNC[button_type](v_cruise_reference / v_cruise_delta) * v_cruise_delta
+    if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
+      self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
     else:
-      v_cruise_reference_new = v_cruise_reference + v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
-
-    if self.software_pcm_cruise_speed:
-      delta_kph = v_cruise_reference_new - v_cruise_reference
-
-      # If SET is pressed while overriding, do not lower the target below the current speed.
-      if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
-        delta_kph = max(delta_kph, CS.vEgo * CV.MS_TO_KPH - self.v_cruise_kph)
-
-      self._apply_software_pcm_cruise_delta(delta_kph, is_metric)
-      return
-
-    self.v_cruise_kph += v_cruise_reference_new - v_cruise_reference
+      self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
 
     # If set is pressed while overriding, clip cruise speed to minimum of vEgo
     if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
@@ -178,12 +127,6 @@ class VCruiseHelper(VCruiseHelperSP):
     self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
 
   def update_button_timers(self, CS, enabled):
-    if self.software_pcm_cruise_speed and (not enabled or not CS.cruiseState.available or not self.software_pcm_cruise_initialized):
-      for k in self.button_timers:
-        self.button_timers[k] = 0
-        self.button_change_states[k] = {"standstill": False, "enabled": False}
-      return
-
     # increment timer for buttons still pressed
     for k in self.button_timers:
       if self.button_timers[k] > 0:

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -4,7 +4,6 @@ from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from openpilot.common.pid import PIDController
 from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import LongControlSP
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
@@ -40,9 +39,8 @@ def long_control_state_trans(CP_SP, active, long_control_state,
 
   return long_control_state
 
-class LongControl(LongControlSP):
+class LongControl:
   def __init__(self, CP, CP_SP):
-    LongControlSP.__init__(self)
     self.CP = CP
     self.CP_SP = CP_SP
     self.long_control_state = LongCtrlState.off
@@ -62,7 +60,6 @@ class LongControl(LongControlSP):
     self.long_control_state = long_control_state_trans(self.CP_SP, active, self.long_control_state,
                                                        should_stop, CS.brakePressed,
                                                        CS.cruiseState.standstill)
-    LongControlSP.update_state(self, self.long_control_state == LongCtrlState.stopping)
     if self.long_control_state == LongCtrlState.off:
       self.reset()
       output_accel = 0.
@@ -72,7 +69,7 @@ class LongControl(LongControlSP):
       if output_accel > self.CP.stopAccel:
         output_accel = min(output_accel, 0.0)
         # TODO: can we just go straight to stopAccel?
-        output_accel -= LongControlSP.stopping_decel_rate(self, CS, a_target) * DT_CTRL
+        output_accel -= 1.0 * DT_CTRL  # m/s^2/s while trying to stop
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -9,7 +9,6 @@ from openpilot.common.swaglog import cloudlog
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
 
 if __name__ == '__main__':  # generating code
   from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -214,9 +213,8 @@ def gen_long_ocp():
   return ocp
 
 
-class LongitudinalMpc(LongitudinalMpcSP):
+class LongitudinalMpc:
   def __init__(self, dt=DT_MDL):
-    LongitudinalMpcSP.__init__(self)
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
@@ -268,8 +266,7 @@ class LongitudinalMpc(LongitudinalMpcSP):
   def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
     jerk_factor = get_jerk_factor(personality)
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
-    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost,
-                    LongitudinalMpcSP.scale_jerk_cost(self, jerk_factor * J_EGO_COST)]
+    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
     constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
     self.set_cost_weights(cost_weights, constraint_cost_weights)
 
@@ -329,7 +326,7 @@ class LongitudinalMpc(LongitudinalMpcSP):
     # when the leads are no factor.
     v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
     # TODO does this make sense when max_a is negative?
-    v_upper = v_ego + (T_IDXS * self.cruise_accel_max(CRUISE_MAX_ACCEL) * 1.05)
+    v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
     v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper)
     cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
 
@@ -343,7 +340,6 @@ class LongitudinalMpc(LongitudinalMpcSP):
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = ACCEL_MAX
-    LongitudinalMpcSP.apply_accel_limits(self)
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.a_prev)
     self.params[:,4] = t_follow
@@ -363,7 +359,6 @@ class LongitudinalMpc(LongitudinalMpcSP):
     self.solver.constraints_set(0, "ubx", self.x0)
 
     self.solution_status = self.solver.solve()
-    LongitudinalMpcSP.save_solution_status(self)
     self.solve_time = float(self.solver.get_stats('time_tot')[0])
 
     for i in range(N+1):

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -51,7 +51,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
   def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
     self.CP = CP
     self.mpc = LongitudinalMpc(dt=dt)
-    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc, dt=dt)
+    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
     self.fcw = False
     self.dt = dt
     self.allow_throttle = True
@@ -110,15 +110,15 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
       clipped_accel_coast = max(accel_coast, accel_clip[0])
       clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
       accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
+
     # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
     v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+
     if force_slow_decel:
       v_cruise = 0.0
 
-    is_e2e, v_cruise = LongitudinalPlannerSP.update_accel_controller(self, sm, v_cruise, prev_accel_constraint, accel_clip[1], reset_state)
-
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
-    self.mpc.set_cur_state(self.v_desired_filter.x, self.mpc_accel_seed)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
@@ -135,13 +135,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
-    action_t = self.CP.longitudinalActuatorDelay + DT_MDL
+    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
     output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
                                                                         action_t=action_t)
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
-    if is_e2e:
+    if self.is_e2e(sm):
       output_a_target = min(output_a_target_e2e, output_a_target_mpc)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc:
@@ -149,7 +149,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
-    self.output_should_stop = LongitudinalPlannerSP.update_should_stop(self, self.output_should_stop)
 
     for idx in range(2):
       accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)

openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:a501760a9d1d5fef0eab2b8c5d122d06124fc26dc8e0782e0aa94b82a208f0ff
-size 1757355221

openpilot/selfdrive/modeld/models/driving_policy.onnx

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2b85e82079a2d31c5ce8616f2b429ccfcdcb8ebb5aefd72a62b8bd78aa9c7621
+size 15583592

openpilot/selfdrive/modeld/models/driving_supercombo.onnx

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:659727c4d4839adc4992a254409a54259a8756a743f2d567bf5fdc6579f8009b
-size 60881999

openpilot/selfdrive/modeld/models/driving_vision.onnx

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c824f68646a3b94f117f01c70dc8316fb466e05fbd42ccdba440b8a8dc86914b
+size 46265993

openpilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -11,14 +11,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
-class PlannerSM(dict):
-  def __init__(self, radar_frame: int, services: dict):
-    super().__init__(services)
-    self.logMonoTime = {"radarState": radar_frame}
-    self.valid = {"radarState": True}
-    self.alive = {"radarState": True}
-
-
 class Plant:
   messaging_initialized = False
 
@@ -140,7 +132,7 @@ class Plant:
     car_control.carControl.orientationNED = [0., float(pitch), 0.]
 
     # ******** get controlsState messages for plotting ***
-    sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
+    sm = {'radarState': radar.radarState,
           'carState': car_state.carState,
           'carControl': car_control.carControl,
           'controlsState': control.controlsState,
@@ -149,7 +141,7 @@ class Plant:
           'modelV2': model.modelV2,
           'carStateSP': car_state_sp.carStateSP,
           'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-          'gpsLocation': gps_data.gpsLocation})
+          'gpsLocation': gps_data.gpsLocation}
     self.planner.update(sm)
     self.acceleration = self.planner.output_a_target
     if self.planner.output_should_stop:

openpilot/selfdrive/ui/layouts/settings/toggles.py

@@ -27,12 +27,6 @@ DESCRIPTIONS = {
     "In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
     "your steering wheel distance button."
   ),
-  "AccelPersonalityEnabled": tr_noop(
-    "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
-  ),
-  "AccelPersonality": tr_noop(
-    "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly."
-  ),
   "IsLdwEnabled": tr_noop(
     "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
     "without a turn signal activated while driving over 31 mph (50 km/h)."
@@ -112,24 +106,6 @@ class TogglesLayout(Widget):
       icon="speed_limit.png"
     )
 
-    self._accel_personality_enabled = toggle_item(
-      lambda: tr("Enable Accel Controller"),
-      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
-      self._params.get_bool("AccelPersonalityEnabled"),
-      callback=self._set_accel_personality_enabled,
-      icon="speed_limit.png",
-    )
-
-    self._accel_personality_setting = multiple_button_item(
-      lambda: tr("Acceleration Profile"),
-      lambda: tr(DESCRIPTIONS["AccelPersonality"]),
-      buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
-      button_width=300,
-      callback=self._set_accel_personality,
-      selected_index=self._params.get("AccelPersonality", return_default=True),
-      icon="speed_limit.png"
-    )
-
     self._toggles = {}
     self._locked_toggles = set()
     for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
@@ -159,11 +135,9 @@ class TogglesLayout(Widget):
 
       self._toggles[param] = toggle
 
-      # insert longitudinal personality and Accel Controller settings after NDOG toggle
+      # insert longitudinal personality after NDOG toggle
       if param == "DisengageOnAccelerator":
         self._toggles["LongitudinalPersonality"] = self._long_personality_setting
-        self._toggles["AccelPersonalityEnabled"] = self._accel_personality_enabled
-        self._toggles["AccelPersonality"] = self._accel_personality_setting
 
     self._update_experimental_mode_icon()
     self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
@@ -184,7 +158,6 @@ class TogglesLayout(Widget):
 
   def _update_toggles(self):
     ui_state.update_params()
-    accel_personality_enabled = self._params.get_bool("AccelPersonalityEnabled")
 
     e2e_description = tr(
       "sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
@@ -203,15 +176,11 @@ class TogglesLayout(Widget):
         self._toggles["ExperimentalMode"].action_item.set_enabled(True)
         self._toggles["ExperimentalMode"].set_description(e2e_description)
         self._long_personality_setting.action_item.set_enabled(True)
-        self._accel_personality_enabled.action_item.set_enabled(True)
-        self._accel_personality_setting.action_item.set_enabled(accel_personality_enabled)
       else:
         # no long for now
         self._toggles["ExperimentalMode"].action_item.set_enabled(False)
         self._toggles["ExperimentalMode"].action_item.set_state(False)
         self._long_personality_setting.action_item.set_enabled(False)
-        self._accel_personality_enabled.action_item.set_enabled(False)
-        self._accel_personality_setting.action_item.set_enabled(False)
         self._params.remove("ExperimentalMode")
 
         unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
@@ -234,10 +203,6 @@ class TogglesLayout(Widget):
     # refresh toggles from params to mirror external changes
     for param in self._toggle_defs:
       self._toggles[param].action_item.set_state(self._params.get_bool(param))
-    self._accel_personality_enabled.action_item.set_state(accel_personality_enabled)
-    self._accel_personality_setting.action_item.set_selected_button(
-      self._params.get("AccelPersonality", return_default=True)
-    )
 
     # these toggles need restart, block while engaged
     for toggle_def in self._toggle_defs:
@@ -282,10 +247,3 @@ class TogglesLayout(Widget):
 
   def _set_longitudinal_personality(self, button_index: int):
     self._params.put("LongitudinalPersonality", button_index, block=True)
-
-  def _set_accel_personality(self, button_index: int):
-    self._params.put("AccelPersonality", button_index, block=True)
-
-  def _set_accel_personality_enabled(self, state: bool):
-    self._params.put_bool("AccelPersonalityEnabled", state, block=True)
-    self._accel_personality_setting.action_item.set_enabled(state and ui_state.has_longitudinal_control)

openpilot/selfdrive/ui/mici/layouts/main.py

@@ -14,7 +14,6 @@ from openpilot.system.ui.lib.application import gui_app
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.mici.layouts.settings import SettingsLayoutSP as SettingsLayout
   from openpilot.selfdrive.ui.sunnypilot.mici.layouts.home import MiciHomeLayoutSP as MiciHomeLayout
-  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad import OnroadViewContainerSP as AugmentedRoadView
 
 ONROAD_DELAY = 2.5  # seconds
 
@@ -70,9 +69,6 @@ class MiciMainLayout(Scroller):
     # For scroll_to
     return self._body_onroad_layout if ui_state.is_body else self._car_onroad_layout
 
-  def _should_auto_scroll_to_onroad(self) -> bool:
-    return True
-
   def _setup_callbacks(self):
     self._home_layout.set_callbacks(
       on_settings=lambda: gui_app.push_widget(self._settings_layout),
@@ -123,15 +119,13 @@ class MiciMainLayout(Scroller):
 
     # FIXME: these two pops can interrupt user interacting in the settings
     if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:
-      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
-        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
       self._onroad_time_delay = None
 
     # When car leaves standstill, pop nav stack and scroll to onroad
     CS = ui_state.sm["carState"]
     if not CS.standstill and self._prev_standstill:
-      if not gui_app.sunnypilot_ui() or self._should_auto_scroll_to_onroad():
-        gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
+      gui_app.pop_widgets_to(self, lambda: self._scroll_to(self._onroad_layout))
     self._prev_standstill = CS.standstill
 
   def _on_interactive_timeout(self):

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

@@ -14,8 +14,6 @@ class TogglesLayoutMici(NavScroller):
     super().__init__()
 
     self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
-    self._accel_personality_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
-    self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
     self._experimental_btn = BigParamControl("experimental mode", "ExperimentalMode")
     is_metric_toggle = BigParamControl("use metric units", "IsMetric")
     ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
@@ -26,8 +24,6 @@ class TogglesLayoutMici(NavScroller):
 
     self._scroller.add_widgets([
       self._personality_toggle,
-      self._accel_personality_enabled,
-      self._accel_personality_toggle,
       self._experimental_btn,
       is_metric_toggle,
       ldw_toggle,
@@ -40,7 +36,6 @@ class TogglesLayoutMici(NavScroller):
     # Toggle lists
     self._refresh_toggles = (
       ("ExperimentalMode", self._experimental_btn),
-      ("AccelPersonalityEnabled", self._accel_personality_enabled),
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
@@ -50,9 +45,6 @@ class TogglesLayoutMici(NavScroller):
     )
 
     enable_openpilot.set_enabled(lambda: not ui_state.engaged)
-    self._accel_personality_toggle.set_enabled(
-      lambda: ui_state.has_longitudinal_control and ui_state.params.get_bool("AccelPersonalityEnabled")
-    )
     record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged))
     record_mic.set_enabled(lambda: not ui_state.engaged)
 
@@ -83,18 +75,13 @@ class TogglesLayoutMici(NavScroller):
       if ui_state.has_longitudinal_control:
         self._experimental_btn.set_visible(True)
         self._personality_toggle.set_visible(True)
-        self._accel_personality_enabled.set_visible(True)
-        self._accel_personality_toggle.set_visible(True)
       else:
         # no long for now
         self._experimental_btn.set_visible(False)
         self._experimental_btn.set_checked(False)
         self._personality_toggle.set_visible(False)
-        self._accel_personality_enabled.set_visible(False)
-        self._accel_personality_toggle.set_visible(False)
         ui_state.params.remove("ExperimentalMode")
 
     # Refresh toggles from params to mirror external changes
     for key, item in self._refresh_toggles:
       item.set_checked(ui_state.params.get_bool(key))
-    self._accel_personality_toggle.refresh()

openpilot/selfdrive/ui/mici/onroad/model_renderer.py

@@ -154,8 +154,8 @@ class ModelRenderer(Widget, ModelRendererSP):
       self._draw_lane_lines()
       self._draw_path(sm)
 
-    if render_lead_indicator and radar_state:
-       self._draw_lead_indicator()
+    # if render_lead_indicator and radar_state:
+    #   self._draw_lead_indicator()
 
   def _update_raw_points(self, model):
     """Update raw 3D points from model data"""

openpilot/selfdrive/ui/mici/widgets/button.py

@@ -383,18 +383,13 @@ class BigMultiParamToggle(BigMultiToggle):
     self._load_value()
 
   def _load_value(self):
-    value = self._params.get(self._param, return_default=True)
-    index = value if isinstance(value, int) else 0
-    self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
+    self.set_value(self._options[self._params.get(self._param) or 0])
 
   def _handle_mouse_release(self, mouse_pos: MousePos):
     super()._handle_mouse_release(mouse_pos)
     new_idx = self._options.index(self.value)
     self._params.put(self._param, new_idx)
 
-  def refresh(self):
-    self._load_value()
-
 
 class BigParamControl(BigToggle):
   def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):

openpilot/selfdrive/ui/sunnypilot/layouts/settings/cruise.py

@@ -143,8 +143,7 @@ class CruiseLayout(Widget):
           self.icbm_toggle.show_description(True)
 
       if has_long or has_icbm:
-        software_cruise_speed = has_long and (not ui_state.CP.pcmCruise or not ui_state.CP_SP.pcmCruiseSpeed)
-        self.custom_acc_toggle.action_item.set_enabled((software_cruise_speed or has_icbm) and ui_state.is_offroad())
+        self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
         self.dec_toggle.action_item.set_enabled(has_long)
         self.scc_v_toggle.action_item.set_enabled(True)
         self.scc_m_toggle.action_item.set_enabled(True)
@@ -170,7 +169,7 @@ class CruiseLayout(Widget):
       show_custom_acc_desc = True
     else:
       if has_long or has_icbm:
-        if has_long and ui_state.CP.pcmCruise and ui_state.CP_SP.pcmCruiseSpeed:
+        if has_long and ui_state.CP.pcmCruise:
           new_custom_acc_desc = tr(ACC_PCMCRUISE_DISABLED_DESCRIPTION)
           show_custom_acc_desc = True
         else:

openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/toyota.py

@@ -11,12 +11,10 @@ from openpilot.system.ui.lib.multilang import tr, tr_noop
 from openpilot.system.ui.widgets import DialogResult
 from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog
 from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp
-from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
 
 
 ONROAD_ONLY_DESCRIPTION = tr_noop("Start the vehicle to check vehicle compatibility.")
 SNG_HACK_UNAVAILABLE = tr_noop("sunnypilot Longitudinal Control must be available and enabled for your vehicle to use this feature.")
-VIRTUAL_CRUISE_UNAVAILABLE = tr_noop("Virtual Cruise Speed is available only on supported Toyota TSS2 configurations with sunnypilot Longitudinal Control.")
 
 DESCRIPTIONS = {
   'enforce_stock_longitudinal': tr_noop(
@@ -25,14 +23,7 @@ DESCRIPTIONS = {
   'stop_and_go_hack': tr_noop(
     'sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. ' +
     'This feature is only applicable to certain models that are able to use longitudinal control. This is an alpha feature. Use at your own risk.'
-  ),
-  'virtual_cruise_speed': tr_noop(
-    'Use a sunnypilot-owned cruise target with the Toyota RES/SET buttons while sunnypilot longitudinal control is active. ' +
-    'This unlocks Custom ACC Speed Increments; set the short interval to 5 for next-5-unit tap behavior. ' +
-    'The Toyota cluster will continue to show the factory target and may differ from sunnypilot. ' +
-    'The direct button signals are route-validated on Corolla Cross and Prius TSS2, but held-button timing differs by platform. ' +
-    'This is an alpha feature; validate acceleration above the factory target in a controlled setting.'
-  ),
+  )
 }
 
 
@@ -56,17 +47,8 @@ class ToyotaSettings(BrandSettings):
       enabled=lambda: not ui_state.engaged,
     )
 
-    self.virtual_cruise_speed = toggle_item_sp(
-      lambda: tr("Virtual Cruise Speed (Alpha)"),
-      description=lambda: tr(DESCRIPTIONS["virtual_cruise_speed"]),
-      initial_state=ui_state.params.get_bool("ToyotaVirtualCruiseSpeed"),
-      callback=self._on_enable_virtual_cruise_speed,
-      enabled=lambda: not ui_state.engaged,
-    )
-
     self.items = [
       self.enforce_stock_longitudinal,
-      self.virtual_cruise_speed,
       self.stop_and_go_hack,
     ]
 
@@ -78,9 +60,7 @@ class ToyotaSettings(BrandSettings):
           if ui_state.params.get_bool("AlphaLongitudinalEnabled"):
             ui_state.params.put_bool("AlphaLongitudinalEnabled", False)
           ui_state.params.put_bool("ToyotaStopAndGoHack", False)
-          ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", False)
           self.stop_and_go_hack.action_item.set_state(False)
-          self.virtual_cruise_speed.action_item.set_state(False)
           ui_state.params.put_bool("OnroadCycleRequested", True)
         else:
           self.enforce_stock_longitudinal.action_item.set_state(False)
@@ -114,46 +94,10 @@ class ToyotaSettings(BrandSettings):
       ui_state.params.put_bool("ToyotaStopAndGoHack", False)
       ui_state.params.put_bool("OnroadCycleRequested", True)
 
-  def _on_enable_virtual_cruise_speed(self, state: bool):
-    if state:
-      def confirm_callback(result: int):
-        enabled = result == DialogResult.CONFIRM
-        ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", enabled)
-        self.virtual_cruise_speed.action_item.set_state(enabled)
-        if enabled:
-          ui_state.params.put_bool("OnroadCycleRequested", True)
-
-      content = (f"<h1>{self.virtual_cruise_speed.title}</h1><br>" +
-                 f"<p>{self.virtual_cruise_speed.description}</p>")
-      dlg = ConfirmDialog(content, tr("Enable"), rich=True, callback=confirm_callback)
-      gui_app.push_widget(dlg)
-    else:
-      ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", False)
-      ui_state.params.put_bool("OnroadCycleRequested", True)
-
   def update_settings(self):
     if ui_state.CP is not None:
       longitudinal = ui_state.CP.openpilotLongitudinalControl
       enforce_stock = self.enforce_stock_longitudinal.action_item.get_state()
-      virtual_cruise_available = bool(ui_state.CP_SP is not None and
-                                      ui_state.CP_SP.flags & ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE)
-
-      if longitudinal and virtual_cruise_available:
-        self.virtual_cruise_speed.action_item.set_enabled(not ui_state.engaged)
-        virtual_cruise_desc = tr(DESCRIPTIONS["virtual_cruise_speed"])
-        show_virtual_cruise_desc = False
-      else:
-        self.virtual_cruise_speed.action_item.set_enabled(False)
-        if self.virtual_cruise_speed.action_item.get_state():
-          self.virtual_cruise_speed.action_item.set_state(False)
-          ui_state.params.put_bool("ToyotaVirtualCruiseSpeed", False)
-        virtual_cruise_desc = "<b>" + tr(VIRTUAL_CRUISE_UNAVAILABLE) + "</b>\n\n" + tr(DESCRIPTIONS["virtual_cruise_speed"])
-        show_virtual_cruise_desc = True
-
-      if self.virtual_cruise_speed.description != virtual_cruise_desc:
-        self.virtual_cruise_speed.set_description(virtual_cruise_desc)
-        if show_virtual_cruise_desc:
-          self.virtual_cruise_speed.show_description(True)
 
       if longitudinal and not enforce_stock:
         self.stop_and_go_hack.action_item.set_enabled(not ui_state.engaged)
@@ -170,12 +114,6 @@ class ToyotaSettings(BrandSettings):
         if show_desc:
           self.stop_and_go_hack.show_description(True)
     else:
-      self.virtual_cruise_speed.action_item.set_enabled(False)
-      virtual_cruise_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["virtual_cruise_speed"])
-      if self.virtual_cruise_speed.description != virtual_cruise_desc:
-        self.virtual_cruise_speed.set_description(virtual_cruise_desc)
-        self.virtual_cruise_speed.show_description(True)
-
       self.stop_and_go_hack.action_item.set_enabled(False)
       new_desc = "<b>" + tr(ONROAD_ONLY_DESCRIPTION) + "</b>\n\n" + tr(DESCRIPTIONS["stop_and_go_hack"])
       if self.stop_and_go_hack.description != new_desc:

openpilot/selfdrive/ui/sunnypilot/mici/layouts/main.py

@@ -1,19 +0,0 @@
-"""
-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.selfdrive.ui.mici.layouts.main import MiciMainLayout
-from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
-
-
-class MiciMainLayoutSP(MiciMainLayout):
-  def __init__(self):
-    super().__init__()
-    scroller = self._scroller
-    scroller.scroll_panel = GuiScrollPanel2SP(scroller._horizontal, handle_out_of_bounds=not scroller._snap_items)
-
-  def _should_auto_scroll_to_onroad(self) -> bool:
-    return not self._onroad_layout.is_on_info_panel()

openpilot/selfdrive/ui/sunnypilot/mici/layouts/onroad.py

@@ -1,64 +0,0 @@
-"""
-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 collections.abc import Callable
-import pyray as rl
-from openpilot.system.ui.lib.application import gui_app
-from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroller_sp import ScrollerSP
-from openpilot.selfdrive.ui.sunnypilot.mici.onroad.augmented_road_view import AugmentedRoadViewSP
-from openpilot.selfdrive.ui.sunnypilot.mici.layouts.onroad_info_panel import OnroadInfoPanel
-
-CONFIDENCE_BALL_VISIBLE_RATIO = 0.4
-HORIZONTAL_SETTLE_PX = 5
-HORIZONTAL_RESET_RATIO = 0.5
-
-
-class OnroadViewContainerSP(ScrollerSP):
-  def __init__(self, bookmark_callback=None):
-    super().__init__(horizontal=False, snap_items=True, spacing=0, pad=0, scroll_indicator=False, edge_shadows=False)
-    self.road_view = AugmentedRoadViewSP(bookmark_callback=bookmark_callback)
-    self.onroad_info_panel = OnroadInfoPanel(bookmark_callback=bookmark_callback)
-
-    self._scroller.add_widgets([
-      self.road_view,
-      self.onroad_info_panel,
-    ])
-    self._scroller.set_reset_scroll_at_show(False)
-    self._scroller.set_scrolling_enabled(lambda: abs(self.rect.x) < HORIZONTAL_SETTLE_PX)
-
-    for child in (self.road_view, self.onroad_info_panel):
-      inner_touch_valid = child._touch_valid_callback
-      child.set_touch_valid_callback(
-        lambda inner=inner_touch_valid: self._touch_valid() and (inner() if inner else True)
-      )
-
-  def set_rect(self, rect: rl.Rectangle):
-    super().set_rect(rect)
-    self.road_view.set_rect(rect)
-    self.onroad_info_panel.set_rect(rect)
-    return self
-
-  def is_swiping_left(self) -> bool:
-    return self.road_view.is_swiping_left() or self.onroad_info_panel.is_swiping_left()
-
-  def set_click_callback(self, click_callback: Callable[[], None] | None) -> None:
-    self.road_view.set_click_callback(click_callback)
-    self.onroad_info_panel.set_click_callback(click_callback)
-
-  def is_on_info_panel(self) -> bool:
-    """True when scrolled past halfway toward onroad_info_panel (used by main layout
-    to skip auto-pop-back-to-camera while user is reading the info panel)."""
-    return abs(self._scroller.scroll_panel.get_offset()) > self._rect.height / 2
-
-  def _render(self, rect: rl.Rectangle):
-    if abs(self.rect.x) > gui_app.width * HORIZONTAL_RESET_RATIO:
-      self._scroller.scroll_panel.set_offset(0)
-
-    vertical_offset = self._scroller.scroll_panel.get_offset()
-    show_ball = abs(vertical_offset) < rect.height * CONFIDENCE_BALL_VISIBLE_RATIO
-    self.road_view.set_show_confidence_ball(show_ball)
-
-    super()._render(rect)

openpilot/selfdrive/ui/sunnypilot/mici/layouts/onroad_info_panel.py

@@ -1,403 +0,0 @@
-"""
-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 pyray as rl
-from dataclasses import dataclass, field
-from openpilot.common.constants import CV
-from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.selfdrive.ui.ui_state import ui_state
-from openpilot.system.ui.lib.application import gui_app, FontWeight, MousePos
-from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.ui.lib.text_measure import measure_text_cached
-from openpilot.system.ui.widgets import Widget
-from openpilot.selfdrive.ui.mici.onroad.alert_renderer import AlertRenderer
-from openpilot.selfdrive.ui.mici.onroad.augmented_road_view import BookmarkIcon
-
-METER_TO_KM = 0.001
-METER_TO_MILE = 0.000621371
-
-CONTENT_MARGIN = 16
-SPEED_LIMIT_SIGN_WIDTH = 146
-VIENNA_SIGN_SIZE = 146
-MUTCD_SIGN_HEIGHT = 178
-OFFSET_BADGE_SIZE = 50
-OFFSET_BADGE_PANEL_PADDING = 4
-MUTCD_OFFSET_SIGN_Y_SHIFT = 6
-VIENNA_BADGE_X_RATIO = 0.80
-VIENNA_BADGE_UPCOMING_X_RATIO = 0.70
-VIENNA_BADGE_Y_RATIO = -0.82
-UPCOMING_SIGN_SIZE_RATIO = 0.76
-UPCOMING_SIGN_OVERLAP_RATIO = 0.05
-UNIT_FONT_SIZE = 40
-SPEED_FONT_SIZE = 114
-ROAD_FONT_SIZE = 32
-SCC_TAG_WIDTH = 78
-SCC_TAG_HEIGHT = 30
-SCC_TAG_GAP = 5
-COLUMN_GAP = 12
-
-
-@dataclass(frozen=True)
-class OnroadInfoPanelColors:
-  white: rl.Color = rl.WHITE
-  black: rl.Color = rl.BLACK
-  red: rl.Color = field(default_factory=lambda: rl.Color(255, 0, 0, 255))
-  green: rl.Color = field(default_factory=lambda: rl.Color(0, 255, 0, 255))
-  grey: rl.Color = field(default_factory=lambda: rl.Color(190, 195, 190, 255))
-  light_grey: rl.Color = field(default_factory=lambda: rl.Color(200, 200, 200, 255))
-  dark_grey: rl.Color = field(default_factory=lambda: rl.Color(100, 100, 100, 255))
-  bg_dark: rl.Color = field(default_factory=lambda: rl.Color(0, 0, 0, 255))
-  card_bg: rl.Color = field(default_factory=lambda: rl.Color(50, 50, 50, 200))
-  badge_bg: rl.Color = field(default_factory=lambda: rl.Color(60, 60, 60, 255))
-
-
-COLORS = OnroadInfoPanelColors()
-
-
-class OnroadInfoPanel(Widget):
-  def __init__(self, bookmark_callback=None):
-    super().__init__()
-    self.speed_limit: float = 0.0
-    self.speed_limit_valid: bool = False
-    self.speed_limit_offset: float = 0.0
-    self.next_speed_limit: float = 0.0
-    self.next_speed_limit_distance: float = 0.0
-    self.road_name: str = ""
-    self.current_speed: float = 0.0
-    self.set_speed: float = 0.0
-    self.cruise_enabled: bool = False
-
-    self._sign_slide: float = 0.0
-
-    self._font_bold: rl.Font = gui_app.font(FontWeight.BOLD)
-    self._font_semi_bold: rl.Font = gui_app.font(FontWeight.SEMI_BOLD)
-    self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM)
-
-    self._marquee_offset: float = 0.0
-    self._marquee_direction: int = 1
-    self._marquee_pause_timer: float = 0.0
-    self._marquee_speed: float = 40.0
-    self._marquee_pause_duration: float = 1.5
-
-    self._alert_renderer = AlertRenderer()
-    self._alert_alpha_filter = FirstOrderFilter(0, 0.05, 1 / gui_app.target_fps)
-
-    self._bookmark_icon = BookmarkIcon(bookmark_callback)
-
-  def is_swiping_left(self) -> bool:
-    return self._bookmark_icon.is_swiping_left()
-
-  def _handle_mouse_release(self, mouse_pos: MousePos) -> None:
-    # Mirror stock AugmentedRoadView: suppress click while bookmark gesture active
-    if not self._bookmark_icon.interacting():
-      super()._handle_mouse_release(mouse_pos)
-
-  def _update_state(self) -> None:
-    sm = ui_state.sm
-    speed_conv = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH
-
-    if sm.valid["longitudinalPlanSP"]:
-      lp_sp = sm["longitudinalPlanSP"]
-      resolver = lp_sp.speedLimit.resolver
-      self.speed_limit = resolver.speedLimit * speed_conv
-      self.speed_limit_valid = resolver.speedLimitValid
-      self.speed_limit_offset = resolver.speedLimitOffset * speed_conv
-
-    if sm.valid["liveMapDataSP"]:
-      lmd = sm["liveMapDataSP"]
-      self.next_speed_limit = lmd.speedLimitAhead * speed_conv
-      self.next_speed_limit_distance = lmd.speedLimitAheadDistance
-      self.road_name = lmd.roadName
-
-    if sm.updated["carState"]:
-      self.current_speed = sm["carState"].vEgo * speed_conv
-
-    if sm.valid["carState"] and sm.valid["controlsState"]:
-      self.cruise_enabled = sm["carState"].cruiseState.enabled
-      v_cruise_cluster = sm["carState"].vCruiseCluster
-      set_speed_kph = sm["controlsState"].vCruiseDEPRECATED if v_cruise_cluster == 0.0 else v_cruise_cluster
-      self.set_speed = set_speed_kph * (METER_TO_MILE / METER_TO_KM) if not ui_state.is_metric else set_speed_kph
-
-  def _render(self, rect: rl.Rectangle) -> None:
-    self._update_state()
-
-    rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), COLORS.bg_dark)
-
-    left_x = rect.x + CONTENT_MARGIN
-
-    if self.cruise_enabled:
-      unit = tr("MAX")
-      display_speed = self.set_speed
-    else:
-      unit = tr("km/h") if ui_state.is_metric else tr("MPH")
-      display_speed = self.current_speed
-
-    display_speed_text = str(round(display_speed))
-    if self.speed_limit_valid and display_speed > self.speed_limit:
-      speed_color = COLORS.red
-    else:
-      speed_color = COLORS.white
-
-    sign_width = min(SPEED_LIMIT_SIGN_WIDTH, rect.width * 0.30)
-    sign_height = VIENNA_SIGN_SIZE if ui_state.is_metric else MUTCD_SIGN_HEIGHT
-
-    has_upcoming_limit = self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit
-    target_sign_slide = 1.0 if has_upcoming_limit else 0.0
-    slide_speed = 3.0 * rl.get_frame_time()
-    if self._sign_slide < target_sign_slide:
-      self._sign_slide = min(self._sign_slide + slide_speed, target_sign_slide)
-    elif self._sign_slide > target_sign_slide:
-      self._sign_slide = max(self._sign_slide - slide_speed, target_sign_slide)
-
-    upcoming_width = int(sign_width * UPCOMING_SIGN_SIZE_RATIO)
-    upcoming_height = int(sign_height * UPCOMING_SIGN_SIZE_RATIO)
-    upcoming_reserved_width = int(upcoming_width * 0.85) + 5
-    sign_x_without_upcoming = rect.x + rect.width - sign_width - CONTENT_MARGIN
-    sign_x_with_upcoming = rect.x + rect.width - sign_width - CONTENT_MARGIN - upcoming_reserved_width
-    sign_x = sign_x_without_upcoming + (sign_x_with_upcoming - sign_x_without_upcoming) * self._sign_slide
-    sign_y = rect.y + (rect.height - sign_height) / 2
-    if not ui_state.is_metric and self.speed_limit_offset != 0 and self.speed_limit_valid:
-      sign_y += MUTCD_OFFSET_SIGN_Y_SHIFT
-
-    readout_right = sign_x - COLUMN_GAP
-    readout_width = max(1, readout_right - left_x)
-    road_y = rect.y + rect.height - 44
-
-    unit_font_size = self._fit_font_size(self._font_semi_bold, unit, readout_width, 46, UNIT_FONT_SIZE, 28)
-    speed_font_size = self._fit_font_size(self._font_bold, display_speed_text, readout_width, road_y - (rect.y + 54) - 8,
-                                          SPEED_FONT_SIZE, 76)
-    speed_size = measure_text_cached(self._font_bold, display_speed_text, speed_font_size)
-    speed_y = min(rect.y + 54, road_y - speed_size.y - 8)
-    unit_y = max(rect.y + 14, speed_y - unit_font_size - 6)
-
-    rl.draw_text_ex(self._font_semi_bold, unit, rl.Vector2(left_x, unit_y), unit_font_size, 0, COLORS.grey)
-    rl.draw_text_ex(self._font_bold, display_speed_text, rl.Vector2(left_x, speed_y), speed_font_size, 0, speed_color)
-    self._draw_road_name(left_x, road_y, readout_width)
-
-    if has_upcoming_limit and self._sign_slide > 0.01:
-      upcoming_speed_text = str(round(self.next_speed_limit))
-      distance_text = self._format_distance(self.next_speed_limit_distance)
-      upcoming_x = sign_x + sign_width - int(upcoming_width * UPCOMING_SIGN_OVERLAP_RATIO)
-      upcoming_y = sign_y + (sign_height - upcoming_height) / 2
-
-      upcoming_speed_color = COLORS.black
-      if ui_state.is_metric:
-        self._draw_vienna_sign(upcoming_x, upcoming_y, upcoming_width, upcoming_height, upcoming_speed_text, upcoming_speed_color, is_upcoming=True)
-      else:
-        self._draw_mutcd_sign(upcoming_x, upcoming_y, upcoming_width, upcoming_height, upcoming_speed_text, upcoming_speed_color, is_upcoming=True)
-
-      distance_font_size = self._fit_font_size(self._font_medium, distance_text, upcoming_width, 30, 24, 16)
-      distance_size = measure_text_cached(self._font_medium, distance_text, distance_font_size)
-      rl.draw_text_ex(self._font_medium, distance_text, rl.Vector2(upcoming_x + upcoming_width / 2 - distance_size.x / 2, upcoming_y + upcoming_height),
-                      distance_font_size, 0, COLORS.grey)
-
-    self._draw_speed_limit_sign(sign_x, sign_y, sign_width, sign_height)
-
-    if self.speed_limit_offset != 0 and self.speed_limit_valid:
-      offset_text = str(abs(round(self.speed_limit_offset)))
-      badge_size = OFFSET_BADGE_SIZE
-      badge_rect = self._offset_badge_rect(rect, sign_x, sign_y, sign_width, sign_height, badge_size, has_upcoming_limit)
-
-      if ui_state.is_metric:
-        badge_radius = badge_size / 2
-        badge_center_x = badge_rect.x + badge_radius
-        badge_center_y = badge_rect.y + badge_radius
-        rl.draw_circle(int(badge_center_x), int(badge_center_y), badge_radius + 2, COLORS.dark_grey)
-        rl.draw_circle(int(badge_center_x), int(badge_center_y), badge_radius, COLORS.badge_bg)
-        self._draw_text_centered_fit(self._font_bold, offset_text, 32, rl.Vector2(badge_center_x, badge_center_y), COLORS.white,
-                                     badge_size - 10, badge_size - 8, min_size=24)
-      else:
-        rl.draw_rectangle_rounded(badge_rect, 0.25, 10, COLORS.badge_bg)
-        rl.draw_rectangle_rounded_lines_ex(badge_rect, 0.25, 10, 2, COLORS.dark_grey)
-        self._draw_text_centered_fit(self._font_bold, offset_text, 32, rl.Vector2(badge_rect.x + badge_size / 2, badge_rect.y + badge_size / 2),
-                                     COLORS.white, badge_size - 10, badge_size - 8, min_size=24)
-
-    scc_tag_x = min(left_x + speed_size.x + COLUMN_GAP, readout_right - SCC_TAG_WIDTH)
-    scc_tag_y = speed_y + (speed_size.y - (SCC_TAG_HEIGHT * 2 + SCC_TAG_GAP)) / 2
-    if scc_tag_x >= left_x + speed_size.x + 8:
-      self._draw_scc_icons(scc_tag_x, scc_tag_y, readout_right)
-
-    self._bookmark_icon.render(rect)
-
-    if ui_state.started:
-      alert_obj, no_alert = self._alert_renderer.will_render()
-      self._alert_alpha_filter.update(0 if no_alert else 1)
-      alpha = self._alert_alpha_filter.x
-      if alpha > 0.01:
-        rl.draw_rectangle(int(rect.x), int(rect.y), int(rect.width), int(rect.height), rl.Color(0, 0, 0, int(150 * alpha)))
-      self._alert_renderer.render(rect)
-
-  def _draw_scc_icons(self, x: float, y: float, right_limit: float) -> None:
-    sm = ui_state.sm
-    if not sm.valid["longitudinalPlanSP"]:
-      return
-    scc = sm["longitudinalPlanSP"].smartCruiseControl
-
-    drawn = 0
-
-    for label, active in [("SCC-V", scc.vision.active), ("SCC-M", scc.map.active)]:
-      if not active:
-        continue
-      tag_x = x
-      if tag_x + SCC_TAG_WIDTH > right_limit:
-        return
-      tag_y = y + drawn * (SCC_TAG_HEIGHT + SCC_TAG_GAP)
-      rl.draw_rectangle_rounded(rl.Rectangle(tag_x, tag_y, SCC_TAG_WIDTH, SCC_TAG_HEIGHT), 0.3, 10, COLORS.green)
-      self._draw_text_centered_fit(self._font_bold, label, 18, rl.Vector2(tag_x + SCC_TAG_WIDTH / 2, tag_y + SCC_TAG_HEIGHT / 2), COLORS.black,
-                                   SCC_TAG_WIDTH - 10, SCC_TAG_HEIGHT - 4, min_size=14)
-      drawn += 1
-
-  def _draw_speed_limit_sign(self, x: float, y: float, sign_width: float, sign_height: float) -> None:
-    speed_str = str(round(self.speed_limit)) if self.speed_limit_valid and self.speed_limit > 0 else "--"
-    speed_color = COLORS.black if not self.speed_limit_valid or self.current_speed <= self.speed_limit else COLORS.red
-
-    if ui_state.is_metric:
-      self._draw_vienna_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
-    else:
-      self._draw_mutcd_sign(x, y, sign_width, sign_height, speed_str, speed_color, is_upcoming=False)
-
-  def _draw_road_name(self, x: float, y: float, width: float) -> None:
-    if width <= 0:
-      return
-
-    road_display = self.road_name if self.road_name else "--"
-    font_size = self._fit_font_size(self._font_semi_bold, road_display, width, 38, ROAD_FONT_SIZE, 28)
-    road_size = measure_text_cached(self._font_semi_bold, road_display, font_size)
-    text_width = road_size.x
-
-    if text_width <= width:
-      self._marquee_offset = 0.0
-      self._marquee_direction = 1
-      self._marquee_pause_timer = 0.0
-      rl.draw_text_ex(self._font_semi_bold, road_display, rl.Vector2(x, y), font_size, 0, COLORS.white)
-    else:
-      overflow = text_width - width
-      dt = rl.get_frame_time()
-
-      if self._marquee_pause_timer > 0:
-        self._marquee_pause_timer -= dt
-      else:
-        self._marquee_offset += self._marquee_direction * self._marquee_speed * dt
-
-        if self._marquee_offset >= overflow:
-          self._marquee_offset = overflow
-          self._marquee_direction = -1
-          self._marquee_pause_timer = self._marquee_pause_duration
-        elif self._marquee_offset <= 0:
-          self._marquee_offset = 0
-          self._marquee_direction = 1
-          self._marquee_pause_timer = self._marquee_pause_duration
-
-      rl.begin_scissor_mode(int(x), int(y), int(width), int(road_size.y + 4))
-      text_pos = rl.Vector2(x - self._marquee_offset, y)
-      rl.draw_text_ex(self._font_semi_bold, road_display, text_pos, font_size, 0, COLORS.white)
-      rl.end_scissor_mode()
-
-  def _draw_vienna_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
-    center = rl.Vector2(x + width / 2, y + height / 2)
-    outer_radius = min(width, height) / 2
-
-    rl.draw_circle_v(center, outer_radius, COLORS.white)
-    ring_width = outer_radius * 0.18
-    rl.draw_ring(center, outer_radius - ring_width, outer_radius, 0, 360, 36, COLORS.red)
-
-    font_size = outer_radius * (0.7 if len(speed_str) >= 3 else 0.9)
-    self._draw_text_centered_fit(self._font_bold, speed_str, int(font_size), center, speed_color, width * 0.72, height * 0.50, min_size=24)
-
-  def _draw_mutcd_sign(self, x: float, y: float, width: float, height: float, speed_str: str, speed_color: rl.Color, is_upcoming: bool = False) -> None:
-    sign_rect = rl.Rectangle(x, y, width, height)
-    rl.draw_rectangle_rounded(sign_rect, 0.35, 10, COLORS.white)
-
-    inset = max(4, width * 0.05)
-    inner_rect = rl.Rectangle(x + inset, y + inset, width - inset * 2, height - inset * 2)
-    outer_radius = 0.35 * width / 2.0
-    inner_radius = outer_radius - inset
-    inner_roundness = inner_radius / (inner_rect.width / 2.0)
-    rl.draw_rectangle_rounded_lines_ex(inner_rect, inner_roundness, 10, 3, COLORS.black)
-
-    mid_x = x + width / 2
-    label_size = max(18, int(width * 0.26))
-    if is_upcoming:
-      self._draw_text_centered_fit(self._font_bold, tr("AHEAD"), int(width * 0.34), rl.Vector2(mid_x, y + height * 0.28), COLORS.black,
-                                   width * 0.94, height * 0.32, min_size=20)
-    else:
-      self._draw_text_centered_fit(self._font_bold, tr("SPEED"), label_size, rl.Vector2(mid_x, y + height * 0.20), COLORS.black,
-                                   width * 0.84, height * 0.24, min_size=16)
-      self._draw_text_centered_fit(self._font_bold, tr("LIMIT"), label_size, rl.Vector2(mid_x, y + height * 0.40), COLORS.black,
-                                   width * 0.84, height * 0.24, min_size=16)
-
-    speed_font_size = int(width * 0.60) if len(speed_str) >= 3 else int(width * 0.72)
-    self._draw_text_centered_fit(self._font_bold, speed_str, speed_font_size, rl.Vector2(mid_x, y + height * 0.72), speed_color,
-                                 width * 0.90, height * 0.52, min_size=32)
-
-  def _draw_text_centered(self, font, text, size, pos_center, color):
-    sz = measure_text_cached(font, text, size)
-    rl.draw_text_ex(font, text, rl.Vector2(pos_center.x - sz.x / 2, pos_center.y - sz.y / 2), size, 0, color)
-
-  def _draw_text_centered_fit(self, font, text, size, pos_center, color, max_width: float, max_height: float, min_size: int = 10):
-    size = self._fit_font_size(font, text, max_width, max_height, size, min_size)
-    self._draw_text_centered(font, text, size, pos_center, color)
-
-  def _fit_font_size(self, font, text: str, max_width: float, max_height: float, max_size: int | float, min_size: int) -> int:
-    size = int(max_size)
-    while size > min_size:
-      text_size = measure_text_cached(font, text, size)
-      if text_size.x <= max_width and text_size.y <= max_height:
-        return size
-      size -= 2
-    return min_size
-
-  def _offset_badge_rect(self, panel_rect: rl.Rectangle, sign_x: float, sign_y: float, sign_width: float, sign_height: float,
-                         badge_size: float, has_upcoming_limit: bool) -> rl.Rectangle:
-    if ui_state.is_metric:
-      radius = min(sign_width, sign_height) / 2
-      center_x = sign_x + sign_width / 2
-      center_y = sign_y + sign_height / 2
-      badge_x_ratio = VIENNA_BADGE_UPCOMING_X_RATIO if has_upcoming_limit else VIENNA_BADGE_X_RATIO
-      badge_center_x = center_x + radius * badge_x_ratio
-      badge_center_y = center_y + radius * VIENNA_BADGE_Y_RATIO
-      badge_x = badge_center_x - badge_size / 2
-      badge_y = badge_center_y - badge_size / 2
-    else:
-      badge_x = sign_x + sign_width - badge_size * 0.45
-      badge_y = sign_y - badge_size * 0.75
-
-    return rl.Rectangle(
-      self._clamp(
-        badge_x,
-        panel_rect.x + OFFSET_BADGE_PANEL_PADDING,
-        panel_rect.x + panel_rect.width - badge_size - OFFSET_BADGE_PANEL_PADDING,
-      ),
-      self._clamp(
-        badge_y,
-        panel_rect.y + OFFSET_BADGE_PANEL_PADDING,
-        panel_rect.y + panel_rect.height - badge_size - OFFSET_BADGE_PANEL_PADDING,
-      ),
-      badge_size,
-      badge_size,
-    )
-
-  @staticmethod
-  def _clamp(value: float, min_value: float, max_value: float) -> float:
-    return max(min_value, min(max_value, value))
-
-  def _format_distance(self, distance: float) -> str:
-    if ui_state.is_metric:
-      if distance < 50:
-        return tr("Near")
-      if distance >= 1000:
-        return f"{distance * METER_TO_KM:.1f}" + tr("km")
-      if distance < 200:
-        rounded = max(10, int(distance / 10) * 10)
-      else:
-        rounded = int(distance / 100) * 100
-      return str(rounded) + tr("m")
-    else:
-      distance_mi = distance * METER_TO_MILE
-      if distance_mi < 0.1:
-        return tr("Near")
-      return f"{distance_mi:.1f}" + tr("mi")

openpilot/selfdrive/ui/sunnypilot/mici/onroad/augmented_road_view.py

@@ -1,29 +0,0 @@
-"""
-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.selfdrive.ui.mici.onroad.augmented_road_view import AugmentedRoadView
-
-
-class _SuppressedConfidenceBall:
-  def render(self, *_):
-    pass
-
-
-class AugmentedRoadViewSP(AugmentedRoadView):
-  def __init__(self, **kwargs):
-    super().__init__(**kwargs)
-    self._show_confidence_ball: bool = True
-    self._real_confidence_ball = self._confidence_ball
-    self._confidence_ball = _SuppressedConfidenceBall()
-
-  def set_show_confidence_ball(self, show: bool) -> None:
-    self._show_confidence_ball = show
-
-  def _render(self, _) -> None:
-    super()._render(_)
-    if self._show_confidence_ball:
-      self._real_confidence_ball.render(self.rect)

openpilot/selfdrive/ui/sunnypilot/mici/tests/test_scroller_sp.py

@@ -1,83 +0,0 @@
-import pyray as rl
-
-from openpilot.system.ui.lib.application import MouseEvent, MousePos, gui_app
-from openpilot.system.ui.lib.scroll_panel2 import ScrollState
-from openpilot.system.ui.widgets import Widget
-from openpilot.system.ui.widgets import scroller as scroller_mod
-from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
-
-
-class DummyScrollIndicator:
-  def update(self, *_) -> None:
-    pass
-
-  def render(self) -> None:
-    pass
-
-
-class DummyWidget(Widget):
-  def __init__(self, rect: rl.Rectangle):
-    super().__init__()
-    self.set_rect(rect)
-
-  def _render(self, _) -> None:
-    pass
-
-
-def _mouse_event(x: float, y: float, *, pressed: bool = False, released: bool = False,
-                 down: bool = True, t: float = 0.0) -> MouseEvent:
-  return MouseEvent(MousePos(x, y), 0, pressed, released, down, t)
-
-
-def test_vertical_snap_items_are_supported(monkeypatch):
-  monkeypatch.setattr(scroller_mod, "ScrollIndicator", DummyScrollIndicator)
-
-  scroller = scroller_mod._Scroller([], horizontal=False, snap_items=True, scroll_indicator=False)
-  scroller.set_rect(rl.Rectangle(0, 0, 100, 100))
-  scroller.scroll_panel.set_offset(-60)
-
-  captured_snap_target = None
-
-  def update(_, __, snap_target=None):
-    nonlocal captured_snap_target
-    captured_snap_target = snap_target
-    return scroller.scroll_panel.get_offset()
-
-  monkeypatch.setattr(scroller.scroll_panel, "update", update)
-
-  visible_items: list[Widget] = [
-    DummyWidget(rl.Rectangle(0, -60, 100, 100)),
-    DummyWidget(rl.Rectangle(0, 40, 100, 100)),
-  ]
-  scroller._get_scroll(visible_items, 200)
-
-  assert captured_snap_target == -100
-
-
-def test_scroll_panel_sp_rejects_orthogonal_drags(monkeypatch):
-  panel = GuiScrollPanel2SP(horizontal=True)
-  bounds = rl.Rectangle(0, 0, 100, 100)
-
-  monkeypatch.setattr(gui_app, "_mouse_events", [_mouse_event(10, 10, pressed=True, t=1.0)])
-  panel.update(bounds, 200)
-  assert panel.state == ScrollState.PRESSED
-
-  monkeypatch.setattr(gui_app, "_mouse_events", [_mouse_event(23, 60, t=1.1)])
-  panel.update(bounds, 200)
-
-  assert panel.state == ScrollState.STEADY
-  assert panel.get_offset() == 0
-
-
-def test_scroll_panel_sp_can_disable_out_of_bounds_handling(monkeypatch):
-  panel = GuiScrollPanel2SP(horizontal=False, handle_out_of_bounds=False)
-  bounds = rl.Rectangle(0, 0, 100, 100)
-  monkeypatch.setattr(gui_app, "_mouse_events", [])
-
-  panel.set_offset(20)
-  panel.update(bounds, 200)
-  assert panel.get_offset() == 0
-
-  panel.set_offset(-150)
-  panel.update(bounds, 200)
-  assert panel.get_offset() == -100

openpilot/selfdrive/ui/sunnypilot/mici/widgets/scroll_panel_sp.py

@@ -1,33 +0,0 @@
-"""
-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 pyray as rl
-from openpilot.system.ui.lib.application import MouseEvent
-from openpilot.system.ui.lib.scroll_panel2 import GuiScrollPanel2, ScrollState
-
-
-class GuiScrollPanel2SP(GuiScrollPanel2):
-  """Scroll panel behavior for nested Mici pagers."""
-
-  def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
-    super().__init__(horizontal, handle_out_of_bounds=handle_out_of_bounds)
-
-  def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
-                          content_size: float) -> None:
-    state_before_update = self._state
-    super()._handle_mouse_event(mouse_event, bounds, bounds_size, content_size)
-
-    if self._state == ScrollState.MANUAL_SCROLL and state_before_update == ScrollState.PRESSED and \
-       self._initial_click_event is not None:
-      drag_x = abs(mouse_event.pos.x - self._initial_click_event.pos.x)
-      drag_y = abs(mouse_event.pos.y - self._initial_click_event.pos.y)
-      primary_drag = drag_x if self._horizontal else drag_y
-      cross_drag = drag_y if self._horizontal else drag_x
-      if cross_drag > primary_drag:
-        self._state = ScrollState.STEADY
-        self._velocity = 0.0
-        self._velocity_buffer.clear()

openpilot/selfdrive/ui/sunnypilot/mici/widgets/scroller_sp.py

@@ -1,16 +0,0 @@
-"""
-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.system.ui.widgets.scroller import Scroller
-from openpilot.selfdrive.ui.sunnypilot.mici.widgets.scroll_panel_sp import GuiScrollPanel2SP
-
-
-class ScrollerSP(Scroller):
-  def __init__(self, **kwargs):
-    super().__init__(**kwargs)
-    inner = self._scroller
-    inner.scroll_panel = GuiScrollPanel2SP(inner._horizontal, handle_out_of_bounds=not inner._snap_items)

openpilot/selfdrive/ui/ui.py

@@ -10,9 +10,6 @@ from openpilot.selfdrive.ui.layouts.main import MainLayout
 from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
 from openpilot.selfdrive.ui.ui_state import ui_state
 
-if gui_app.sunnypilot_ui():
-  from openpilot.selfdrive.ui.sunnypilot.mici.layouts.main import MiciMainLayoutSP as MiciMainLayout
-
 BIG_UI = gui_app.big_ui()
 
 

openpilot/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

@@ -115,7 +115,7 @@ class IntelligentCruiseButtonManagement:
     self.is_ready = ready and not button_pressed
 
   def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
-    if self.CP_SP.pcmCruiseSpeed or not self.CP_SP.intelligentCruiseButtonManagementAvailable:
+    if self.CP_SP.pcmCruiseSpeed:
       return
 
     self.is_metric = is_metric

openpilot/sunnypilot/selfdrive/car/interfaces.py

@@ -136,9 +136,6 @@ def initialize_params(params) -> list[dict[str, Any]]:
   keys.extend([
     "ToyotaEnforceStockLongitudinal",
     "ToyotaStopAndGoHack",
-    "ToyotaEnhancedBsm",
-    "ToyotaAutoHold",
-    "ToyotaVirtualCruiseSpeed",
   ])
 
   return [{k: params.get(k, return_default=True)} for k in keys]

openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py

@@ -1,14 +1,10 @@
 import pytest
 
-from opendbc.can.parser import CANParser
-from opendbc.car import create_button_events
 from opendbc.car.structs import car
-from opendbc.car.toyota.carstate import get_virtual_cruise_button, VIRTUAL_CRUISE_BUTTONS
-from openpilot.cereal import custom
 from openpilot.common.constants import CV
 from openpilot.common.parameterized import parameterized_class
 from openpilot.common.params import Params
-from openpilot.selfdrive.car.cruise import TOYOTA_VIRTUAL_CRUISE_LONG_PRESS, VCruiseHelper, V_CRUISE_INITIAL, V_CRUISE_UNSET
+from openpilot.selfdrive.car.cruise import V_CRUISE_INITIAL
 from openpilot.selfdrive.car.tests.test_cruise_speed import TestVCruiseHelper
 
 ButtonEvent = car.CarState.ButtonEvent
@@ -152,290 +148,3 @@ class TestCustomAccIncrements(TestVCruiseHelper):
     initial_speed = self.v_cruise_helper.v_cruise_kph
     self.press_button_long(ButtonType.accelCruise)
     assert self.v_cruise_helper.v_cruise_kph == initial_speed + 10  # Should fallback to 10
-
-
-class TestToyotaVirtualCruiseSpeed:
-  def setup_method(self):
-    self.params = Params()
-    self.params.put_bool("CustomAccIncrementsEnabled", True, block=True)
-    self.params.put("CustomAccShortPressIncrement", 5, block=True)
-    self.params.put("CustomAccLongPressIncrement", 5, block=True)
-
-    CP = car.CarParams(brand="toyota", pcmCruise=True, openpilotLongitudinalControl=True)
-    CP_SP = custom.CarParamsSP(pcmCruiseSpeed=False)
-    self.v_cruise_helper = VCruiseHelper(CP, CP_SP)
-    self.v_cruise_helper.read_custom_set_speed_params()
-    self.route_parser = CANParser("toyota_nodsu_pt_generated", [("CLUTCH", 16)], 0)
-    self.route_button = 0
-
-  @staticmethod
-  def car_state(canonical_kph, cluster_kph, *, available=True, standstill=False, gas_pressed=False, v_ego_kph=0., button_events=None):
-    CS = car.CarState(
-      gasPressed=gas_pressed,
-      vEgo=v_ego_kph * CV.KPH_TO_MS,
-      cruiseState={
-        "available": available,
-        "speed": canonical_kph * CV.KPH_TO_MS,
-        "speedCluster": cluster_kph * CV.KPH_TO_MS,
-        "standstill": standstill,
-      },
-    )
-    CS.buttonEvents = button_events or []
-    return CS
-
-  def seed_enabled(self, canonical_kph, cluster_kph, *, is_metric=True):
-    CS = self.car_state(canonical_kph, cluster_kph)
-    self.v_cruise_helper.update_v_cruise(CS, enabled=False, is_metric=is_metric)
-    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=is_metric)
-    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=is_metric)
-    assert self.v_cruise_helper.v_cruise_kph == canonical_kph
-    assert self.v_cruise_helper.v_cruise_cluster_kph == cluster_kph
-
-  def press(self, button_type, canonical_kph, cluster_kph, hold_frames=0, *, standstill=False, gas_pressed=False, v_ego_kph=0., is_metric=True):
-    pressed = [ButtonEvent(type=button_type, pressed=True)]
-    self.v_cruise_helper.update_v_cruise(
-      self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph, button_events=pressed),
-      enabled=True, is_metric=is_metric,
-    )
-    for _ in range(hold_frames):
-      self.v_cruise_helper.update_v_cruise(
-        self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph),
-        enabled=True, is_metric=is_metric,
-      )
-    released = [ButtonEvent(type=button_type, pressed=False)]
-    self.v_cruise_helper.update_v_cruise(
-      self.car_state(canonical_kph, cluster_kph, standstill=standstill, gas_pressed=gas_pressed, v_ego_kph=v_ego_kph, button_events=released),
-      enabled=True, is_metric=is_metric,
-    )
-
-  def set_increments(self, short_increment, long_increment):
-    self.params.put("CustomAccShortPressIncrement", short_increment, block=True)
-    self.params.put("CustomAccLongPressIncrement", long_increment, block=True)
-    self.v_cruise_helper.read_custom_set_speed_params()
-
-  def route_button_events(self, payload):
-    self.route_parser.update((1, [(0x361, bytes.fromhex(payload), 0)]))
-    current = get_virtual_cruise_button(
-      self.route_parser.vl["CLUTCH"]["CRUISE_RES"],
-      self.route_parser.vl["CLUTCH"]["CRUISE_SET"],
-    )
-    events = create_button_events(current, self.route_button, VIRTUAL_CRUISE_BUTTONS)
-    self.route_button = current
-    return events
-
-  def test_short_press_rounds_display_target_and_preserves_offset(self):
-    self.seed_enabled(27, 31)
-    self.press(ButtonType.accelCruise, 28, 32)
-
-    assert self.v_cruise_helper.v_cruise_kph == 31
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
-
-  def test_decel_at_display_minimum_does_not_increase_target(self):
-    self.seed_enabled(26, 30)
-    self.press(ButtonType.decelCruise, 25, 29)
-
-    assert self.v_cruise_helper.v_cruise_kph == 26
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 30
-
-  @pytest.mark.parametrize("hold_frames", (52, TOYOTA_VIRTUAL_CRUISE_LONG_PRESS - 1))
-  def test_route_length_short_press_is_not_a_long_press(self, hold_frames):
-    self.set_increments(short_increment=2, long_increment=5)
-    self.seed_enabled(27, 31)
-    self.press(ButtonType.accelCruise, 28, 32, hold_frames=hold_frames)
-
-    assert self.v_cruise_helper.v_cruise_kph == 29
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 33
-
-  def test_toyota_long_press_uses_route_validated_cadence_and_suppresses_release(self):
-    self.set_increments(short_increment=2, long_increment=5)
-    self.seed_enabled(27, 31)
-
-    pressed = [ButtonEvent(type=ButtonType.accelCruise, pressed=True)]
-    self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=pressed), enabled=True, is_metric=True)
-    for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS):
-      self.v_cruise_helper.update_v_cruise(self.car_state(31, 35), enabled=True, is_metric=True)
-
-    assert self.v_cruise_helper.v_cruise_kph == 31
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
-
-    released = [ButtonEvent(type=ButtonType.accelCruise, pressed=False)]
-    self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=released), enabled=True, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == 31
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
-
-  def test_route_4_32_second_hold_repeats_six_times(self):
-    self.seed_enabled(26, 30)
-    self.press(ButtonType.accelCruise, 30, 34, hold_frames=432)
-
-    assert self.v_cruise_helper.v_cruise_kph == 56
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 60
-
-  def test_maximum_boundary_caps_pair_and_preserves_offset(self):
-    self.seed_enabled(141, 145)
-    self.press(ButtonType.accelCruise, 142, 146)
-
-    assert self.v_cruise_helper.v_cruise_kph == 141
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 145
-
-    self.press(ButtonType.accelCruise, 143, 147)
-    assert self.v_cruise_helper.v_cruise_kph == 141
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 145
-
-  @pytest.mark.parametrize(("canonical_kph", "cluster_kph", "button_type"), (
-    (25, 29, ButtonType.decelCruise),
-    (141, 147, ButtonType.accelCruise),
-  ))
-  def test_out_of_range_raw_pair_is_not_moved_in_opposite_direction(self, canonical_kph, cluster_kph, button_type):
-    self.seed_enabled(canonical_kph, cluster_kph)
-    self.press(button_type, canonical_kph, cluster_kph)
-
-    assert self.v_cruise_helper.v_cruise_kph == canonical_kph
-    assert self.v_cruise_helper.v_cruise_cluster_kph == cluster_kph
-
-  def test_imperial_increment_preserves_canonical_cluster_pair(self):
-    self.seed_enabled(45, 50, is_metric=False)
-    self.press(ButtonType.accelCruise, 46, 51, is_metric=False)
-
-    assert self.v_cruise_helper.v_cruise_kph == 51
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 56
-
-  def test_engagement_button_held_does_not_change_target(self):
-    initial = self.car_state(27, 31)
-    self.v_cruise_helper.update_v_cruise(initial, enabled=False, is_metric=True)
-
-    pressed = [ButtonEvent(type=ButtonType.decelCruise, pressed=True)]
-    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=False, is_metric=True)
-    for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS + 10):
-      self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
-
-    released = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, button_events=released), enabled=True, is_metric=True)
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
-
-    assert self.v_cruise_helper.v_cruise_kph == 28
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 32
-
-  def test_delayed_pcm_target_seeds_before_software_ownership(self):
-    invalid = self.car_state(0, 0)
-    self.v_cruise_helper.update_v_cruise(invalid, enabled=False, is_metric=True)
-
-    release = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
-    for _ in range(4):
-      self.v_cruise_helper.update_v_cruise(self.car_state(0, 0, button_events=release), enabled=True, is_metric=True)
-      assert self.v_cruise_helper.v_cruise_kph == V_CRUISE_UNSET
-      assert self.v_cruise_helper.v_cruise_cluster_kph == V_CRUISE_UNSET
-
-    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31), enabled=True, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(27)
-    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(31)
-
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(27)
-    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(31)
-
-  def test_route_payload_short_press_drives_virtual_target(self):
-    self.seed_enabled(27, 31)
-
-    pressed = self.route_button_events("a61a0000561a1a81")
-    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=True, is_metric=True)
-    for _ in range(52):
-      self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
-
-    released = self.route_button_events("861a0000561b1a81")
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, button_events=released), enabled=True, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == 31
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 35
-
-  def test_prius_route_payload_short_set_drives_virtual_target(self):
-    self.seed_enabled(31, 35)
-
-    pressed = self.route_button_events("965f000056666585")
-    self.v_cruise_helper.update_v_cruise(self.car_state(31, 35, button_events=pressed), enabled=True, is_metric=True)
-    for _ in range(45):
-      self.v_cruise_helper.update_v_cruise(self.car_state(30, 34), enabled=True, is_metric=True)
-
-    released = self.route_button_events("865f000056666585")
-    self.v_cruise_helper.update_v_cruise(self.car_state(30, 34, button_events=released), enabled=True, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == 26
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 30
-
-  def test_prius_route_payload_standstill_res_does_not_change_target(self):
-    self.seed_enabled(27, 31)
-
-    pressed = self.route_button_events("a61b0000561c1c80")
-    self.v_cruise_helper.update_v_cruise(
-      self.car_state(27, 31, standstill=True, button_events=pressed), enabled=True, is_metric=True,
-    )
-    for _ in range(TOYOTA_VIRTUAL_CRUISE_LONG_PRESS):
-      self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, standstill=True), enabled=True, is_metric=True)
-
-    released = self.route_button_events("865f000056666585")
-    self.v_cruise_helper.update_v_cruise(
-      self.car_state(27, 31, standstill=True, button_events=released), enabled=True, is_metric=True,
-    )
-    assert self.v_cruise_helper.v_cruise_kph == 27
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 31
-
-  def test_route_payload_disengage_mid_hold_clears_pending_action(self):
-    self.seed_enabled(27, 31)
-
-    pressed = self.route_button_events("a61a0000561a1a81")
-    self.v_cruise_helper.update_v_cruise(self.car_state(27, 31, button_events=pressed), enabled=True, is_metric=True)
-    for _ in range(30):
-      self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
-
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
-    released = self.route_button_events("861a0000561b1a81")
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32, available=False, button_events=released), enabled=False, is_metric=True)
-
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=True, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(28)
-    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(32)
-
-  def test_standstill_resume_does_not_change_target(self):
-    self.seed_enabled(27, 31)
-    self.press(ButtonType.accelCruise, 27, 31, standstill=True)
-
-    assert self.v_cruise_helper.v_cruise_kph == 27
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 31
-
-  def test_disengagement_discards_virtual_target_and_reseeds_raw_pair(self):
-    self.seed_enabled(27, 31)
-    self.press(ButtonType.accelCruise, 28, 32)
-    assert self.v_cruise_helper.v_cruise_kph == 31
-
-    raw = self.car_state(28, 32)
-    self.v_cruise_helper.update_v_cruise(raw, enabled=False, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(28)
-    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(32)
-
-    self.v_cruise_helper.update_v_cruise(raw, enabled=True, is_metric=True)
-    self.v_cruise_helper.update_v_cruise(raw, enabled=True, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(28)
-    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(32)
-
-  def test_unavailable_and_mads_handback_discard_virtual_target(self):
-    self.seed_enabled(27, 31)
-    self.press(ButtonType.accelCruise, 28, 32)
-    assert self.v_cruise_helper.v_cruise_kph == 31
-
-    self.v_cruise_helper.update_v_cruise(self.car_state(28, 32), enabled=False, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(28)
-    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(32)
-
-    self.v_cruise_helper.update_v_cruise(self.car_state(0, 0, available=False), enabled=False, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == V_CRUISE_UNSET
-    assert self.v_cruise_helper.v_cruise_cluster_kph == V_CRUISE_UNSET
-
-    self.v_cruise_helper.update_v_cruise(self.car_state(29, 33), enabled=False, is_metric=True)
-    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(29)
-    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(33)
-
-  def test_set_during_gas_override_clips_target_to_ego_speed(self):
-    self.seed_enabled(27, 31)
-    self.press(ButtonType.decelCruise, 26, 30, gas_pressed=True, v_ego_kph=50)
-
-    assert self.v_cruise_helper.v_cruise_kph == 50
-    assert self.v_cruise_helper.v_cruise_cluster_kph == 54

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

@@ -1,241 +0,0 @@
-import math
-
-from opendbc.car.interfaces import ACCEL_MAX
-from openpilot.cereal import custom
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot import get_sanitize_int_param
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL, LEAD_DROPOUT_COAST_TIME, MPC_DECEL_JERK_COST_MULTIPLIER,
-  MPC_DECEL_JERK_LONG_TREND_FRAMES, MPC_DECEL_JERK_LONG_TREND_RATE, MPC_DECEL_JERK_MAX_REQUIRED_DECEL,
-  MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MPC_DECEL_TREND_FRAMES,
-  PARAM_READ_INTERVAL, LEAD_RELEASE_CONFIRM_TIME, RADAR_STALE_TIMEOUT, SPEED_DEADBAND, VEGO_NOISE_TOLERANCE,
-  AccelProfile, profile_accel_max, sanitize_profile,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling, is_valid_context
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead_controller import LeadController
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
-
-AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
-
-
-class AccelController:
-  def __init__(self, CP, dt: float = DT_MDL):
-    if not math.isfinite(dt) or dt <= 0.0:
-      raise ValueError("dt must be finite and positive")
-
-    self.dt = dt
-    self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
-    self.lead_confirm_frames = max(LEAD_SAMPLE_FILTER_FRAMES, math.ceil(LEAD_RELEASE_CONFIRM_TIME / dt))
-    self.dropout_frames = max(self.lead_confirm_frames, math.ceil(LEAD_DROPOUT_COAST_TIME / dt))
-    self.dropout_release_frames = max(1, self.dropout_frames - self.lead_confirm_frames)
-    self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
-    self.params = Params()
-    self.available = bool(CP.openpilotLongitudinalControl)
-    self.enabled = False
-    self.profile = AccelProfile.normal
-    self._param_read_frames = max(1, int(round(PARAM_READ_INTERVAL / dt)))
-    self._param_frame = 0
-    self._jerk_smoothing_blocked = False
-    self._required_decel_samples: list[float] = []
-    self._required_decel_long_samples: list[float] = []
-    self._required_decel_lead = -1
-    self._required_decel_lead_track_id = -1
-    self._lead_trend_warmup = False
-
-    self.lead_controller = LeadController()
-    self._stale_frames = 0
-    self._cruise_accel_limited = False
-
-    self.is_active = False
-    self.output_v_target = 0.0
-    self.mpc_accel_max: tuple[float, ...] | None = None
-    self.cruise_accel_max: float | None = None
-    self.state = AccelControllerState.inactive
-    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.required_decel = 0.0
-
-  @property
-  def is_enabled(self) -> bool:
-    return self.available and self.enabled
-
-  @property
-  def launching(self) -> bool:
-    return self.lead_controller.launching
-
-  @property
-  def departure_launching(self) -> bool:
-    return self.lead_controller.departure_launching
-
-  def update_params(self) -> None:
-    if self._param_frame % self._param_read_frames == 0:
-      self.enabled = self.params.get_bool("AccelPersonalityEnabled")
-      self.profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
-    self._param_frame += 1
-
-  def reset(self) -> None:
-    self.lead_controller = LeadController()
-    self._stale_frames = 0
-    self._jerk_smoothing_blocked = False
-    self._required_decel_samples.clear()
-    self._required_decel_long_samples.clear()
-    self._required_decel_lead = self._required_decel_lead_track_id = -1
-    self._lead_trend_warmup = False
-    self._cruise_accel_limited = False
-    self.is_active = False
-    self.output_v_target = 0.0
-    self.mpc_accel_max = None
-    self.cruise_accel_max = None
-    self.state = AccelControllerState.inactive
-    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.required_decel = 0.0
-
-  def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, follow_personality, acc_selected: bool,
-             engaged: bool, cruise_initialized: bool, stock_accel_max: float, radar_fresh: bool = True,
-             previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0,
-             previous_plan_accel: float = 0.0) -> None:
-    self.profile = sanitize_profile(self.profile)
-    sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
-    profile_max_accel = profile_accel_max(self.profile, sanitized_v_ego)
-    stock_accel_max = float(stock_accel_max)
-    positive_accel_max = (max(0.0, min(profile_max_accel, stock_accel_max, ACCEL_MAX))
-                          if math.isfinite(profile_max_accel) and math.isfinite(stock_accel_max) else math.nan)
-    planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
-    valid_context = is_valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self.delay,
-                                     engaged, cruise_initialized)
-    enabled_context = valid_context and self.is_enabled and bool(acc_selected)
-
-    lead_plan = None
-    if enabled_context:
-      if radar_fresh:
-        self._stale_frames = 0
-        lead_plan = calculate_lead_plan(radar_state, sanitized_v_ego, a_ego, self.delay, self.profile, follow_personality)
-      else:
-        self._stale_frames += 1
-        new_acc_handoff = (self.lead_controller.target_speed is None and previous_mpc_source == LongitudinalPlanSource.e2e
-                           and math.isfinite(previous_plan_accel))
-        if new_acc_handoff:
-          lead_plan = LeadPlan()
-
-      if lead_plan is not None:
-        self.lead_controller.update(lead_plan, base_speed, sanitized_v_ego, COMFORT_DECEL[self.profile], profile_max_accel, self.dt,
-                         self.lead_confirm_frames, self.dropout_frames, planner_speed, planner_accel,
-                         previous_mpc_source, previous_plan_accel)
-      stale_limit = self.dropout_frames if self.lead_controller.launching else self.radar_stale_frames
-      if not radar_fresh and self._stale_frames >= stale_limit and not self.lead_controller.stop_hold:
-        self.lead_controller.reset()
-    else:
-      self._stale_frames = 0
-
-    active = enabled_context and (radar_fresh or self.lead_controller.target_speed is not None)
-    self.is_active = active
-    if not active:
-      self.lead_controller.reset()
-      self._cruise_accel_limited = False
-      self.output_v_target = base_speed
-      self.mpc_accel_max = None
-      self.cruise_accel_max = None
-      self.state = AccelControllerState.inactive
-      self.selected_lead = -1
-      self.selected_lead_track_id = -1
-      self.required_decel = 0.0
-      return
-
-    lead_controller = self.lead_controller
-    self.output_v_target = lead_controller.target_speed if lead_controller.target_speed is not None else base_speed
-    self.selected_lead = lead_controller.selected_lead
-    self.selected_lead_track_id = lead_controller.selected_lead_track_id
-    self.required_decel = lead_controller.required_decel
-
-    recovery_limit_active = lead_controller.lead_recovery and lead_controller.recovery_accel_limit is not None and not lead_controller.e2e_braking_handoff
-    lead_recovery_accel = lead_controller.lead_recovery and planner_accel >= 0.0
-    dropout_coast = lead_controller.should_coast_on_dropout
-    profile_limit_active = (not lead_controller.stop_hold and (not lead_controller.has_lead or lead_recovery_accel
-                            or lead_controller.departure_launching or lead_controller.leadless_departure)
-                            and not lead_controller.e2e_braking_handoff)
-    if recovery_limit_active:
-      effective_accel_max = min(positive_accel_max, lead_controller.recovery_accel_limit)
-    elif profile_limit_active:
-      effective_accel_max = positive_accel_max
-    else:
-      effective_accel_max = math.inf
-    self.mpc_accel_max = (build_accel_ceiling(effective_accel_max, planner_accel)
-                          if recovery_limit_active or profile_limit_active else None)
-
-    lead_context = lead_controller.has_lead or math.isfinite(lead_controller.lead_speed_ceiling)
-    start_cruise_accel_limit = (lead_controller.target_speed is not None and not lead_controller.restricting and not lead_controller.releasing
-                                and lead_controller.has_lead and previous_mpc_source == LongitudinalPlanSource.cruise)
-    keep_cruise_accel_limit = (self._cruise_accel_limited and lead_context and not lead_controller.restricting and not lead_controller.releasing
-                               and not lead_controller.e2e_braking_handoff)
-    self._cruise_accel_limited = start_cruise_accel_limit or keep_cruise_accel_limit
-    if dropout_coast:
-      release_frame = max(0, lead_controller.lead_loss_frames - self.lead_confirm_frames)
-      self.cruise_accel_max = positive_accel_max * release_frame / self.dropout_release_frames
-    else:
-      self.cruise_accel_max = positive_accel_max if self._cruise_accel_limited else None
-
-    if lead_controller.stop_hold:
-      self.state = AccelControllerState.stopHold
-    elif lead_controller.restricting:
-      self.state = AccelControllerState.restrict
-    elif lead_controller.releasing:
-      self.state = AccelControllerState.release
-    elif lead_controller.target_speed >= base_speed - SPEED_DEADBAND:
-      self.state = AccelControllerState.free
-    else:
-      self.state = AccelControllerState.hold
-
-  def get_jerk_cost_multiplier(self, actuating: bool, prev_accel_constraint: bool, target_reduction: float,
-                               previous_mpc_failed: bool) -> float:
-    lead_restriction = (actuating and prev_accel_constraint and self.state == AccelControllerState.restrict and self.selected_lead >= 0
-      and not self.launching and target_reduction > 1e-6)
-    same_lead = self.selected_lead == self._required_decel_lead and self.selected_lead_track_id == self._required_decel_lead_track_id
-    lead_changed = lead_restriction and self._required_decel_lead >= 0 and not same_lead
-    if lead_changed:
-      self._lead_trend_warmup = True
-    elif not lead_restriction:
-      self._lead_trend_warmup = False
-    if not lead_restriction or not same_lead or not math.isfinite(self.required_decel):
-      self._required_decel_samples.clear()
-      self._required_decel_long_samples.clear()
-    if lead_restriction and math.isfinite(self.required_decel):
-      self._required_decel_samples.append(self.required_decel)
-      if len(self._required_decel_samples) > MPC_DECEL_TREND_FRAMES:
-        self._required_decel_samples.pop(0)
-      self._required_decel_long_samples.append(self.required_decel)
-      if len(self._required_decel_long_samples) > MPC_DECEL_JERK_LONG_TREND_FRAMES:
-        self._required_decel_long_samples.pop(0)
-    self._required_decel_lead = self.selected_lead if lead_restriction else -1
-    self._required_decel_lead_track_id = self.selected_lead_track_id if lead_restriction else -1
-
-    history = self._required_decel_samples
-    history_ready = len(history) == MPC_DECEL_TREND_FRAMES
-    tightening_lead = (history_ready
-      and (history[-1] - history[0]) / (self.dt * (len(history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
-      and sum(after > before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
-    long_history = self._required_decel_long_samples
-    long_history_ready = len(long_history) == MPC_DECEL_JERK_LONG_TREND_FRAMES
-    sustained_tightening = (long_history_ready
-      and (long_history[-1] - long_history[0]) / (self.dt * (len(long_history) - 1)) > MPC_DECEL_JERK_LONG_TREND_RATE)
-    modest_decel = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
-      and 0.0 < self.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
-    smoothing_eligible = (modest_decel and (not self._lead_trend_warmup or history_ready)
-      and not tightening_lead and not sustained_tightening)
-    if history_ready:
-      self._lead_trend_warmup = False
-    if previous_mpc_failed or (lead_restriction and not self._jerk_smoothing_blocked
-                               and (not modest_decel or tightening_lead or sustained_tightening)):
-      self._jerk_smoothing_blocked = True
-    elif not lead_restriction:
-      self._jerk_smoothing_blocked = False
-    return MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not self._jerk_smoothing_blocked else 1.0
-
-  def update_should_stop(self, should_stop: bool, departure_authorized: bool = True) -> bool:
-    if not self.is_active:
-      return should_stop
-    if self.lead_controller.departure_launching and departure_authorized:
-      return False
-    return should_stop or self.lead_controller.stop_hold

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py

@@ -1,72 +0,0 @@
-import math
-
-import numpy as np
-
-from openpilot.cereal import custom
-
-
-AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
-ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())
-
-COMFORT_DECEL = {
-  AccelProfile.eco: 0.25,
-  AccelProfile.normal: 0.30,
-  AccelProfile.sport: 0.35,
-}
-
-ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
-ACCEL_PROFILE_MAX_V = {
-  AccelProfile.eco: [1.56, 1.30, 0.72, 0.32, 0.24],
-  AccelProfile.normal: [1.58, 1.51, 0.98, 0.53, 0.35],
-  AccelProfile.sport: [2.00, 1.91, 1.16, 0.73, 0.47],
-}
-
-BRAKING_ACCEL_THRESHOLD = -0.11
-
-LEAD_SAMPLE_FILTER_FRAMES = 5
-LEAD_RELEASE_CONFIRM_TIME = 0.50
-LEAD_DROPOUT_COAST_TIME = 1.50
-
-SPEED_DEADBAND = 0.15
-
-TARGET_RELEASE_SLEW = 8.75
-LAUNCH_TARGET_HEADROOM = 3.0
-LAUNCH_END_SPEED = 3.0
-
-LEAD_RECOVERY_HEADROOM = 1.25
-LEAD_RECOVERY_ACCEL_SLEW = 0.25
-LEAD_RECOVERY_DECEL_RATE = 0.50
-
-STOP_HOLD_EGO_SPEED = 0.30
-STOP_HOLD_SPEED_FLOOR = 0.15
-STOP_HOLD_EXIT_FRAMES = 4
-STOP_HOLD_CREEP_DISTANCE = 0.30
-STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
-DISTANCE_JUMP_CONFIRM_FRAMES = 3
-
-STOP_GAP_RESERVE = 0.75
-
-RADAR_STALE_TIMEOUT = 0.50
-MAX_LEAD_ACCEL_TAU = 10.0
-MIN_LEAD_SPEED = -1.0
-VEGO_NOISE_TOLERANCE = 0.10
-PARAM_READ_INTERVAL = 0.25
-ACCEL_LIMIT_HORIZON_JERK = 1.0
-
-MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
-MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
-MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35
-MPC_DECEL_JERK_LONG_TREND_FRAMES = 6
-MPC_DECEL_JERK_LONG_TREND_RATE = 0.02
-MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
-MPC_DECEL_TREND_FRAMES = 4
-
-
-def sanitize_profile(profile: int) -> int:
-  return profile if profile in ACCEL_PROFILES else AccelProfile.normal
-
-
-def profile_accel_max(profile: int, v_ego: float) -> float:
-  if not math.isfinite(v_ego):
-    return math.nan
-  return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[sanitize_profile(profile)]))

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py

@@ -1,22 +0,0 @@
-import math
-
-import numpy as np
-
-from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ACCEL_LIMIT_HORIZON_JERK, VEGO_NOISE_TOLERANCE
-
-
-def is_valid_context(base_speed: float, v_ego: float, a_ego: float, planner_speed: float, planner_accel: float, stock_accel_max: float,
-                     delay: float, engaged: bool, cruise_initialized: bool) -> bool:
-  values = (base_speed, v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, delay)
-  return (engaged and cruise_initialized and base_speed >= 0.0 and v_ego >= -VEGO_NOISE_TOLERANCE
-          and planner_speed >= 0.0 and stock_accel_max >= 0.0 and delay >= 0.0 and all(math.isfinite(value) for value in values))
-
-
-def build_accel_ceiling(limit: float, planner_accel: float) -> tuple[float, ...] | None:
-  if limit >= ACCEL_MAX - 1e-9:
-    return None
-  a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
-  ceiling = np.clip(np.maximum(limit, a0 - ACCEL_LIMIT_HORIZON_JERK * T_IDXS), 0.0, ACCEL_MAX)
-  return tuple(float(value) for value in ceiling)

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py

@@ -1,134 +0,0 @@
-"""
-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 math
-from typing import NamedTuple
-
-import numpy as np
-
-from openpilot.cereal import log
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
-  LongitudinalMpc, STOP_DISTANCE, T_IDXS, get_T_FOLLOW, get_stopped_equivalence_factor,
-)
-from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  COMFORT_DECEL, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, STOP_GAP_RESERVE, STOP_HOLD_SPEED_FLOOR, sanitize_profile,
-)
-
-
-class LeadPlan(NamedTuple):
-  speed_ceiling: float = math.inf
-  selected_lead: int = -1
-  selected_lead_track_id: int = -1
-  selected_lead_speed: float = math.inf
-  selected_lead_accel: float = 0.0
-  departure_lead: int = -1
-  departure_lead_track_id: int = -1
-  departure_lead_speed: float = math.inf
-  departure_lead_raw_speed: float = math.inf
-  departure_lead_distance: float = math.inf
-  departure_lead_separation: float = math.inf
-  departure_speed_ceiling: float = math.inf
-  closing_speed: float = 0.0
-  required_decel: float = 0.0
-  has_nearly_stopped_lead: bool = False
-  lead_status: bool = False
-
-
-def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]:
-  if a_ego < 0.0:
-    stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0
-    if stop_time <= delay:
-      distance = -v_ego**2 / (2.0 * a_ego) if v_ego > 0.0 else 0.0
-      return distance, 0.0
-  return max(v_ego * delay + 0.5 * a_ego * delay**2, 0.0), max(v_ego + a_ego * delay, 0.0)
-
-
-def _lead_values(lead) -> tuple[float, float, float, float, float] | None:
-  if not lead.present:
-    return None
-  d_rel, v_lead = float(lead.dRel), float(lead.vLeadK)
-  if not math.isfinite(d_rel) or d_rel < 0.0 or not math.isfinite(v_lead) or v_lead < MIN_LEAD_SPEED:
-    return None
-
-  a_lead = float(lead.aLeadK)
-  if not math.isfinite(a_lead):
-    a_lead = 0.0
-  a_lead_tau = float(lead.aLeadTau)
-  if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
-    a_lead_tau = _LEAD_ACCEL_TAU
-  raw_v_lead = float(getattr(lead, "vLead", v_lead))
-  if not math.isfinite(raw_v_lead):
-    raw_v_lead = 0.0
-  return d_rel, max(v_lead, 0.0), max(raw_v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
-
-
-def calculate_lead_plan(radar_state, v_ego: float, a_ego: float, delay: float, profile: int,
-                        follow_personality=log.LongitudinalPersonality.standard) -> LeadPlan:
-  if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
-    return LeadPlan()
-
-  leads = (radar_state.leadOne, radar_state.leadTwo)
-  lead_status = any(lead.present for lead in leads)
-  t_follow = get_T_FOLLOW(follow_personality)
-  if not math.isfinite(t_follow) or t_follow < 0.0:
-    return LeadPlan(lead_status=lead_status)
-
-  profile = sanitize_profile(profile)
-  x_ego, v_ego_delay = _project_ego(v_ego, a_ego, delay)
-  comfort_decel = COMFORT_DECEL[profile]
-  candidates: list[LeadPlan] = []
-  departure_candidates: list[tuple[float, LeadPlan]] = []
-
-  for lead_index, lead in enumerate(leads):
-    values = _lead_values(lead)
-    if values is None:
-      continue
-
-    d_rel, v_lead, raw_v_lead, a_lead, a_lead_tau = values
-    lead_xv = LongitudinalMpc.extrapolate_lead(d_rel, v_lead, a_lead, a_lead_tau)
-    x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
-    v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
-    safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
-    closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
-    required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap)
-    usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0)
-    speed_ceiling = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
-    departure_speed_ceiling = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_gap)
-    separation = x_lead - x_ego
-    departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
-
-    finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, speed_ceiling, departure_speed_ceiling, departure_distance)
-    if (not all(math.isfinite(value) and value >= 0.0 for value in finite_values) or math.isnan(required_decel)
-        or required_decel < 0.0 or not math.isfinite(separation)):
-      continue
-
-    track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1
-    candidate = LeadPlan(
-      speed_ceiling=speed_ceiling, selected_lead=lead_index, selected_lead_track_id=track_id,
-      selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead, departure_lead=lead_index,
-      departure_lead_track_id=track_id, departure_lead_speed=v_lead_delay, departure_lead_raw_speed=raw_v_lead,
-      departure_lead_distance=d_rel, departure_lead_separation=separation,
-      departure_speed_ceiling=departure_speed_ceiling, closing_speed=closing_speed, required_decel=required_decel,
-      has_nearly_stopped_lead=v_lead_delay < STOP_HOLD_SPEED_FLOOR, lead_status=lead_status,
-    )
-    candidates.append(candidate)
-    departure_candidates.append((departure_distance, candidate))
-
-  if not candidates:
-    return LeadPlan(lead_status=lead_status)
-
-  selected = min(candidates, key=lambda candidate: candidate.speed_ceiling)
-  departure = min(departure_candidates, key=lambda candidate: candidate[0])[1]
-  return selected._replace(
-    departure_lead=departure.selected_lead, departure_lead_track_id=departure.selected_lead_track_id,
-    departure_lead_speed=departure.selected_lead_speed, departure_lead_raw_speed=departure.departure_lead_raw_speed,
-    departure_lead_distance=departure.departure_lead_distance,
-    departure_lead_separation=departure.departure_lead_separation,
-    departure_speed_ceiling=departure.departure_speed_ceiling,
-    has_nearly_stopped_lead=departure.has_nearly_stopped_lead,
-  )

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead_controller.py

@@ -1,386 +0,0 @@
-"""
-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 math
-
-import numpy as np
-
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  BRAKING_ACCEL_THRESHOLD, LEAD_SAMPLE_FILTER_FRAMES, DISTANCE_JUMP_CONFIRM_FRAMES, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM,
-  LEAD_RECOVERY_ACCEL_SLEW, LEAD_RECOVERY_HEADROOM, LEAD_RECOVERY_DECEL_RATE, SPEED_DEADBAND, STOP_HOLD_CREEP_DISTANCE,
-  STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_MAX_LEAD_DISTANCE, STOP_HOLD_SPEED_FLOOR, TARGET_RELEASE_SLEW,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
-
-
-def _median(samples: list[float]) -> float:
-  return sorted(samples)[len(samples) // 2]
-
-
-def _slew(current: float, target: float, rate: float, dt: float) -> float:
-  return float(np.clip(target, current - rate * dt, current + rate * dt))
-
-
-def _max_distance_step(lead_speed: float, dt: float) -> float:
-  return max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * max(lead_speed, 0.0) * dt)
-
-
-def _same_lead(first: int, first_track_id: int, second: int, second_track_id: int) -> bool:
-  if first < 0 or second < 0:
-    return False
-  if first_track_id >= 0 or second_track_id >= 0:
-    return first_track_id >= 0 and first_track_id == second_track_id
-  return first == second
-
-
-class LeadController:
-  def __init__(self) -> None:
-    self.lead_speed_samples = [math.inf] * LEAD_SAMPLE_FILTER_FRAMES
-    self.lead_accel_samples = [0.0] * LEAD_SAMPLE_FILTER_FRAMES
-
-    self.lead_speed_ceiling = math.inf
-    self.release_confirm_frames = 0
-    self.lead_loss_frames = 0
-    self._dropout_was_restricting = False
-    self._dropout_was_braking = False
-
-    self.target_speed: float | None = None
-    self.e2e_braking_handoff = False
-    self.lead_recovery = False
-    self.recovery_accel_limit: float | None = None
-
-    self.stop_hold = False
-    self.launching = False
-    self.departure_launching = False
-    self.leadless_departure = False
-    self.held_lead = -1
-    self.held_lead_track_id = -1
-    self.held_lead_trusted = False
-    self.departure_confirm_frames = 0
-    self.no_departure_lead_frames = 0
-    self.departure_distance_ref: float | None = None
-    self.last_departure_distance: float | None = None
-    self.pending_distance_jump: float | None = None
-    self.distance_jump_frames = 0
-    self.braking_for_lead = False
-    self._lead_frames = 0
-
-    self.restricting = False
-    self.releasing = False
-    self.has_lead = False
-    self.required_decel = 0.0
-    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.raw_speed_ceiling = math.inf
-
-  @property
-  def filtered_lead_speed(self) -> float:
-    return _median(self.lead_speed_samples)
-
-  @property
-  def filtered_lead_accel(self) -> float:
-    return _median(self.lead_accel_samples)
-
-  @property
-  def should_coast_on_dropout(self) -> bool:
-    return not self.has_lead and self._dropout_was_restricting and self._dropout_was_braking and math.isfinite(self.lead_speed_ceiling)
-
-  def reset(self) -> None:
-    self.__init__()
-
-  def _update_lead_bookkeeping(self, lead_plan: LeadPlan, was_restricting: bool) -> None:
-    self.has_lead = lead_plan.selected_lead >= 0
-    self.raw_speed_ceiling = lead_plan.speed_ceiling if self.has_lead else math.inf
-    if self.has_lead:
-      self.lead_loss_frames = 0
-      self._dropout_was_braking = False
-    else:
-      if self.lead_loss_frames == 0:
-        self._dropout_was_restricting = was_restricting
-      self.lead_loss_frames += 1
-    self.selected_lead = lead_plan.selected_lead
-    self.selected_lead_track_id = lead_plan.selected_lead_track_id if self.has_lead else -1
-
-  def _update_speed_sample(self, lead_plan: LeadPlan) -> None:
-    if not self.has_lead:
-      self.lead_speed_samples.append(math.inf)
-      self.lead_speed_samples.pop(0)
-      self.lead_accel_samples.append(0.0)
-      self.lead_accel_samples.pop(0)
-      return
-    if self.raw_speed_ceiling <= self.lead_speed_ceiling + 1e-9:
-      self.lead_speed_samples.append(lead_plan.selected_lead_speed)
-      self.lead_speed_samples.pop(0)
-      self.lead_accel_samples.append(lead_plan.selected_lead_accel)
-      self.lead_accel_samples.pop(0)
-
-  def _update_speed_ceiling(self, lead_confirm_frames: int, dropout_frames: int) -> None:
-    candidate = self.raw_speed_ceiling
-    if candidate <= self.lead_speed_ceiling:
-      self.lead_speed_ceiling = candidate
-      self.release_confirm_frames = 0
-      return
-
-    if not self.has_lead:
-      hold_frames = dropout_frames if self._dropout_was_restricting else lead_confirm_frames
-      if self.lead_loss_frames <= hold_frames:
-        return
-      self.lead_speed_ceiling = candidate
-      self.release_confirm_frames = 0
-      return
-
-    if candidate >= self.lead_speed_ceiling + SPEED_DEADBAND:
-      self.release_confirm_frames += 1
-    else:
-      self.release_confirm_frames = 0
-    if self.release_confirm_frames > lead_confirm_frames:
-      self.lead_speed_ceiling = candidate
-      self.release_confirm_frames = 0
-
-  def _guarded_distance(self, raw: float, lead_speed: float, dt: float) -> float:
-    if self.last_departure_distance is not None:
-      delta = raw - self.last_departure_distance
-      if abs(delta) > _max_distance_step(lead_speed, dt):
-        consistent = self.pending_distance_jump is not None and delta * self.pending_distance_jump > 0.0
-        self.distance_jump_frames = self.distance_jump_frames + 1 if consistent else 1
-        self.pending_distance_jump = delta
-        if self.distance_jump_frames >= DISTANCE_JUMP_CONFIRM_FRAMES:
-          self.pending_distance_jump = None
-          self.distance_jump_frames = 0
-          self.departure_confirm_frames = 0
-          if abs(delta) >= STOP_HOLD_CREEP_DISTANCE:
-            self.held_lead_trusted = False
-        else:
-          raw = self.last_departure_distance
-      else:
-        self.pending_distance_jump = None
-        self.distance_jump_frames = 0
-    self.last_departure_distance = raw
-    return raw
-
-  def _reset_distance_guard(self) -> None:
-    self.last_departure_distance = self.pending_distance_jump = None
-    self.distance_jump_frames = 0
-
-  def _reset_departure_confirmation(self) -> None:
-    self.departure_confirm_frames = 0
-    self.departure_distance_ref = None
-    self.no_departure_lead_frames = 0
-
-  def _set_held_lead(self, lead_plan: LeadPlan, replacement: bool = False) -> None:
-    self.held_lead = lead_plan.departure_lead
-    self.held_lead_track_id = lead_plan.departure_lead_track_id
-    self.held_lead_trusted = not replacement and self.held_lead_track_id >= 0
-    self._reset_distance_guard()
-    if self.held_lead >= 0:
-      self.last_departure_distance = lead_plan.departure_lead_distance
-    self._reset_departure_confirmation()
-
-  def _update_stop_hold(self, lead_plan: LeadPlan, v_ego: float, base_speed: float, dt: float, lead_confirm_frames: int) -> bool:
-    if self.stop_hold:
-      has_departure_lead = _same_lead(self.held_lead, self.held_lead_track_id,
-                                      lead_plan.departure_lead, lead_plan.departure_lead_track_id)
-      if lead_plan.departure_lead >= 0 and not has_departure_lead:
-        continuous_vision_lead = (self.held_lead_track_id < 0 and lead_plan.departure_lead_track_id < 0
-                                  and self.last_departure_distance is not None
-                                  and abs(lead_plan.departure_lead_distance - self.last_departure_distance)
-                                  <= _max_distance_step(lead_plan.departure_lead_speed, dt))
-        if continuous_vision_lead:
-          self.held_lead = lead_plan.departure_lead
-          self.held_lead_trusted = False
-        else:
-          self._set_held_lead(lead_plan, replacement=True)
-        has_departure_lead = True
-      if has_departure_lead or lead_plan.lead_status:
-        self.no_departure_lead_frames = 0
-      else:
-        self.no_departure_lead_frames += 1
-      lead_speed = lead_plan.departure_lead_speed if has_departure_lead else 0.0
-      raw_lead_speed = lead_plan.departure_lead_raw_speed if has_departure_lead else 0.0
-      evidence = has_departure_lead and min(lead_speed, raw_lead_speed) > STOP_HOLD_SPEED_FLOOR
-      distance = None
-      if has_departure_lead:
-        distance = self._guarded_distance(lead_plan.departure_lead_distance, lead_speed, dt)
-
-      if evidence:
-        if self.departure_confirm_frames == 0:
-          self.departure_distance_ref = distance
-        self.departure_confirm_frames += 1
-      else:
-        self.departure_confirm_frames = 0
-        self.departure_distance_ref = None
-        if not has_departure_lead:
-          self.held_lead_trusted = False
-          self._reset_distance_guard()
-
-      growth = 0.0
-      if self.departure_confirm_frames > 0 and self.departure_distance_ref is not None and distance is not None:
-        growth = distance - self.departure_distance_ref
-
-      dwell_ready = self.departure_confirm_frames >= STOP_HOLD_EXIT_FRAMES
-      departing_with_lead = has_departure_lead and dwell_ready and growth + 1e-9 >= STOP_HOLD_CREEP_DISTANCE
-      departing_no_lead = (not lead_plan.lead_status and self.no_departure_lead_frames >= lead_confirm_frames
-                           and base_speed > STOP_HOLD_SPEED_FLOOR)
-
-      if departing_with_lead or departing_no_lead:
-        trusted_departure = departing_with_lead and self.held_lead_trusted
-        self.stop_hold = False
-        self.launching = True
-        self.departure_launching = trusted_departure
-        self.leadless_departure = not trusted_departure
-        self.held_lead = self.held_lead_track_id = -1
-        self.held_lead_trusted = False
-        self._reset_departure_confirmation()
-        self.target_speed = min(v_ego, base_speed)
-      else:
-        self.target_speed = 0.0
-      return self.stop_hold
-
-    departure_separation = lead_plan.departure_lead_separation if lead_plan.departure_lead >= 0 else math.inf
-    stopped_lead_hold = lead_plan.has_nearly_stopped_lead and (
-      lead_plan.departure_speed_ceiling < STOP_HOLD_SPEED_FLOOR
-      or (self.braking_for_lead and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE)
-    )
-    retained_stop_hold = not self.has_lead and math.isfinite(self.lead_speed_ceiling) and self.lead_speed_ceiling < STOP_HOLD_SPEED_FLOOR
-    if not self.launching and v_ego < STOP_HOLD_EGO_SPEED and (
-      stopped_lead_hold or retained_stop_hold
-    ):
-      self.stop_hold = True
-      self._set_held_lead(lead_plan)
-      self.launching = self.departure_launching = self.leadless_departure = False
-      self.lead_recovery = False
-      self.recovery_accel_limit = None
-      self.target_speed = 0.0
-      return True
-
-    return False
-
-  def _update_launch(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, dt: float, lead_confirm_frames: int) -> None:
-    if not self.launching:
-      return
-    if v_ego >= LAUNCH_END_SPEED:
-      self.launching = self.departure_launching = self.leadless_departure = False
-      return
-    invalid_lead = lead_plan.lead_status and not self.has_lead
-    renewed_stop = self.has_lead and lead_plan.has_nearly_stopped_lead
-    if invalid_lead or renewed_stop:
-      self.launching = self.departure_launching = self.leadless_departure = False
-      if v_ego < STOP_HOLD_EGO_SPEED:
-        self.stop_hold = True
-        self._set_held_lead(lead_plan)
-        self.target_speed = 0.0
-      return
-    self.releasing = True
-    if self.departure_launching:
-      self.target_speed = base_speed
-    elif self.leadless_departure:
-      self.target_speed = min(base_speed, max(self.target_speed or 0.0, v_ego) + TARGET_RELEASE_SLEW * dt)
-    elif not self.has_lead and self.lead_loss_frames >= lead_confirm_frames:
-      self.target_speed = min(base_speed, self.lead_speed_ceiling)
-    else:
-      launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
-      self.target_speed = min(base_speed, max(self.target_speed or 0.0, launch_target) + TARGET_RELEASE_SLEW * dt)
-
-  def _update_recovery(self, ceiling: float, base_speed: float, v_ego: float, profile_max_accel: float, dt: float) -> None:
-    if math.isfinite(self.filtered_lead_speed):
-      recovery_speed = min(base_speed, self.filtered_lead_speed + LEAD_RECOVERY_HEADROOM)
-      desired_accel_limit = float(np.clip(recovery_speed - v_ego, 0.0, profile_max_accel))
-    else:
-      desired_accel_limit = 0.0
-    if self.filtered_lead_accel < BRAKING_ACCEL_THRESHOLD:
-      # Avoid stacking the recovery slew on top of a braking lead.
-      desired_accel_limit = profile_max_accel
-    if self.recovery_accel_limit is None:
-      self.recovery_accel_limit = profile_max_accel
-    self.recovery_accel_limit = _slew(self.recovery_accel_limit, desired_accel_limit, LEAD_RECOVERY_ACCEL_SLEW, dt)
-
-    if ceiling <= self.target_speed - SPEED_DEADBAND:
-      self.target_speed = max(ceiling, self.target_speed - LEAD_RECOVERY_DECEL_RATE * dt)
-      self.restricting = True
-    elif ceiling >= self.target_speed + SPEED_DEADBAND:
-      self.target_speed = min(ceiling, self.target_speed + profile_max_accel * dt)
-      self.releasing = True
-
-  def _update_target_law(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, comfort_decel: float,
-                         profile_max_accel: float, dt: float, planner_speed: float,
-                         dropout_frames: int, was_restricting: bool) -> None:
-    ceiling = min(base_speed, self.lead_speed_ceiling)
-    new_recovery = self.has_lead and lead_plan.closing_speed <= 0.0
-    still_within_dropout = not self.has_lead and self.lead_loss_frames <= dropout_frames
-    self.lead_recovery = new_recovery or (self.lead_recovery and (self.has_lead or still_within_dropout))
-    if self.lead_recovery:
-      self._update_recovery(ceiling, base_speed, v_ego, profile_max_accel, dt)
-      return
-
-    self.recovery_accel_limit = None
-    synced_to_planner = ceiling < self.target_speed and planner_speed < self.target_speed
-    if synced_to_planner:
-      self.target_speed = max(planner_speed, self.target_speed - comfort_decel * dt)
-
-    if ceiling <= self.target_speed - SPEED_DEADBAND or (was_restricting and ceiling < self.target_speed):
-      if not synced_to_planner:
-        self.target_speed = max(ceiling, self.target_speed - comfort_decel * dt)
-      self.restricting = True
-    elif ceiling >= self.target_speed + SPEED_DEADBAND:
-      self.target_speed = min(ceiling, self.target_speed + TARGET_RELEASE_SLEW * dt)
-      self.releasing = True
-
-  def update(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, comfort_decel: float, profile_max_accel: float,
-             dt: float, lead_confirm_frames: int, dropout_frames: int, planner_speed: float,
-             planner_accel: float, previous_mpc_source, previous_plan_accel: float) -> float:
-    was_restricting = self.restricting
-    was_braking_for_lead = self.braking_for_lead
-    previous_lead = self.selected_lead
-    previous_track_id = self.selected_lead_track_id
-    holding_below_cruise = (not self.lead_recovery and self.target_speed is not None and math.isfinite(self.lead_speed_ceiling)
-                            and self.lead_speed_ceiling < base_speed - SPEED_DEADBAND
-                            and self.lead_speed_ceiling - v_ego < LEAD_RECOVERY_HEADROOM)
-    self.restricting = self.releasing = False
-    self._update_lead_bookkeeping(lead_plan, was_restricting or holding_below_cruise)
-    lead_changed = not _same_lead(previous_lead, previous_track_id, self.selected_lead, self.selected_lead_track_id)
-    if not self.has_lead or lead_changed:
-      self._lead_frames = 0
-    if self.braking_for_lead and lead_changed:
-      self.braking_for_lead = False
-    if not self.has_lead and self.lead_loss_frames == 1:
-      self._dropout_was_braking = was_braking_for_lead and planner_accel <= BRAKING_ACCEL_THRESHOLD
-    self._update_speed_ceiling(lead_confirm_frames, dropout_frames)
-    self._update_speed_sample(lead_plan)
-    self.required_decel = lead_plan.required_decel
-
-    self._lead_frames += int(self.has_lead)
-    if (self._lead_frames >= lead_confirm_frames and math.isfinite(self.lead_speed_ceiling)
-        and self.has_lead and planner_accel <= BRAKING_ACCEL_THRESHOLD):
-      self.braking_for_lead = True
-    elif not self.has_lead and self.lead_loss_frames >= lead_confirm_frames:
-      self.braking_for_lead = False
-
-    if self.target_speed is None:
-      self.target_speed = min(base_speed, v_ego)
-      e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
-      self.e2e_braking_handoff = e2e_handoff and math.isfinite(previous_plan_accel) and previous_plan_accel <= BRAKING_ACCEL_THRESHOLD
-      stop_hold_reason = lead_plan.has_nearly_stopped_lead or (math.isfinite(self.lead_speed_ceiling) and self.lead_speed_ceiling < STOP_HOLD_SPEED_FLOOR)
-      if v_ego < STOP_HOLD_EGO_SPEED and not stop_hold_reason:
-        self.target_speed = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
-        self.launching = True
-        self.departure_launching = False
-    elif self.e2e_braking_handoff and planner_accel > BRAKING_ACCEL_THRESHOLD:
-      self.e2e_braking_handoff = False
-
-    self.target_speed = min(self.target_speed, base_speed)
-
-    if self._update_stop_hold(lead_plan, v_ego, base_speed, dt, lead_confirm_frames):
-      return self.target_speed
-
-    self._update_launch(lead_plan, base_speed, v_ego, dt, lead_confirm_frames)
-    if self.launching or self.stop_hold:
-      return self.target_speed
-
-    self._update_target_law(lead_plan, base_speed, v_ego, comfort_decel, profile_max_accel, dt, planner_speed,
-                            dropout_frames, was_restricting)
-    return self.target_speed

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py

@@ -1,789 +0,0 @@
-import math
-from types import SimpleNamespace
-
-import numpy as np
-import pytest
-
-from openpilot.cereal import log
-from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
-  STOP_DISTANCE, T_IDXS, LongitudinalMpc, LongitudinalPlanSource, get_T_FOLLOW,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, ACCEL_PROFILES, LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL,
-  LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LEAD_RECOVERY_ACCEL_SLEW, LEAD_RECOVERY_HEADROOM, MPC_DECEL_JERK_COST_MULTIPLIER,
-  STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, TARGET_RELEASE_SLEW, AccelProfile, profile_accel_max, sanitize_profile,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import _project_ego, calculate_lead_plan
-
-
-def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5, radar_track_id=-1):
-  return SimpleNamespace(present=status, dRel=d_rel, vLead=v_lead_k, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau,
-                         radarTrackId=radar_track_id)
-
-
-def make_radar(lead_one=None, lead_two=None):
-  return SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead())
-
-
-def make_controller(delay=0.10):
-  return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True))
-
-
-def get_lead_plan(controller, radar_state, v_ego: float, a_ego: float, profile: int):
-  return calculate_lead_plan(radar_state, v_ego, a_ego, controller.delay, profile)
-
-
-def update(controller, radar_state=None, **overrides):
-  args = {
-    "base_speed": 25.0,
-    "v_ego": 10.0,
-    "a_ego": 0.0,
-    "profile": AccelProfile.normal,
-    "follow_personality": log.LongitudinalPersonality.standard,
-    "enabled": True,
-    "acc_selected": True,
-    "engaged": True,
-    "cruise_initialized": True,
-    "stock_accel_max": ACCEL_MAX,
-  }
-  args.update(overrides)
-  controller.profile = args.pop("profile")
-  controller.enabled = args.pop("enabled")
-  controller.update(radar_state or make_radar(), **args)
-  lead_controller = controller.lead_controller
-  return SimpleNamespace(
-    target_speed=controller.output_v_target, active=controller.is_active, launching=lead_controller.launching,
-    departure_launching=lead_controller.departure_launching, mpc_accel_max=controller.mpc_accel_max,
-    cruise_accel_max=controller.cruise_accel_max, state=controller.state, selected_lead=controller.selected_lead,
-    required_decel=controller.required_decel, stop_hold=lead_controller.stop_hold, lead_recovery=lead_controller.lead_recovery,
-    recovery_accel_limit=lead_controller.recovery_accel_limit, lead_speed_ceiling=lead_controller.lead_speed_ceiling, restricting=lead_controller.restricting,
-    releasing=lead_controller.releasing,
-  )
-
-
-def effective_accel_max(result):
-  return math.inf if result.mpc_accel_max is None else min(result.mpc_accel_max)
-
-
-def restrictive_radar():
-  return make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
-
-
-def enter_stop_hold(controller, *, base_speed=8.0, v_ego=0.0, frames=6, track_id=-1):
-  stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=track_id))
-  result = None
-  for _ in range(frames):
-    result = update(controller, stopped, base_speed=base_speed, v_ego=v_ego)
-  assert result.stop_hold
-  return result
-
-
-class TestProfiles:
-  @pytest.mark.parametrize("profile", ACCEL_PROFILES)
-  def test_lookup_interpolates_and_stays_inside_global_limit(self, profile):
-    for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True):
-      assert profile_accel_max(profile, speed) == expected
-
-    limits = [profile_accel_max(profile, speed) for speed in np.linspace(-1.0, 50.0, 201)]
-    assert all(0.0 <= limit <= ACCEL_MAX for limit in limits)
-    assert np.all(np.diff(limits) <= 0.0)
-
-  @pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP)
-  def test_profile_order_is_distinct(self, speed):
-    eco, normal, sport = (profile_accel_max(profile, speed) for profile in ACCEL_PROFILES)
-    assert eco < normal < sport
-
-  def test_invalid_profile_defaults_to_normal(self):
-    assert sanitize_profile(999) == AccelProfile.normal
-
-  def test_stock_limit_intersects_profile_before_mpc(self):
-    controller = make_controller()
-    results = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
-               for _ in range(controller.lead_confirm_frames)]
-    result = results[-1]
-    assert profile_accel_max(AccelProfile.sport, 10.0) > 0.30
-    assert effective_accel_max(result) == pytest.approx(0.30)
-    assert all(sample.mpc_accel_max is not None for sample in results)
-    assert all(max(sample.mpc_accel_max) <= 0.30 + 1e-9 for sample in results)
-
-  def test_runtime_profile_switch_applies_the_lookup_value_directly(self):
-    controller = make_controller()
-    sport = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
-             for _ in range(controller.lead_confirm_frames)][-1]
-    eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20)
-
-    assert effective_accel_max(sport) == pytest.approx(profile_accel_max(AccelProfile.sport, 10.0))
-    assert effective_accel_max(eco) == pytest.approx(profile_accel_max(AccelProfile.eco, 10.0))
-
-  def test_stock_limit_reduction_applies_immediately(self):
-    controller = make_controller()
-    for _ in range(controller.lead_confirm_frames):
-      update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
-
-    reduced = update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
-    assert effective_accel_max(reduced) == pytest.approx(0.30)
-    assert reduced.mpc_accel_max is not None
-    assert max(reduced.mpc_accel_max) <= 0.30 + 1e-9
-
-  def test_one_frame_stock_zero_does_not_poison_profile_recovery(self):
-    clean_controller, glitch_controller = make_controller(), make_controller()
-    for _ in range(clean_controller.lead_confirm_frames + 10):
-      clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
-      update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
-
-    limited = update(glitch_controller, v_ego=10.0, stock_accel_max=0.0)
-    clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
-    recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
-
-    assert effective_accel_max(limited) == 0.0
-    assert effective_accel_max(recovered) == pytest.approx(effective_accel_max(clean))
-
-  @pytest.mark.parametrize("radar_fresh", (True, False), ids=("dropout", "stale"))
-  def test_lead_recovery_ceiling_obeys_current_stock_limit(self, radar_fresh):
-    controller = make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    for _ in range(controller.lead_confirm_frames + 10):
-      update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-    for _ in range(20):
-      update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    assert controller.lead_controller.lead_recovery
-
-    limited = update(controller, stock_accel_max=0.0, radar_fresh=radar_fresh)
-    assert effective_accel_max(limited) == 0.0
-    assert limited.mpc_accel_max is not None
-    assert max(limited.mpc_accel_max) == 0.0
-
-  def test_exact_global_max_uses_stock_ceiling(self):
-    result = update(make_controller(), base_speed=8.0, v_ego=0.0, profile=AccelProfile.sport)
-    assert profile_accel_max(AccelProfile.sport, 0.0) == ACCEL_MAX
-    assert result.mpc_accel_max is None
-
-
-class TestBuildAccelCeiling:
-  @pytest.mark.parametrize("planner_accel", (-1.0, 0.0, 1.2, ACCEL_MAX))
-  def test_ceiling_is_finite_feasible_and_jerk_bounded(self, planner_accel):
-    limit = 0.50
-    ceiling = np.asarray(build_accel_ceiling(limit, planner_accel))
-    a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
-
-    assert ceiling.shape == T_IDXS.shape
-    assert np.all(np.isfinite(ceiling))
-    assert np.all((0.0 <= ceiling) & (ceiling <= ACCEL_MAX))
-    assert ceiling[0] + 1e-9 >= a0
-    assert np.all(ceiling + 1e-9 >= limit)
-    assert np.all(np.diff(ceiling) <= 1e-9)
-    assert np.all(-np.diff(ceiling) <= ACCEL_LIMIT_HORIZON_JERK * np.diff(T_IDXS) + 1e-9)
-
-  def test_zero_limit_remains_feasible_for_positive_x0(self):
-    ceiling = np.asarray(build_accel_ceiling(0.0, 0.8))
-    assert ceiling[0] == pytest.approx(0.8)
-    assert ceiling[-1] == pytest.approx(0.0)
-    assert np.all(ceiling >= 0.0)
-
-
-class TestMpcCeilingIntegration:
-  def test_inactive_controller_has_no_custom_ceiling(self):
-    controller = make_controller()
-    result = update(controller, enabled=False)
-    assert not result.active
-    assert result.mpc_accel_max is None
-    assert math.isinf(effective_accel_max(result))
-    assert controller.lead_controller.target_speed is None
-
-  def test_closing_on_a_lead_has_no_ceiling_regardless_of_planner_accel_sign(self):
-    # Leave the lead MPC unconstrained while ego is closing.
-    controller = make_controller()
-    radar = restrictive_radar()
-    for planner_accel in (-0.2, 0.2, -0.2):
-      result = update(controller, radar, v_ego=10.0, planner_accel=planner_accel)
-      assert result.mpc_accel_max is None
-    assert not controller.lead_controller.lead_recovery
-
-    bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False)
-    assert not bypassed.active and bypassed.mpc_accel_max is None
-
-  def test_eco_cruise_limit_remains_active_while_closing_on_a_lead(self):
-    controller = make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=80.0, v_lead_k=19.25))
-    result = update(controller, radar, base_speed=20.0, v_ego=20.0, profile=AccelProfile.eco, planner_accel=0.16,
-                    previous_mpc_source=LongitudinalPlanSource.cruise)
-
-    assert result.state == AccelControllerState.free
-    assert result.mpc_accel_max is None
-    assert result.cruise_accel_max == pytest.approx(profile_accel_max(AccelProfile.eco, 20.0))
-
-  def test_lead_cruise_limit_does_not_follow_mpc_source_or_accel_sign(self):
-    controller = make_controller()
-    expected = profile_accel_max(AccelProfile.eco, 20.0)
-    inputs = (
-      (LongitudinalPlanSource.cruise, 0.02, 19.9, 100),
-      (LongitudinalPlanSource.lead0, -0.02, 20.1, -1),
-      (LongitudinalPlanSource.cruise, -0.10, 19.9, 101),
-      (LongitudinalPlanSource.lead0, -0.12, 20.1, -1),
-      (LongitudinalPlanSource.lead1, 0.02, 20.1, -1),
-    )
-
-    for source, planner_accel, lead_speed, track_id in inputs:
-      radar = make_radar(make_lead(status=True, d_rel=150.0, v_lead_k=lead_speed, radar_track_id=track_id))
-      result = update(controller, radar, base_speed=20.0, v_ego=20.0, profile=AccelProfile.eco,
-                      planner_accel=planner_accel, previous_mpc_source=source)
-      assert result.cruise_accel_max == pytest.approx(expected)
-
-  def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self):
-    controller = make_controller()
-    for _ in range(controller.lead_confirm_frames):
-      update(controller, restrictive_radar(), v_ego=10.0, planner_accel=-0.2)
-
-    pulling_away = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=12.0))
-    result = update(controller, pulling_away, v_ego=10.0, planner_accel=0.2)
-
-    assert result.state == AccelControllerState.restrict
-    assert result.lead_recovery
-    assert result.mpc_accel_max is not None
-    profile_ceiling = profile_accel_max(AccelProfile.normal, 10.0)
-    assert profile_ceiling - LEAD_RECOVERY_ACCEL_SLEW * DT_MDL - 1e-9 <= effective_accel_max(result) <= profile_ceiling + 1e-9
-
-
-class TestLead:
-  def test_speed_ceiling_matches_stopping_energy_formula(self):
-    controller = make_controller()
-    lead = make_lead(status=True, d_rel=50.0, v_lead_k=8.0)
-    result = get_lead_plan(controller, make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    delay = controller.delay
-    lead_xv = LongitudinalMpc.extrapolate_lead(lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau)
-    x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
-    v_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
-    x_ego, _ = _project_ego(10.0, 0.0, delay)
-    safety_gap = max(x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW(log.LongitudinalPersonality.standard) * v_lead, 0.0)
-    usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0)
-    expected = v_lead + math.sqrt(2.0 * COMFORT_DECEL[AccelProfile.normal] * usable_gap)
-
-    assert result.speed_ceiling == pytest.approx(expected)
-
-  def test_profile_order_controls_approach_timing(self):
-    radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0))
-    ceilings = [get_lead_plan(make_controller(), radar, 10.0, 0.0, profile).speed_ceiling for profile in ACCEL_PROFILES]
-    assert ceilings[0] < ceilings[1] < ceilings[2]
-
-  @pytest.mark.parametrize("v_lead_k", (0.0, 8.0), ids=("stopped", "moving"))
-  def test_reserve_is_flat_not_speed_or_decel_scaled(self, v_lead_k):
-    lead = get_lead_plan(make_controller(), make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=v_lead_k)),
-                         5.0, 0.0, AccelProfile.normal)
-    comfort_decel = COMFORT_DECEL[AccelProfile.normal]
-    safety_gap = (lead.departure_speed_ceiling - lead.departure_lead_speed) ** 2 / (2.0 * comfort_decel)
-    usable_gap = (lead.speed_ceiling - lead.selected_lead_speed) ** 2 / (2.0 * comfort_decel)
-    assert safety_gap - usable_gap == pytest.approx(STOP_GAP_RESERVE)
-    assert lead.departure_speed_ceiling > lead.speed_ceiling
-
-  def test_more_restrictive_lead_is_selected(self):
-    radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0))
-    assert get_lead_plan(make_controller(), radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1
-
-  def test_departure_lead_prefers_nearer_lead_over_speed_governing_lead(self):
-    radar = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.2, radar_track_id=100),
-                       make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
-    result = get_lead_plan(make_controller(), radar, 0.0, 0.0, AccelProfile.normal)
-    assert result.selected_lead == 1
-    assert result.departure_lead == 0
-
-  @pytest.mark.parametrize("field,value", [
-    ("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0), ("radarTrackId", math.nan),
-  ])
-  def test_nonessential_invalid_lead_fields_are_sanitized(self, field, value):
-    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
-    setattr(lead, field, value)
-    result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert result.selected_lead == 0
-    assert math.isfinite(result.speed_ceiling)
-
-  @pytest.mark.parametrize("field,value", [("dRel", math.nan), ("dRel", -1.0), ("vLeadK", math.nan), ("vLeadK", -2.0)])
-  def test_invalid_geometry_is_not_used(self, field, value):
-    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
-    setattr(lead, field, value)
-    result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert result.selected_lead == -1
-    assert result.lead_status
-    assert math.isinf(result.speed_ceiling)
-
-  def test_raw_radar_is_never_mutated(self):
-    lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0, a_lead_k=-15.0, a_lead_tau=math.nan)
-    before = vars(lead).copy()
-    get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert vars(lead) == before
-
-
-class TestTargetAndSpeedCeiling:
-  def test_lead_recovery_accel_limit_ignores_a_two_frame_speed_jump(self):
-    clean_controller, noisy_controller = make_controller(), make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    for controller in (clean_controller, noisy_controller):
-      for _ in range(controller.lead_confirm_frames + 10):
-        update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-      for _ in range(20):
-        update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-
-    speed_jump = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=16.0))
-    for _ in range(2):
-      clean = update(clean_controller, radar, v_ego=8.0)
-      noisy = update(noisy_controller, speed_jump, v_ego=8.0)
-      assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean))
-      assert noisy.target_speed == pytest.approx(clean.target_speed)
-  def test_restriction_target_speed_is_rate_limited(self):
-    controller = make_controller()
-    targets = [update(controller, restrictive_radar()).target_speed for _ in range(15)]
-    max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
-
-    steps = -np.diff(targets[1:])
-    assert np.all(steps <= max_step + 1e-9)
-    assert controller.state == AccelControllerState.restrict
-    assert targets[-1] < targets[1]
-
-  @pytest.mark.parametrize("previous_mpc_source", (None, LongitudinalPlanSource.cruise, LongitudinalPlanSource.lead0))
-  def test_target_speed_syncs_down_to_planner_speed_regardless_of_previous_mpc_source(self, previous_mpc_source):
-    controller = make_controller()
-    for _ in range(15):
-      restricted = update(controller, restrictive_radar())
-    planner_speed = restricted.target_speed - 2.0
-
-    synced = update(controller, restrictive_radar(), previous_mpc_source=previous_mpc_source, planner_speed=planner_speed,
-                    planner_accel=-0.2)
-    assert synced.target_speed == pytest.approx(restricted.target_speed - COMFORT_DECEL[AccelProfile.normal] * DT_MDL)
-
-  def test_short_dropout_holds_then_releases_at_a_bounded_rate(self):
-    controller = make_controller()
-    for _ in range(15):
-      restricted = update(controller, restrictive_radar())
-
-    held = [update(controller) for _ in range(controller.dropout_frames - 1)]
-    assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held)
-
-    released = [update(controller) for _ in range(60)]
-    targets = [restricted.target_speed, *(result.target_speed for result in released)]
-    assert np.max(np.diff(targets)) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
-    assert released[-1].target_speed >= 25.0 - 0.15 - 1e-9
-    assert released[-1].state == AccelControllerState.free
-
-  def test_restricting_lead_dropout_coasts_before_release(self):
-    controller = make_controller()
-    for _ in range(15):
-      update(controller, restrictive_radar(), planner_accel=-0.5)
-
-    coast = update(controller, planner_accel=-0.5)
-
-    assert coast.cruise_accel_max == 0.0
-    assert coast.mpc_accel_max is not None
-    assert effective_accel_max(coast) == pytest.approx(profile_accel_max(AccelProfile.normal, 10.0))
-
-    ceilings = [coast.cruise_accel_max]
-    ceilings.extend(update(controller, planner_accel=-0.5).cruise_accel_max for _ in range(controller.dropout_frames - 1))
-    assert ceilings == sorted(ceilings)
-    assert ceilings[-1] == pytest.approx(profile_accel_max(AccelProfile.normal, 10.0))
-
-  def test_should_coast_on_dropout_lifecycle(self):
-    controller = make_controller()
-    for _ in range(15):
-      update(controller, restrictive_radar(), planner_accel=-0.5)
-
-    update(controller, planner_accel=-0.5)
-    assert controller.lead_controller.should_coast_on_dropout
-
-    update(controller, restrictive_radar(), planner_accel=0.2)
-    assert not controller.lead_controller.should_coast_on_dropout
-    update(controller, planner_accel=0.2)
-    assert not controller.lead_controller.should_coast_on_dropout
-
-    controller.reset()
-    assert not controller.lead_controller.should_coast_on_dropout
-
-  def test_e2e_braking_handoff_clears_when_braking_ends(self):
-    controller = make_controller()
-    update(controller, previous_mpc_source=LongitudinalPlanSource.e2e, previous_plan_accel=-1.0, planner_accel=-0.5)
-    assert controller.lead_controller.e2e_braking_handoff
-
-    still_braking = update(controller, planner_accel=-0.12)
-    assert controller.lead_controller.e2e_braking_handoff
-    assert still_braking.mpc_accel_max is None
-
-    coasting = update(controller, planner_accel=-0.10)
-    assert not controller.lead_controller.e2e_braking_handoff
-    assert coasting.mpc_accel_max is not None
-
-
-class TestMatchedLead:
-  def test_recovery_accel_limit_unthrottled_when_ego_well_below_lead_speed(self):
-    controller = make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    for _ in range(20):
-      update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-    result = update(controller, radar, v_ego=3.0, planner_accel=-0.2)
-
-    assert result.lead_recovery
-    assert result.recovery_accel_limit == pytest.approx(profile_accel_max(AccelProfile.normal, 3.0))
-    assert effective_accel_max(result) == pytest.approx(profile_accel_max(AccelProfile.normal, 3.0))
-
-  def test_recovery_accel_limit_throttles_toward_recovery_headroom_when_near_lead_speed(self):
-    controller = make_controller()
-    slow_radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=3.0))
-    for _ in range(20):
-      update(controller, slow_radar, v_ego=5.0, planner_accel=-0.2)
-    for _ in range(40):
-      result = update(controller, slow_radar, v_ego=3.0, planner_accel=-0.2)
-
-    assert result.lead_recovery
-    assert result.recovery_accel_limit == pytest.approx(LEAD_RECOVERY_HEADROOM)
-    assert result.recovery_accel_limit < profile_accel_max(AccelProfile.normal, 3.0)
-
-  def test_recovery_accel_limit_slew_bounded_and_independent_of_planner_accel_sign(self):
-    braking_controller, accelerating_controller = make_controller(), make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    for controller in (braking_controller, accelerating_controller):
-      for _ in range(20):
-        update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-      for _ in range(20):
-        update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    before = braking_controller.lead_controller.recovery_accel_limit
-    assert accelerating_controller.lead_controller.recovery_accel_limit == pytest.approx(before)
-
-    braking = update(braking_controller, radar, v_ego=8.0, planner_accel=-0.2)
-    accelerating = update(accelerating_controller, radar, v_ego=8.0, planner_accel=0.2)
-
-    assert abs(braking.recovery_accel_limit - before) <= LEAD_RECOVERY_ACCEL_SLEW * DT_MDL + 1e-9
-    assert accelerating.recovery_accel_limit == pytest.approx(braking.recovery_accel_limit)
-
-
-class TestLaunchAndDeparture:
-  def test_clear_road_launch_has_immediate_headroom_and_bounded_target_slew(self):
-    controller = make_controller()
-    initial = update(controller, base_speed=12.0, v_ego=0.0, profile=AccelProfile.normal)
-    rolling = update(controller, base_speed=12.0, v_ego=0.31, profile=AccelProfile.normal)
-
-    assert initial.active and initial.launching
-    assert LAUNCH_TARGET_HEADROOM <= initial.target_speed <= LAUNCH_TARGET_HEADROOM + TARGET_RELEASE_SLEW * DT_MDL
-    assert rolling.launching
-    assert rolling.target_speed >= 0.31 + LAUNCH_TARGET_HEADROOM
-    assert rolling.target_speed - max(initial.target_speed, 0.31 + LAUNCH_TARGET_HEADROOM) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9
-
-    finished = update(controller, base_speed=12.0, v_ego=LAUNCH_END_SPEED, profile=AccelProfile.normal)
-    assert not finished.launching
-
-  def test_departure_launch_keeps_profile_ceiling_through_lead_recovery_transition(self):
-    controller = make_controller()
-    enter_stop_hold(controller, base_speed=12.0, track_id=100)
-    for frame in range(STOP_HOLD_EXIT_FRAMES):
-      departing = make_radar(make_lead(status=True, d_rel=6.5 + 0.1 * frame, v_lead_k=5.0, radar_track_id=100))
-      launching = update(controller, departing, base_speed=12.0, v_ego=0.1, profile=AccelProfile.eco, planner_accel=1.4)
-
-    exited = update(controller, departing, base_speed=12.0, v_ego=LAUNCH_END_SPEED, profile=AccelProfile.eco, planner_accel=1.4)
-
-    assert launching.departure_launching and effective_accel_max(launching) == pytest.approx(profile_accel_max(AccelProfile.eco, 0.1))
-    profile_ceiling = profile_accel_max(AccelProfile.eco, LAUNCH_END_SPEED)
-    assert exited.lead_recovery
-    assert profile_ceiling - LEAD_RECOVERY_ACCEL_SLEW * DT_MDL <= effective_accel_max(exited) <= profile_ceiling
-
-  def test_e2e_braking_handoff_arms_only_on_seed_frame_from_previous_plan_accel(self):
-    armed = make_controller()
-    update(armed, base_speed=20.0, v_ego=15.0, previous_mpc_source=LongitudinalPlanSource.e2e,
-          previous_plan_accel=-1.0, planner_accel=0.5)
-    assert armed.lead_controller.e2e_braking_handoff
-
-    not_armed = make_controller()
-    update(not_armed, base_speed=20.0, v_ego=15.0, previous_mpc_source=LongitudinalPlanSource.e2e,
-          previous_plan_accel=0.5, planner_accel=-1.0)
-    assert not not_armed.lead_controller.e2e_braking_handoff
-
-  def test_stop_hold_needs_four_confirmed_departure_frames_with_real_radar(self):
-    controller = make_controller()
-    held = enter_stop_hold(controller, track_id=100)
-    assert held.stop_hold and held.target_speed == 0.0 and held.mpc_accel_max is None
-
-    results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)),
-                      base_speed=8.0, v_ego=0.1) for frame in range(STOP_HOLD_EXIT_FRAMES + 4)]
-    launch_index = next(index for index, result in enumerate(results) if result.launching)
-
-    assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
-    assert all(result.stop_hold and not result.launching for result in results[:launch_index])
-    assert results[launch_index].departure_launching
-    assert results[launch_index].target_speed == 8.0
-
-  def test_renewed_stop_mid_launch_aborts_back_to_stop_hold(self):
-    controller = make_controller()
-    enter_stop_hold(controller, track_id=100)
-    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
-      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100))
-      update(controller, moving, base_speed=8.0, v_ego=0.1)
-    assert controller.lead_controller.launching and controller.lead_controller.departure_launching
-
-    renewed_stop_lead = make_radar(make_lead(status=True, d_rel=6.5, v_lead_k=0.05, radar_track_id=100))
-    result = update(controller, renewed_stop_lead, base_speed=8.0, v_ego=0.1)
-    assert result.stop_hold
-    assert result.target_speed == 0.0
-    assert not result.launching
-
-  def test_invalid_lead_mid_launch_aborts_launch_without_reentering_stop_hold(self):
-    controller = make_controller()
-    enter_stop_hold(controller, track_id=100)
-    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
-      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100))
-      update(controller, moving, base_speed=8.0, v_ego=0.5)
-    assert controller.lead_controller.launching
-
-    invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0))
-    result = update(controller, invalid, base_speed=8.0, v_ego=0.5)
-    assert not result.stop_hold
-    assert not result.launching
-    assert result.active
-
-  def test_genuine_departure_survives_lead_slot_and_track_flicker(self):
-    controller = make_controller()
-    enter_stop_hold(controller, track_id=100)
-    results = []
-    for frame in range(STOP_HOLD_EXIT_FRAMES + 4):
-      moving = make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)
-      secondary = make_lead(status=True, d_rel=7.0, v_lead_k=2.0, radar_track_id=200)
-      radar = make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving)
-      results.append(update(controller, radar, base_speed=8.0, v_ego=0.0))
-
-    launch_index = next(index for index, result in enumerate(results) if result.launching)
-    assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
-    assert all(result.stop_hold for result in results[:launch_index])
-    assert results[-1].launching and results[-1].departure_launching
-
-  def test_confirmed_creep_departure_departs_within_budget(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    results = []
-    for frame in range(60):
-      creeping = make_radar(make_lead(status=True, d_rel=6.0 + frame * 0.01, v_lead_k=0.2))
-      results.append(update(controller, creeping, base_speed=8.0, v_ego=0.0))
-
-    launch_index = next(index for index, result in enumerate(results) if result.launching)
-    assert launch_index * DT_MDL <= 2.0
-    assert all(not result.stop_hold for result in results[launch_index:])
-
-  def test_departure_dropout_holds_without_resurrecting_stop_hold(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
-      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
-      update(controller, moving, base_speed=8.0, v_ego=0.1)
-    assert controller.lead_controller.launching
-
-    dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_confirm_frames + 5)]
-    assert all(not result.stop_hold for result in dropout)
-    assert all(result.launching for result in dropout)
-
-  def test_departure_launch_expires_after_bounded_radar_staleness(self):
-    controller = make_controller()
-    enter_stop_hold(controller, track_id=100)
-    for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
-      moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100))
-      update(controller, moving, base_speed=8.0, v_ego=0.1)
-    assert controller.lead_controller.departure_launching
-
-    held = [update(controller, moving, base_speed=8.0, v_ego=0.1, radar_fresh=False)
-            for _ in range(controller.dropout_frames - 1)]
-    expired = update(controller, moving, base_speed=8.0, v_ego=0.1, radar_fresh=False)
-
-    assert all(result.active and result.departure_launching for result in held)
-    assert not expired.active
-    assert not controller.lead_controller.departure_launching
-    assert controller.update_should_stop(True)
-
-  def test_stop_hold_without_usable_lead_stays_pinned_to_zero(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    missing = update(controller, base_speed=8.0, v_ego=0.1)
-
-    assert missing.stop_hold
-    assert missing.target_speed == 0.0
-    assert missing.mpc_accel_max is None
-
-  def test_leadless_departure_does_not_override_should_stop(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    for _ in range(controller.lead_confirm_frames):
-      result = update(controller, base_speed=8.0, v_ego=0.0)
-
-    assert result.launching
-    assert not result.departure_launching
-    assert controller.update_should_stop(True)
-
-  def test_invalid_lead_does_not_release_stop_hold(self):
-    controller = make_controller()
-    enter_stop_hold(controller, track_id=100)
-    invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0, radar_track_id=100))
-
-    results = [update(controller, invalid, base_speed=8.0, v_ego=0.31) for _ in range(controller.lead_confirm_frames + 2)]
-    first_absent = update(controller, base_speed=8.0, v_ego=0.31)
-
-    assert all(result.stop_hold and result.target_speed == 0.0 for result in results)
-    assert first_absent.stop_hold and first_absent.target_speed == 0.0
-    assert controller.update_should_stop(False)
-
-  def test_vision_lead_departure_does_not_override_should_stop(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    for frame in range(STOP_HOLD_EXIT_FRAMES):
-      moving = make_radar(make_lead(status=True, d_rel=6.0 + 0.1 * frame, v_lead_k=2.0))
-      result = update(controller, moving, base_speed=8.0, v_ego=0.0)
-
-    assert result.launching
-    assert not result.departure_launching
-    assert controller.update_should_stop(True)
-
-  def test_replacement_lead_cannot_claim_a_confirmed_departure(self):
-    controller = make_controller()
-    stopped = make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100)
-    moving = make_lead(status=True, d_rel=20.0, v_lead_k=2.0, radar_track_id=200)
-    for _ in range(6):
-      held = update(controller, make_radar(stopped, moving), base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
-    assert held.stop_hold
-
-    for _ in range(STOP_HOLD_EXIT_FRAMES):
-      moving.dRel += 0.1
-      result = update(controller, make_radar(lead_two=moving), base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
-
-    assert result.launching
-    assert not result.departure_launching
-    assert controller.update_should_stop(True)
-
-  def test_moving_departure_does_not_reenter_stop_hold_once_launching(self):
-    controller = make_controller()
-    enter_stop_hold(controller, track_id=100)
-    distance = 6.0
-    results = []
-    for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72, 0.70):
-      distance += speed * DT_MDL
-      radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=100))
-      results.append(update(controller, radar, base_speed=8.0, v_ego=0.0))
-
-    launch_index = next(index for index, result in enumerate(results) if result.launching)
-    assert all(not result.stop_hold for result in results[launch_index:])
-    assert all(result.target_speed > 0.0 and result.departure_launching for result in results[launch_index:])
-
-  def test_far_stopped_lead_should_not_create_stop_hold(self):
-    controller = make_controller()
-    far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0))
-    results = [update(controller, far_stopped, base_speed=12.0, v_ego=0.0) for _ in range(4)]
-    assert all(not result.stop_hold for result in results)
-
-  def test_fast_speed_glitch_without_distance_progress_should_stay_in_stop_hold(self):
-    controller = make_controller()
-    stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
-    update(controller, stopped, base_speed=8.0, v_ego=0.0)
-    glitch = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.9, radar_track_id=100))
-    results = [update(controller, glitch, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES + 2)]
-
-    assert all(result.stop_hold and result.target_speed == 0.0 and not result.launching for result in results)
-
-
-class TestFreshnessAndReset:
-  def test_frozen_output_during_a_single_non_fresh_radar_frame(self):
-    controller = make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
-    for _ in range(15):
-      fresh = update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-
-    held = update(controller, radar, v_ego=10.0, planner_accel=-0.2, radar_fresh=False)
-    assert held.target_speed == pytest.approx(fresh.target_speed)
-    assert held.state == fresh.state
-    assert held.selected_lead == fresh.selected_lead
-    assert effective_accel_max(held) == pytest.approx(effective_accel_max(fresh))
-
-  def test_stale_timeout_fully_resets_live_state(self):
-    controller = make_controller()
-    radar = restrictive_radar()
-    for _ in range(15):
-      update(controller, radar)
-    held = [update(controller, radar_fresh=False) for _ in range(controller.radar_stale_frames - 1)]
-    timed_out = update(controller, radar_fresh=False)
-
-    assert all(result.active for result in held)
-    assert not timed_out.active
-    assert timed_out.target_speed == 25.0
-    assert timed_out.mpc_accel_max is None
-    assert timed_out.selected_lead == -1
-    assert controller.lead_controller.target_speed is None
-
-  def test_acc_bypass_starts_a_new_stale_episode(self):
-    controller = make_controller()
-    radar = restrictive_radar()
-    update(controller, radar)
-    for _ in range(controller.radar_stale_frames - 1):
-      update(controller, radar, radar_fresh=False)
-
-    bypassed = update(controller, radar, acc_selected=False, radar_fresh=False)
-    handoff = update(controller, radar, radar_fresh=False, previous_mpc_source=LongitudinalPlanSource.e2e,
-                     previous_plan_accel=-0.5, planner_accel=0.5)
-
-    assert not bypassed.active
-    assert handoff.active
-    assert controller.lead_controller.e2e_braking_handoff
-
-  @pytest.mark.parametrize("override", [{"enabled": False}, {"acc_selected": False}, {"engaged": False},
-                                        {"cruise_initialized": False}, {"a_ego": math.inf}])
-  def test_bypass_or_invalid_context_resets_live_state(self, override):
-    controller = make_controller()
-    for _ in range(15):
-      update(controller, restrictive_radar())
-    result = update(controller, restrictive_radar(), **override)
-
-    assert not result.active
-    assert result.target_speed == 25.0
-    assert result.mpc_accel_max is None
-    assert controller.lead_controller.target_speed is None
-
-  def test_acc_bypass_does_not_retain_state_for_live_actuation(self):
-    controller = make_controller()
-    for _ in range(20):
-      bypassed = update(controller, restrictive_radar(), acc_selected=False)
-      assert not bypassed.active
-      assert controller.lead_controller.target_speed is None
-    live = update(controller)
-
-    assert live.active
-    assert 10.0 < live.target_speed <= 10.0 + TARGET_RELEASE_SLEW * DT_MDL + 1e-9
-
-  def test_explicit_reset_clears_lead_state(self):
-    controller = make_controller()
-    for _ in range(15):
-      update(controller, restrictive_radar())
-    controller._jerk_smoothing_blocked = True
-    controller._required_decel_samples = [0.2]
-    controller._required_decel_lead = controller._required_decel_lead_track_id = 1
-    controller._lead_trend_warmup = True
-    controller.reset()
-
-    assert not controller._jerk_smoothing_blocked
-    assert controller._required_decel_samples == []
-    assert controller._required_decel_lead == controller._required_decel_lead_track_id == -1
-    assert not controller._lead_trend_warmup
-    lead_controller = controller.lead_controller
-    assert lead_controller.target_speed is None and lead_controller.recovery_accel_limit is None
-    assert not lead_controller.stop_hold and not lead_controller.launching and not lead_controller.lead_recovery
-    assert math.isinf(lead_controller.lead_speed_ceiling) and math.isinf(lead_controller.filtered_lead_speed)
-    assert lead_controller.lead_speed_samples == [math.inf] * LEAD_SAMPLE_FILTER_FRAMES
-    assert controller.state == AccelControllerState.inactive
-    assert controller.selected_lead == controller.selected_lead_track_id == -1
-
-
-class TestJerkCostMultiplier:
-  @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
-  def test_track_id_change_requires_new_history_before_jerk_smoothing(self, replacement_track_id):
-    controller = make_controller()
-    controller.state = AccelControllerState.restrict
-    controller.selected_lead = 0
-    controller.selected_lead_track_id = 100
-    controller.required_decel = 0.2
-    original = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
-
-    controller.selected_lead_track_id = replacement_track_id
-    replacement = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
-
-    assert original == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
-    assert replacement == [1.0, 1.0, 1.0, MPC_DECEL_JERK_COST_MULTIPLIER]
-    assert controller._required_decel_samples == [0.2] * 4

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py

@@ -1,613 +0,0 @@
-import inspect
-import math
-from types import SimpleNamespace
-
-import numpy as np
-import pytest
-
-from openpilot.cereal import custom, log, messaging
-from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, AccelProfile,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
-
-
-def radar_state():
-  return messaging.new_message("radarState").radarState
-
-
-class PlannerSM(dict):
-  def __init__(self, radar_log_mono_time: int):
-    super().__init__(
-      radarState=radar_state(),
-      carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=20.0),
-      selfdriveState=SimpleNamespace(personality=0),
-      controlsState=SimpleNamespace(forceDecel=False),
-    )
-    self.valid = {"radarState": True}
-    self.alive = {"radarState": True}
-    self.logMonoTime = {"radarState": radar_log_mono_time}
-
-
-class ControllerStub:
-  def __init__(self, *, target_speed=15.0, active=True, mpc_accel_max=None, cruise_accel_max=None,
-               state=AccelControllerState.free, selected_lead=-1,
-               selected_lead_track_id=-1, launching=False, departure_launching=False, required_decel=0.0):
-    self.available = self.enabled = True
-    self.profile = AccelProfile.normal
-    self.output_v_target = target_speed
-    self.is_active = active
-    self.mpc_accel_max = mpc_accel_max
-    self.cruise_accel_max = cruise_accel_max
-    self.state = state
-    self.selected_lead = selected_lead
-    self.selected_lead_track_id = selected_lead_track_id
-    self.launching = launching
-    self.departure_launching = departure_launching
-    self.lead_controller = SimpleNamespace(departure_launching=departure_launching, stop_hold=state == AccelControllerState.stopHold)
-    self.required_decel = required_decel
-    self.dt = DT_MDL
-    self._jerk_smoothing_blocked = False
-    self._required_decel_samples = []
-    self._required_decel_long_samples = []
-    self._required_decel_lead = -1
-    self._required_decel_lead_track_id = -1
-    self._lead_trend_warmup = False
-    self.update_kwargs = None
-    self.reset_calls = 0
-
-  def update(self, _radar_state, **kwargs):
-    self.update_kwargs = kwargs
-
-  @property
-  def is_enabled(self):
-    return self.available and self.enabled
-
-  def update_params(self):
-    pass
-
-  def reset(self):
-    self.reset_calls += 1
-
-  def get_jerk_cost_multiplier(self, *args):
-    return AccelController.get_jerk_cost_multiplier(self, *args)
-
-  def update_should_stop(self, should_stop, departure_authorized=True):
-    return AccelController.update_should_stop(self, should_stop, departure_authorized)
-
-
-def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_accel_max=None,
-                         cruise_accel_max=None,
-                         state=AccelControllerState.free, selected_lead=-1, launching=False,
-                         departure_launching=False, required_decel=0.0,
-                         mpc_source=MpcLongitudinalPlanSource.lead0):
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  is_e2e_calls = []
-  planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e
-  planner.output_v_target = 20.0
-  planner.output_should_stop = False
-  planner._long_active_last_cycle = True
-  planner.previous_plan_accel = 0.0
-  planner.mpc_accel_seed = 0.0
-  planner.allow_throttle = True
-  planner.a_desired = 0.0
-  planner.v_desired_filter = SimpleNamespace(x=10.0)
-  planner._radar_fresh_this_cycle = True
-  planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
-  planner.accel_controller = ControllerStub(
-    target_speed=target_speed, active=active, state=state, selected_lead=selected_lead, launching=launching,
-    departure_launching=departure_launching, required_decel=required_decel, mpc_accel_max=mpc_accel_max,
-    cruise_accel_max=cruise_accel_max,
-  )
-  return planner, is_e2e_calls
-
-
-def prepare_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False, stock_accel_max=ACCEL_MAX):
-  configs = []
-  sm = {
-    "radarState": radar_state(),
-    "controlsState": SimpleNamespace(forceDecel=force_decel),
-    "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
-    "selfdriveState": SimpleNamespace(personality=0),
-  }
-  planner.mpc.set_accel_controller_params = lambda *args: configs.append(args)
-  is_e2e, target = planner.update_accel_controller(sm, mpc_v_cruise, True, stock_accel_max, False)
-  assert len(configs) == 1
-  return is_e2e, target, configs[0]
-
-
-def test_accel_controller_schema_contract():
-  expected = {"eco": 0, "normal": 1, "sport": 2}
-  state = {"inactive": 0, "free": 1, "restrict": 2, "hold": 3, "release": 4, "stopHold": 5}
-  accel_controller = custom.LongitudinalPlanSP.schema.fields["accelController"]
-  fields = custom.LongitudinalPlanSP.AccelController.schema.fields
-
-  assert accel_controller.proto.ordinal.explicit == 8
-  assert {name: field.proto.ordinal.explicit for name, field in fields.items()} == {
-    "enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4,
-  }
-  assert fields["shadowOnlyDEPRECATED"].proto.slot.type.which() == "bool"
-  assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
-  assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
-  assert custom.LongitudinalPlanSP.AccelController.State.schema.enumerants == state
-
-
-def test_accel_controller_schema_round_trip_and_toyota_compatibility():
-  message = custom.LongitudinalPlanSP.new_message()
-  message.accelController.enabled = True
-  message.accelController.active = True
-  message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport
-  message.accelController.state = custom.LongitudinalPlanSP.AccelController.State.release
-
-  with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
-    assert reader.accelController.enabled and reader.accelController.active
-    assert reader.accelController.profile == custom.LongitudinalPlanSP.AccelController.Profile.sport
-    assert reader.accelController.state == custom.LongitudinalPlanSP.AccelController.State.release
-
-  from opendbc.car.toyota.carstate import AccelPersonality, CarState
-
-  assert AccelPersonality.schema.enumerants == {"eco": 0, "normal": 1, "sport": 2}
-  assert CarState.__module__ == "opendbc.car.toyota.carstate"
-
-
-def test_mpc_inherits_accel_controller_extension_without_changing_stock_signature_or_bounds():
-  assert LongitudinalMpc.__bases__ == (LongitudinalMpcSP,)
-  assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality")
-  mpc = LongitudinalMpc()
-  radar = radar_state()
-  mpc.run = lambda: None
-
-  mpc.set_cur_state(10.0, 0.8)
-  mpc.update(radar, 30.0)
-  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
-  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
-  assert mpc.cruise_accel_max(1.6) == 1.6
-
-  mpc.set_accel_controller_params(None, 1.0, 0.4)
-  assert mpc.cruise_accel_max(1.6) == 0.4
-  mpc.set_accel_controller_params(None, 1.0, 0.0)
-  assert mpc.cruise_accel_max(1.6) == 0.0
-
-  requested_ceiling = tuple(np.full(N + 1, 0.4))
-  mpc.set_accel_controller_params(requested_ceiling, 1.0)
-  mpc.update(radar, 30.0)
-  np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
-  assert mpc.params[0, 1] == pytest.approx(0.8)
-  np.testing.assert_array_equal(mpc.params[1:, 1], requested_ceiling[1:])
-
-  for malformed_ceiling in ("bad", [0.4] * N, np.full(N + 1, math.nan), [10**10000] * (N + 1)):
-    mpc.set_accel_controller_params(malformed_ceiling, 1.0)
-    mpc.update(radar, 30.0)
-    np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
-    np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
-
-  mpc.set_accel_controller_params(None, 1.0)
-  mpc.update(radar, 30.0)
-  np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
-
-
-def test_mpc_jerk_cost_multiplier_is_backward_compatible_and_does_not_change_other_costs():
-  mpc = LongitudinalMpc.__new__(LongitudinalMpc)
-  LongitudinalMpcSP.__init__(mpc)
-  captured = []
-  mpc.set_cost_weights = lambda costs, constraints: captured.append((np.asarray(costs), np.asarray(constraints)))
-
-  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
-  default_costs, default_constraints = captured[-1]
-  mpc.set_accel_controller_params(None, 1.0)
-  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
-  explicit_costs, explicit_constraints = captured[-1]
-  mpc.set_accel_controller_params(None, 1.2)
-  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
-  smoothed_costs, smoothed_constraints = captured[-1]
-
-  np.testing.assert_array_equal(explicit_costs, default_costs)
-  np.testing.assert_array_equal(explicit_constraints, default_constraints)
-  np.testing.assert_array_equal(smoothed_costs[:-1], default_costs[:-1])
-  assert smoothed_costs[-1] == pytest.approx(default_costs[-1] * 1.2)
-  np.testing.assert_array_equal(smoothed_constraints, default_constraints)
-
-  mpc.set_weights(False, personality=log.LongitudinalPersonality.standard)
-  assert captured[-1][0][-2] == 0.0
-  assert captured[-1][0][-1] == pytest.approx(default_costs[-1] * 1.2)
-
-
-def test_accel_controller_hook_only_configures_mpc():
-  radar = radar_state()
-  planner, _ = planner_for_mpc_test(active=False)
-  calls = []
-  planner.mpc = SimpleNamespace(
-    source=MpcLongitudinalPlanSource.cruise,
-    last_solution_status=0,
-    set_accel_controller_params=lambda accel_max, multiplier, cruise_accel_max: calls.append(
-      ("configure", accel_max, multiplier, cruise_accel_max)),
-    set_weights=lambda constraint, personality: calls.append(("weights", constraint, personality)),
-    set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)),
-    update=lambda radar_arg, target, *, personality: calls.append(("update", radar_arg, target, personality)),
-  )
-  sm = {
-    "radarState": radar,
-    "controlsState": SimpleNamespace(forceDecel=False),
-    "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
-    "selfdriveState": SimpleNamespace(personality=2),
-  }
-  is_e2e, target = planner.update_accel_controller(sm, 17.5, True, ACCEL_MAX, False)
-
-  assert not is_e2e and target == 17.5
-  assert calls == [("configure", None, 1.0, None)]
-
-
-def test_active_acc_uses_target_and_ceiling_in_exactly_one_solve():
-  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
-  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
-  is_e2e, target, config = prepare_controller_mpc(planner)
-
-  assert not is_e2e
-  assert len(mode_calls) == 1
-  assert target == 15.0
-  assert config == (ceiling, 1.0, None)
-
-
-@pytest.mark.parametrize("cruise_accel_max", (0.0, 0.3))
-def test_cruise_accel_ceiling_is_forwarded_to_mpc(cruise_accel_max):
-  planner, _ = planner_for_mpc_test(cruise_accel_max=cruise_accel_max)
-  _, _, config = prepare_controller_mpc(planner)
-  assert config == (None, 1.0, cruise_accel_max)
-
-
-@pytest.mark.parametrize(("stock_accel_max", "expected"), ((1.2, 1.2), (-0.3, 0.0)))
-def test_controller_receives_stock_allow_throttle_ceiling(stock_accel_max, expected):
-  planner, _ = planner_for_mpc_test()
-  planner.allow_throttle = False
-  prepare_controller_mpc(planner, stock_accel_max=stock_accel_max)
-
-  assert planner.accel_controller.update_kwargs["stock_accel_max"] == expected
-
-
-def test_valid_lead_stop_hold_preplans_from_raw_target_without_an_accel_ceiling():
-  planner, _ = planner_for_mpc_test(
-    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=0,
-  )
-  _, target, config = prepare_controller_mpc(planner)
-
-  assert target == 20.0
-  assert config == (None, 1.0, None)
-
-
-def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling():
-  planner, _ = planner_for_mpc_test(
-    target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=-1,
-  )
-  _, target, config = prepare_controller_mpc(planner)
-
-  assert target == 0.0
-  assert config == (None, 1.0, None)
-
-
-@pytest.mark.parametrize(
-  ("active", "departure_launching", "expected"),
-  [
-    (True, True, False),
-    (True, False, True),
-    (False, True, True),
-  ],
-)
-def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, expected):
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner.accel_controller = ControllerStub(active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold)
-  planner._accel_controller_actuating = active
-  planner._radar_fresh_this_cycle = True
-  planner.mpc = SimpleNamespace(last_solution_status=0)
-  assert planner.update_should_stop(True) is expected
-  assert planner.update_should_stop(False) is (active and not departure_launching)
-
-
-def test_stale_radar_cannot_authorize_departure():
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner.accel_controller = ControllerStub(active=True, departure_launching=True)
-  planner._accel_controller_actuating = True
-  planner._radar_fresh_this_cycle = False
-  planner.mpc = SimpleNamespace(last_solution_status=0)
-
-  assert planner.update_should_stop(True)
-
-
-@pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)])
-def test_disabled_or_e2e_is_an_exact_mpc_bypass(active, is_e2e):
-  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
-  planner, mode_calls = planner_for_mpc_test(active=active, is_e2e=is_e2e, mpc_accel_max=ceiling)
-  returned_e2e, target, config = prepare_controller_mpc(planner)
-
-  assert returned_e2e is is_e2e
-  assert len(mode_calls) == 1
-  assert target == 20.0
-  assert config == (None, 1.0, None)
-
-
-def test_force_decel_target_remains_authoritative_and_disables_ceiling():
-  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
-  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
-  _, target, config = prepare_controller_mpc(planner, mpc_v_cruise=0.0, force_decel=True)
-
-  assert len(mode_calls) == 1
-  assert target == 0.0
-  assert config == (None, 1.0, None)
-
-
-def test_previous_mpc_failure_gets_one_stock_recovery_cycle_without_resetting_controller_state():
-  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
-  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling, departure_launching=True)
-  controller = planner.accel_controller
-  planner.mpc.last_solution_status = 4
-
-  _, failed_target, failed_config = prepare_controller_mpc(planner)
-  assert controller.reset_calls == 0
-  assert controller.update_kwargs["acc_selected"]
-  assert len(mode_calls) == 1
-  assert failed_target == 20.0
-  assert failed_config == (None, 1.0, None)
-  assert planner.update_should_stop(True)
-
-  planner.mpc.last_solution_status = 0
-  _, recovered_target, recovered_config = prepare_controller_mpc(planner)
-  assert controller.reset_calls == 0
-  assert len(mode_calls) == 2
-  assert recovered_target == 15.0
-  assert recovered_config == (ceiling, 1.0, None)
-  assert not planner.update_should_stop(True)
-
-
-@pytest.mark.parametrize(
-  "mpc_source",
-  (MpcLongitudinalPlanSource.cruise, MpcLongitudinalPlanSource.lead0, MpcLongitudinalPlanSource.lead1),
-)
-def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier(mpc_source):
-  planner, _ = planner_for_mpc_test(
-    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
-    mpc_source=mpc_source,
-  )
-  _, target, config = prepare_controller_mpc(planner)
-
-  assert target == 15.0
-  assert config == (None, MPC_DECEL_JERK_COST_MULTIPLIER, None)
-
-
-def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_episode_ends():
-  planner, _ = planner_for_mpc_test(
-    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
-  )
-  _, _, initial_config = prepare_controller_mpc(planner)
-  controller = planner.accel_controller
-  assert initial_config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
-
-  controller.required_decel = MPC_DECEL_JERK_MAX_REQUIRED_DECEL
-  _, _, ineligible_config = prepare_controller_mpc(planner)
-  assert ineligible_config[1] == 1.0
-
-  controller.required_decel = 0.30
-  _, _, flicker_config = prepare_controller_mpc(planner)
-  assert flicker_config[1] == 1.0
-
-  controller.state = AccelControllerState.free
-  controller.output_v_target = 20.0
-  prepare_controller_mpc(planner)
-  controller.state = AccelControllerState.restrict
-  controller.output_v_target = 15.0
-  _, _, rearmed_config = prepare_controller_mpc(planner)
-  assert rearmed_config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
-
-
-def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends():
-  planner, _ = planner_for_mpc_test(
-    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
-  )
-  _, _, config = prepare_controller_mpc(planner)
-  controller = planner.accel_controller
-  multipliers = [config[1]]
-  for required_decel in (0.20, 0.23, 0.25):
-    controller.required_decel = required_decel
-    _, _, config = prepare_controller_mpc(planner)
-    multipliers.append(config[1])
-
-  assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0]
-
-  controller.required_decel = 0.20
-  _, _, config = prepare_controller_mpc(planner)
-  assert config[1] == 1.0
-
-  controller.state = AccelControllerState.free
-  controller.output_v_target = 20.0
-  prepare_controller_mpc(planner)
-  controller.state = AccelControllerState.restrict
-  controller.output_v_target = 15.0
-  controller.required_decel = 0.18
-  _, _, config = prepare_controller_mpc(planner)
-  assert config[1] == MPC_DECEL_JERK_COST_MULTIPLIER
-
-
-def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
-  planner, _ = planner_for_mpc_test(
-    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
-  )
-  _, _, config = prepare_controller_mpc(planner)
-  controller = planner.accel_controller
-  multipliers = [config[1]]
-  for required_decel in (0.24, 0.19, 0.22):
-    controller.required_decel = required_decel
-    _, _, config = prepare_controller_mpc(planner)
-    multipliers.append(config[1])
-
-  assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
-
-
-@pytest.mark.parametrize(
-  ("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"),
-  [
-    (AccelControllerState.free, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.hold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.stopHold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, -1, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, True, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, math.inf, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, math.nan, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, 0.0, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, -0.01, 15.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, 0.30, 20.0 - MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, 0.30, 20.0, MpcLongitudinalPlanSource.cruise),
-    (AccelControllerState.restrict, 0, False, 0.30, 25.0, MpcLongitudinalPlanSource.cruise),
-  ],
-)
-def test_non_routine_or_stock_lead_states_keep_stock_jerk_cost(
-  state, selected_lead, launching, required_decel, target_speed, mpc_source,
-):
-  planner, _ = planner_for_mpc_test(
-    state=state, selected_lead=selected_lead, launching=launching,
-    required_decel=required_decel, target_speed=target_speed, mpc_source=mpc_source,
-  )
-  _, _, config = prepare_controller_mpc(planner)
-
-  assert config[1] == 1.0
-
-
-def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
-  planner, _ = planner_for_mpc_test(mpc_source=log.LongitudinalPlan.LongitudinalPlanSource.lead0)
-  planner._radar_fresh_this_cycle = True
-  planner.a_desired = -0.4
-  planner.previous_plan_accel = -1.1
-  planner.v_desired_filter = SimpleNamespace(x=9.5)
-  prepare_controller_mpc(planner)
-  received = planner.accel_controller.update_kwargs
-
-  assert received["previous_mpc_source"] == log.LongitudinalPlan.LongitudinalPlanSource.lead0
-  assert received["planner_speed"] == 9.5
-  assert received["planner_accel"] == -0.4
-  assert received["previous_plan_accel"] == -1.1
-  assert received["radar_fresh"] is True
-
-
-@pytest.mark.parametrize(("previous_plan_accel", "a_desired", "expected"), (
-  (-1.1, 0.0, -1.1), (0.057, 1.032, 0.057), (0.4, -1.2, -1.2),
-))
-def test_e2e_to_acc_handoff_uses_one_sided_mpc_seed(previous_plan_accel, a_desired, expected):
-  planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e)
-  planner.previous_plan_accel = previous_plan_accel
-  planner.a_desired = a_desired
-  prepare_controller_mpc(planner)
-
-  assert planner.mpc_accel_seed == expected
-  assert planner.a_desired == a_desired
-
-
-def test_disabled_controller_does_not_change_e2e_to_acc_seed():
-  planner, _ = planner_for_mpc_test(active=False, mpc_source=MpcLongitudinalPlanSource.e2e)
-  planner.accel_controller.enabled = False
-  planner.previous_plan_accel = -1.1
-  planner.a_desired = 0.0
-  prepare_controller_mpc(planner)
-
-  assert planner.mpc_accel_seed == planner.a_desired == 0.0
-
-
-def test_inactive_previous_cycle_does_not_restore_an_old_e2e_brake_plan():
-  planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e)
-  planner._long_active_last_cycle = False
-  planner.previous_plan_accel = -1.1
-  planner.a_desired = 0.0
-  prepare_controller_mpc(planner)
-
-  assert planner.mpc_accel_seed == planner.a_desired == 0.0
-  assert math.isinf(planner.accel_controller.update_kwargs["previous_plan_accel"])
-
-
-def test_failed_e2e_solution_does_not_seed_the_next_mpc_cycle():
-  planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e)
-  planner.mpc.last_solution_status = 1
-  planner.previous_plan_accel = -1.1
-  planner.a_desired = 0.0
-  prepare_controller_mpc(planner)
-
-  assert planner.mpc_accel_seed == planner.a_desired == 0.0
-  assert math.isinf(planner.accel_controller.update_kwargs["previous_plan_accel"])
-
-
-def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable():
-  controller = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=False))
-  controller.enabled = True
-  assert not controller.is_enabled
-
-
-def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner._radar_log_mono_time = None
-  planner._radar_fresh_this_cycle = True
-  planner.events_sp = SimpleNamespace(clear=lambda: None)
-  dec_freshness = []
-  planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh))
-  planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
-  planner.output_a_target = 0.0
-  planner.output_v_target = 20.0
-  planner.output_should_stop = False
-  planner._long_active_last_cycle = False
-  planner.previous_plan_accel = 0.0
-  planner.allow_throttle = True
-  planner.a_desired = 0.0
-  planner.v_desired_filter = SimpleNamespace(x=10.0)
-  planner.mpc = SimpleNamespace(
-    source=log.LongitudinalPlan.LongitudinalPlanSource.cruise, last_solution_status=0,
-    set_accel_controller_params=lambda *_args: None,
-  )
-  planner.is_e2e = lambda _sm: False
-  planner.accel_controller = ControllerStub(target_speed=20.0, active=False)
-
-  sm = PlannerSM(100)
-  for expected in (True, False):
-    planner.update(sm)
-    planner.update_accel_controller(sm, 20.0, True, ACCEL_MAX, False)
-    assert dec_freshness[-1] is expected and planner.accel_controller.update_kwargs["radar_fresh"] is expected
-
-  sm.logMonoTime["radarState"] = 101
-  planner.update(sm)
-  planner.update_accel_controller(sm, 20.0, True, ACCEL_MAX, False)
-  assert dec_freshness[-1] is True and planner.accel_controller.update_kwargs["radar_fresh"] is True
-
-
-def test_accel_controller_status_publishes_minimal_fields():
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner.source = LongitudinalPlanSource.cruise
-  planner.output_v_target = 20.0
-  planner.output_a_target = 0.0
-  planner.events_sp = SimpleNamespace(to_msg=list)
-  planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
-  planner.accel_controller = ControllerStub(active=False, state=AccelControllerState.restrict)
-  planner.scc = SimpleNamespace(
-    vision=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, current_lat_acc=0.0, max_pred_lat_acc=0.0, is_enabled=False, is_active=False),
-    map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
-  )
-  planner.resolver = SimpleNamespace(
-    speed_limit=0.0, speed_limit_last=0.0, speed_limit_final=0.0, speed_limit_final_last=0.0,
-    speed_limit_valid=False, speed_limit_last_valid=False, speed_limit_offset=0.0, distance=0.0,
-    source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
-  )
-  planner.sla = SimpleNamespace(
-    state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, is_enabled=False, is_active=False,
-    output_v_target=20.0, output_a_target=0.0,
-  )
-  planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
-  sent = {}
-  planner.publish_longitudinal_plan_sp(
-    SimpleNamespace(all_checks=lambda service_list: True),
-    SimpleNamespace(send=lambda service, message: sent.update({service: message})),
-  )
-
-  telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
-  assert telemetry.enabled and not telemetry.active
-  assert telemetry.profile == int(AccelProfile.normal)
-  assert telemetry.state == int(AccelControllerState.restrict)
-  assert set(custom.LongitudinalPlanSP.AccelController.schema.fields) == {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"}

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_lead_controller.py

@@ -1,341 +0,0 @@
-import math
-
-import pytest
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL, DISTANCE_JUMP_CONFIRM_FRAMES, LEAD_DROPOUT_COAST_TIME, LEAD_RELEASE_CONFIRM_TIME,
-  STOP_HOLD_EXIT_FRAMES, TARGET_RELEASE_SLEW, AccelProfile,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead_controller import LeadController
-
-DT = DT_MDL
-COMFORT_DECEL_NORMAL = COMFORT_DECEL[AccelProfile.normal]
-PROFILE_MAX_ACCEL = 1.5
-
-
-def _frames(seconds: float) -> int:
-  return math.ceil(seconds / DT)
-
-
-LEAD_CONFIRM_FRAMES = max(LEAD_SAMPLE_FILTER_FRAMES, _frames(LEAD_RELEASE_CONFIRM_TIME))
-DROPOUT_FRAMES = max(LEAD_CONFIRM_FRAMES, _frames(LEAD_DROPOUT_COAST_TIME))
-
-
-def _lead_plan(speed: float, distance: float, speed_ceiling: float = 0.0, closing_speed: float = 0.0,
-              required_decel: float = 0.0, track_id: int = 1) -> LeadPlan:
-  return LeadPlan(
-    speed_ceiling=speed_ceiling, selected_lead=0, selected_lead_track_id=track_id, selected_lead_speed=speed, selected_lead_accel=0.0,
-    departure_lead=0, departure_lead_track_id=track_id, departure_lead_speed=speed, departure_lead_distance=distance,
-    departure_lead_raw_speed=speed,
-    departure_lead_separation=distance, departure_speed_ceiling=speed_ceiling,
-    closing_speed=closing_speed, required_decel=required_decel,
-    has_nearly_stopped_lead=speed < 0.15, lead_status=True,
-  )
-
-
-def _no_lead() -> LeadPlan:
-  return LeadPlan(lead_status=False)
-
-
-def _run(lead_controller: LeadController, lead_plan: LeadPlan, base_speed: float, v_ego: float, planner_speed: float | None = None,
-        planner_accel: float = 0.0, previous_mpc_source=None, previous_plan_accel: float = 0.0) -> float:
-  return lead_controller.update(lead_plan, base_speed, v_ego, COMFORT_DECEL_NORMAL, PROFILE_MAX_ACCEL, DT,
-                      LEAD_CONFIRM_FRAMES, DROPOUT_FRAMES, v_ego if planner_speed is None else planner_speed,
-                      planner_accel, previous_mpc_source, previous_plan_accel)
-
-
-def test_repeated_stop_reseeds_departure_distance():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
-  for frame in range(20):
-    _run(lead_controller, _lead_plan(2.0, 6.0 + 2.0 * (frame + 1) * DT), base_speed=8.0, v_ego=min(3.5, frame * 0.3))
-
-  for _ in range(10):
-    _run(lead_controller, _lead_plan(0.0, 3.0), base_speed=8.0, v_ego=0.0)
-  assert lead_controller.stop_hold
-
-  released_frame = None
-  for frame in range(60):
-    _run(lead_controller, _lead_plan(0.2, 3.0 + 0.2 * (frame + 1) * DT), base_speed=8.0, v_ego=0.0)
-    if not lead_controller.stop_hold:
-      released_frame = frame
-      break
-
-  assert released_frame is not None
-  assert released_frame * DT <= 2.0
-
-
-def test_departure_dropout_reseeds_distance_guard():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
-  _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0)
-
-  released_frame = None
-  for frame in range(60):
-    _run(lead_controller, _lead_plan(0.2, 3.0 + 0.2 * (frame + 1) * DT), base_speed=8.0, v_ego=0.0)
-    if not lead_controller.stop_hold:
-      released_frame = frame
-      break
-
-  assert released_frame is not None
-  assert released_frame * DT <= 2.0
-
-
-def test_departure_dropout_revokes_track_identity_trust():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0, track_id=100), base_speed=8.0, v_ego=0.0)
-
-  _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0)
-  for frame in range(4):
-    _run(lead_controller, _lead_plan(2.0, 20.0 + 0.1 * frame, speed_ceiling=8.0, track_id=100), base_speed=8.0, v_ego=0.0)
-
-  assert lead_controller.launching
-  assert lead_controller.leadless_departure
-  assert not lead_controller.departure_launching
-
-
-def test_range_discontinuity_revokes_track_identity_trust():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0, track_id=100), base_speed=8.0, v_ego=0.0)
-
-  for frame in range(DISTANCE_JUMP_CONFIRM_FRAMES + STOP_HOLD_EXIT_FRAMES + 2):
-    _run(lead_controller, _lead_plan(2.0, 20.0 + 0.1 * frame, speed_ceiling=8.0, track_id=100), base_speed=8.0, v_ego=0.0)
-    if not lead_controller.stop_hold:
-      break
-
-  assert lead_controller.launching
-  assert lead_controller.leadless_departure
-  assert not lead_controller.departure_launching
-
-
-def test_fast_lead_speed_requires_full_departure_distance():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
-
-  for distance in (6.00, 6.01, 6.02, 6.03):
-    target = _run(lead_controller, _lead_plan(1.0, distance), base_speed=8.0, v_ego=0.0)
-
-  assert lead_controller.stop_hold
-  assert target == 0.0
-
-
-def test_lead_disappearance_releases_stop_hold_monotonically():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
-
-  targets = [_run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0) for _ in range(DROPOUT_FRAMES + 10)]
-  first_release = next(index for index, target in enumerate(targets) if target > 0.0)
-  release_targets = targets[first_release:]
-
-  steps = [after - before for before, after in zip(release_targets[:-1], release_targets[1:], strict=True)]
-  assert all(0.0 <= step <= TARGET_RELEASE_SLEW * DT + 1e-9 for step in steps)
-
-
-def test_no_lead_departure_stays_bounded_when_lead_returns():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
-  for _ in range(LEAD_CONFIRM_FRAMES):
-    _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0)
-
-  before = lead_controller.target_speed
-  target = _run(lead_controller, _lead_plan(2.0, 6.0, speed_ceiling=8.0), base_speed=8.0, v_ego=0.5)
-
-  assert target <= max(before, 0.5) + TARGET_RELEASE_SLEW * DT + 1e-9
-
-
-def test_leadless_stop_release_never_exceeds_current_base_speed():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
-
-  targets = [_run(lead_controller, _no_lead(), base_speed=0.5, v_ego=4.0) for _ in range(LEAD_CONFIRM_FRAMES)]
-
-  assert max(targets) <= 0.5
-
-
-def test_retained_speed_ceiling_does_not_bind_a_moving_replacement():
-  lead_controller = LeadController()
-  _run(lead_controller, _lead_plan(0.0, 5.0, speed_ceiling=0.1, track_id=100), base_speed=8.0, v_ego=0.4)
-
-  _run(lead_controller, _lead_plan(2.0, 20.0, speed_ceiling=8.0, track_id=200), base_speed=8.0, v_ego=0.2)
-
-  assert lead_controller.lead_speed_ceiling == pytest.approx(0.1)
-  assert not lead_controller.stop_hold
-
-
-def test_braking_state_does_not_cross_confirmed_track_replacement():
-  lead_controller = LeadController()
-  for _ in range(LEAD_CONFIRM_FRAMES + 2):
-    _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=10), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
-  assert lead_controller.braking_for_lead
-
-  _run(lead_controller, _lead_plan(0.0, 25.0, speed_ceiling=4.0, track_id=99), base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
-
-  assert not lead_controller.stop_hold
-
-
-def test_braking_state_does_not_cross_vision_slot_replacement():
-  lead_controller = LeadController()
-  for _ in range(LEAD_CONFIRM_FRAMES + 2):
-    _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=-1), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
-  assert lead_controller.braking_for_lead
-
-  replacement = _lead_plan(0.0, 25.0, speed_ceiling=4.0, track_id=-1)._replace(selected_lead=1, departure_lead=1)
-  _run(lead_controller, replacement, base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
-
-  assert not lead_controller.stop_hold
-
-
-def test_radar_track_keeps_braking_confirmation_across_slots():
-  lead_controller = LeadController()
-  for frame in range(LEAD_CONFIRM_FRAMES + 2):
-    plan = _lead_plan(0.0, 7.0, speed_ceiling=0.8, track_id=100)
-    if frame % 2:
-      plan = plan._replace(selected_lead=1, departure_lead=1)
-    _run(lead_controller, plan, base_speed=8.0, v_ego=0.0, planner_accel=-0.5)
-
-  assert lead_controller.braking_for_lead
-  assert lead_controller.stop_hold
-
-
-def test_vision_slot_churn_releases_conservatively():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0, track_id=-1), base_speed=8.0, v_ego=0.0)
-
-  for frame in range(STOP_HOLD_EXIT_FRAMES + 2):
-    plan = _lead_plan(2.0, 6.0 + 0.1 * (frame + 1), speed_ceiling=8.0, track_id=-1)
-    if frame % 2:
-      plan = plan._replace(selected_lead=1, departure_lead=1)
-    _run(lead_controller, plan, base_speed=8.0, v_ego=0.0)
-    if not lead_controller.stop_hold:
-      break
-
-  assert lead_controller.launching
-  assert lead_controller.leadless_departure
-  assert not lead_controller.departure_launching
-
-
-def test_radar_track_dropout_keeps_braking_context():
-  lead_controller = LeadController()
-  for _ in range(LEAD_CONFIRM_FRAMES + 2):
-    _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=10), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
-  assert lead_controller.braking_for_lead
-
-  _run(lead_controller, _no_lead(), base_speed=15.0, v_ego=10.0, planner_accel=-0.5)
-
-  assert lead_controller.should_coast_on_dropout
-
-
-def test_range_replacement_starts_a_new_departure_baseline():
-  lead_controller = LeadController()
-  for _ in range(20):
-    _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0)
-  for _ in range(2):
-    _run(lead_controller, _lead_plan(0.2, 6.0), base_speed=8.0, v_ego=0.0)
-
-  released_frame = None
-  for frame in range(60):
-    distance = 3.0 + 0.2 * frame * DT
-    _run(lead_controller, _lead_plan(0.2, distance), base_speed=8.0, v_ego=0.0)
-    if not lead_controller.stop_hold:
-      released_frame = frame
-      break
-
-  assert released_frame is not None
-  assert released_frame * DT <= 2.0
-
-
-def test_stop_hold_clears_the_previous_recovery_limit():
-  lead_controller = LeadController()
-  for _ in range(LEAD_CONFIRM_FRAMES + 120):
-    _run(lead_controller, _lead_plan(5.0, 20.0, speed_ceiling=8.0), base_speed=12.0, v_ego=10.0)
-  assert lead_controller.recovery_accel_limit == pytest.approx(0.0)
-
-  _run(lead_controller, _lead_plan(0.0, 5.0), base_speed=8.0, v_ego=0.2, planner_accel=-0.5)
-  assert lead_controller.stop_hold
-  assert lead_controller.recovery_accel_limit is None
-
-  for frame in range(9):
-    _run(lead_controller, _lead_plan(1.0, 5.0 + 0.04 * frame, speed_ceiling=8.0), base_speed=8.0, v_ego=0.0)
-  assert lead_controller.departure_launching
-
-  _run(lead_controller, _lead_plan(5.0, 8.0, speed_ceiling=8.0), base_speed=8.0, v_ego=3.1)
-  assert lead_controller.recovery_accel_limit is not None
-  assert lead_controller.recovery_accel_limit > 1.4
-
-
-def test_speed_ceiling_tightens_immediately():
-  lead_controller = LeadController()
-  _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=25.0), base_speed=30.0, v_ego=20.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(25.0)
-  _run(lead_controller, _lead_plan(15.0, 40.0, speed_ceiling=10.0), base_speed=30.0, v_ego=20.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(10.0)
-
-
-def test_speed_ceiling_requires_confirmation_before_release():
-  lead_controller = LeadController()
-  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
-    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0)
-
-  glitch_len = LEAD_CONFIRM_FRAMES - 2
-  for _ in range(glitch_len):
-    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=28.0), base_speed=30.0, v_ego=20.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0), "brief relief spike must not be trusted"
-
-  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
-    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0)
-
-  for _ in range(LEAD_CONFIRM_FRAMES + LEAD_SAMPLE_FILTER_FRAMES + 2):
-    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=28.0), base_speed=30.0, v_ego=20.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(28.0), "sustained relief must eventually be trusted"
-
-
-def test_conflicting_relief_evidence_stays_restricted():
-  lead_controller = LeadController()
-  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
-    _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0)
-
-  targets = []
-  for frame in range(300):
-    speed_ceiling = 28.0 if frame % 2 == 0 else 20.0
-    targets.append(_run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=speed_ceiling), base_speed=30.0, v_ego=20.0))
-
-  assert lead_controller.lead_speed_ceiling == pytest.approx(20.0)
-  assert max(targets) == pytest.approx(20.0)
-
-
-def test_lead_dropout_holds_speed_ceiling_before_release():
-  lead_controller = LeadController()
-  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
-    _run(lead_controller, _lead_plan(6.0, 50.0, speed_ceiling=6.0), base_speed=18.0, v_ego=10.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(6.0)
-
-  for _ in range(DROPOUT_FRAMES - 1):
-    _run(lead_controller, _no_lead(), base_speed=18.0, v_ego=10.0)
-  assert lead_controller.lead_speed_ceiling == pytest.approx(6.0), "must coast, not snap, before the dropout window elapses"
-
-  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
-    _run(lead_controller, _no_lead(), base_speed=18.0, v_ego=10.0)
-  assert math.isinf(lead_controller.lead_speed_ceiling), "must release promptly once the dropout window has elapsed"
-
-
-def test_target_release_is_rate_limited():
-  lead_controller = LeadController()
-  for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2):
-    _run(lead_controller, _lead_plan(20.0, 60.0, speed_ceiling=22.0), base_speed=30.0, v_ego=22.0)
-  before = lead_controller.target_speed
-
-  target = _run(lead_controller, _lead_plan(28.0, 200.0, speed_ceiling=30.0), base_speed=30.0, v_ego=22.0)
-  assert target <= before + TARGET_RELEASE_SLEW * DT + 1e-9

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

@@ -1,48 +1,17 @@
-from openpilot.common.realtime import DT_MDL
-
-
 class WMACConstants:
-  TRAJECTORY_SIZE = 33
-  PARAM_READ_FRAMES = max(1, int(round(1.0 / DT_MDL)))
+  # Lead detection parameters
+  LEAD_WINDOW_SIZE = 6  # Stable detection window
+  LEAD_PROB = 0.45  # Balanced threshold for lead detection
 
-  EMERGENCY_HOLD_FRAMES = max(1, int(round(0.75 / DT_MDL)))
-  MIN_MODE_DURATION = {'acc': max(1, int(round(0.6 / DT_MDL))), 'blended': max(1, int(round(0.5 / DT_MDL)))}
-  ENTER_BLENDED_FRAMES = max(1, int(round(0.4 / DT_MDL)))
-  EXIT_BLENDED_FRAMES = max(1, int(round(0.35 / DT_MDL)))
-  STANDSTILL_FRAMES = max(1, int(round(0.2 / DT_MDL)))
+  # Slow down detection parameters
+  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
+  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
 
-  LEAD_PROB = 0.45
-  LEAD_EXIT_PROB = 0.25
-  LEAD_RISE_RATE = 1.0
-  LEAD_FALL_RATE = 0.35
-  RADAR_LEAD_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL)))
-  RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL)))
-  RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL)))
-
-  SLOW_DOWN_PROB = 0.5
-  SLOW_DOWN_EXIT_PROB = 0.4
-  SLOW_DOWN_RISE_RATE = 0.65
-  SLOW_DOWN_FALL_RATE = 0.15
+  # Optimized slow down distance curve - smooth and progressive
   SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
   SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
-  URGENT_SLOW_DOWN_PROB = 0.85
 
-  MODEL_DECEL_START = -0.5
-  MODEL_DECEL_RANGE = 2.0
-  MODEL_DECEL_TREND_FRAMES = 4
-  MODEL_DECEL_TREND_ACCEL = -0.075
-  MODEL_DECEL_TREND_RATE = 0.35
-  MODEL_DECEL_TREND_MAX_MPC_ACCEL = 0.075
-  MODEL_DECEL_TREND_MAX_COMMAND_STEP = 0.15
-  MODEL_DECEL_TREND_RELEASE_ACCEL = -0.02
-  ENDPOINT_URGENCY_GAIN = 1.3
-  CRITICAL_ENDPOINT_FACTOR = 0.3
-  CRITICAL_URGENCY_GAIN = 1.5
-  SPEED_URGENCY_MIN = 25.0
-  SPEED_URGENCY_RANGE = 80.0
-
-  SLOWNESS_PROB = 0.55
-  SLOWNESS_EXIT_PROB = 0.45
-  SLOWNESS_RISE_RATE = 0.35
-  SLOWNESS_FALL_RATE = 0.5
-  SLOWNESS_CRUISE_OFFSET = 1.025
+  # Slowness detection parameters
+  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
+  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
+  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset

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

@@ -6,119 +6,129 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
-from collections import deque
-import math
-from typing import Literal
-
 from openpilot.cereal import messaging
-from numpy import interp
 from opendbc.car import structs
+from numpy 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
+from typing import Literal
 
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+SET_MODE_TIMEOUT = 15
+
+# Define the valid mode types
 ModeType = Literal['acc', 'blended']
 
 
-def clip01(value: float) -> float:
-  return max(0.0, min(1.0, float(value)))
+class SmoothKalmanFilter:
+  """Enhanced Kalman filter with smoothing for stable decision making."""
 
+  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
+               alpha=1.0, smoothing_factor=0.85):
+    self.x = initial_value
+    self.P = 1.0
+    self.R = measurement_noise
+    self.Q = process_noise
+    self.alpha = alpha
+    self.smoothing_factor = smoothing_factor
+    self.initialized = False
+    self.history = []
+    self.max_history = 10
+    self.confidence = 0.0
 
-class SmoothedSignal:
-  def __init__(self, rise_rate: float, fall_rate: float, initial_value: float = 0.0):
-    self.rise_rate = clip01(rise_rate)
-    self.fall_rate = clip01(fall_rate)
-    self.value = clip01(initial_value)
+  def add_data(self, measurement):
+    if len(self.history) >= self.max_history:
+      self.history.pop(0)
+    self.history.append(measurement)
 
-  def update(self, measurement: float) -> float:
-    measurement = clip01(measurement)
-    rate = self.rise_rate if measurement > self.value else self.fall_rate
-    self.value += (measurement - self.value) * rate
-    return self.value
+    if not self.initialized:
+      self.x = measurement
+      self.initialized = True
+      self.confidence = 0.1
+      return
 
-  def reset(self, value: float = 0.0) -> None:
-    self.value = clip01(value)
+    self.P = self.alpha * self.P + self.Q
 
+    K = self.P / (self.P + self.R)
+    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
 
-class HysteresisSignal:
-  def __init__(self, enter_threshold: float, exit_threshold: float, rise_rate: float, fall_rate: float):
-    self.enter_threshold = clip01(enter_threshold)
-    self.exit_threshold = clip01(exit_threshold)
-    self.filter = SmoothedSignal(rise_rate, fall_rate)
-    self.active = False
+    innovation = measurement - self.x
+    self.x = self.x + effective_K * innovation
+    self.P = (1 - effective_K) * self.P
 
-  def update(self, measurement: float) -> bool:
-    value = self.filter.update(measurement)
-    threshold = self.exit_threshold if self.active else self.enter_threshold
-    self.active = value > threshold
-    return self.active
+    if abs(innovation) < 0.1:
+      self.confidence = min(1.0, self.confidence + 0.05)
+    else:
+      self.confidence = max(0.1, self.confidence - 0.02)
 
-  def reset(self) -> None:
-    self.filter.reset()
-    self.active = False
+  def get_value(self):
+    return self.x if self.initialized else None
 
-  @property
-  def value(self) -> float:
-    return self.filter.value
+  def get_confidence(self):
+    return self.confidence
+
+  def reset_data(self):
+    self.initialized = False
+    self.history = []
+    self.confidence = 0.0
 
 
 class ModeTransitionManager:
+  """Manages smooth transitions between driving modes with hysteresis."""
+
   def __init__(self):
     self.current_mode: ModeType = 'acc'
+    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
+    self.transition_timeout = 0
+    self.min_mode_duration = 10
     self.mode_duration = 0
-    self._pending_mode: ModeType = 'acc'
-    self._pending_count = 0
-    self._blended_hold_frames = 0
+    self.emergency_override = False
 
-  def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
-    if immediate:
-      self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) if mode == 'blended' else 0
-      self._pending_mode = mode
-      self._pending_count = 0
-      self._switch_mode(mode)
+  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
+    # Emergency override for critical situations (stops, collisions)
+    if emergency:
+      self.emergency_override = True
+      self.current_mode = mode
+      self.transition_timeout = SET_MODE_TIMEOUT
+      self.mode_duration = 0
       return
 
-    if cancel_hold and mode == 'acc':
-      self._blended_hold_frames = 0
+    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
+    for m in self.mode_confidence:
+      if m != mode:
+        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
 
-    if self._blended_hold_frames > 0:
-      mode = 'blended'
-
-    if mode == self.current_mode:
-      self._pending_mode = mode
-      self._pending_count = 0
+    # Require minimum duration in current mode (unless emergency)
+    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
       return
 
-    if mode != self._pending_mode:
-      self._pending_mode = mode
-      self._pending_count = 1
-    else:
-      self._pending_count += 1
+    # Hysteresis: higher threshold for mode changes
+    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
 
-    if self.mode_duration < WMACConstants.MIN_MODE_DURATION[self.current_mode]:
-      return
+    if self.mode_confidence[mode] > confidence_threshold:
+      if mode != self.current_mode and self.transition_timeout == 0:
+        self.transition_timeout = SET_MODE_TIMEOUT
+        self.current_mode = mode
+        self.mode_duration = 0
 
-    required_count = WMACConstants.ENTER_BLENDED_FRAMES if mode == 'blended' else WMACConstants.EXIT_BLENDED_FRAMES
-    if self._pending_count >= required_count:
-      self._switch_mode(mode)
-
-  def update(self) -> None:
-    if self._blended_hold_frames > 0:
-      self._blended_hold_frames -= 1
+  def update(self):
+    if self.transition_timeout > 0:
+      self.transition_timeout -= 1
     self.mode_duration += 1
 
+    # Reset emergency override after some time
+    if self.emergency_override and self.mode_duration > 20:
+      self.emergency_override = False
+
+    # Gradual confidence decay
+    for mode in self.mode_confidence:
+      self.mode_confidence[mode] *= 0.98
+
   def get_mode(self) -> ModeType:
     return self.current_mode
 
-  def _switch_mode(self, mode: ModeType) -> None:
-    if mode == self.current_mode:
-      return
-
-    self.current_mode = mode
-    self.mode_duration = 0
-    self._pending_mode = mode
-    self._pending_count = 0
-
 
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
@@ -132,32 +142,35 @@ class DynamicExperimentalController:
 
     self._mode_manager = ModeTransitionManager()
 
-    self._lead_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.LEAD_PROB,
-      exit_threshold=WMACConstants.LEAD_EXIT_PROB,
-      rise_rate=WMACConstants.LEAD_RISE_RATE,
-      fall_rate=WMACConstants.LEAD_FALL_RATE,
-    )
-    self._slow_down_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.SLOW_DOWN_PROB,
-      exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB,
-      rise_rate=WMACConstants.SLOW_DOWN_RISE_RATE,
-      fall_rate=WMACConstants.SLOW_DOWN_FALL_RATE,
-    )
-    self._slowness_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.SLOWNESS_PROB,
-      exit_threshold=WMACConstants.SLOWNESS_EXIT_PROB,
-      rise_rate=WMACConstants.SLOWNESS_RISE_RATE,
-      fall_rate=WMACConstants.SLOWNESS_FALL_RATE,
+    # Smooth filters for stable decision making with faster response for critical scenarios
+    self._lead_filter = SmoothKalmanFilter(
+      measurement_noise=0.15,
+      process_noise=0.05,
+      alpha=1.02,
+      smoothing_factor=0.8
     )
 
+    self._slow_down_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.1,
+      alpha=1.05,
+      smoothing_factor=0.7
+    )
+
+    self._slowness_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.06,
+      alpha=1.015,
+      smoothing_factor=0.92
+    )
+
+    self._mpc_fcw_filter = SmoothKalmanFilter(
+      measurement_noise=0.2,
+      process_noise=0.1,
+      alpha=1.1,
+      smoothing_factor=0.5
+    )
     self._has_lead_filtered = False
-    self._has_any_lead = False
-    self._has_current_radar_acc_lead = False
-    self._has_radar_acc_lead = False
-    self._radar_acc_lead_frames = 0
-    self._radar_fresh = True
-    self._radar_stale_frames = 0
     self._has_slow_down = False
     self._has_slowness = False
     self._has_mpc_fcw = False
@@ -166,18 +179,13 @@ class DynamicExperimentalController:
     self._has_standstill = False
     self._mpc_fcw_crash_cnt = 0
     self._standstill_count = 0
-
+    # debug
     self._endpoint_x = float('inf')
     self._expected_distance = 0.0
     self._trajectory_valid = False
-    self._raw_urgency = 0.0
-    self._model_accel_samples = deque(maxlen=WMACConstants.MODEL_DECEL_TREND_FRAMES)
-    self._model_decel_trending = False
-    self._model_decel_latched = False
-    self._planner_accel = math.nan
 
   def _read_params(self) -> None:
-    if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
+    if self._frame % int(1. / DT_MDL) == 0:
       self._enabled = self._params.get_bool("DynamicExperimentalControl")
 
   def mode(self) -> str:
@@ -190,221 +198,191 @@ class DynamicExperimentalController:
     return self._active
 
   def set_mpc_fcw_crash_cnt(self) -> None:
+    """Set MPC FCW crash count"""
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
 
-  def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None:
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
     car_state = sm['carState']
-    radar_state = sm['radarState']
-    lead_one = radar_state.leadOne
-    lead_two = radar_state.leadTwo
+    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_standstill = car_state.standstill
 
+    # standstill detection
     if self._has_standstill:
-      self._standstill_count = min(WMACConstants.STANDSTILL_FRAMES * 3, self._standstill_count + 1)
+      self._standstill_count = min(20, self._standstill_count + 1)
     else:
       self._standstill_count = max(0, self._standstill_count - 1)
 
-    self._radar_fresh = bool(radar_fresh)
-    if self._radar_fresh:
-      self._radar_stale_frames = 0
-      self._has_lead_filtered = self._lead_tracker.update(float(lead_one.present))
-      self._has_any_lead = bool(lead_one.present or lead_two.present)
-      self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two)))
-      self._update_radar_acc_lead()
-    else:
-      self._radar_stale_frames += 1
-      self._has_current_radar_acc_lead = False
-      if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES:
-        self._update_radar_acc_lead()
-      else:
-        self._lead_tracker.reset()
-        self._has_lead_filtered = False
-        self._has_any_lead = False
-        self._has_radar_acc_lead = False
-        self._radar_acc_lead_frames = 0
-    self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0
+    # Lead detection
+    self._lead_filter.add_data(float(lead_one.present))
+    lead_value = self._lead_filter.get_value() or 0.0
+    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
+
+    # MPC FCW detection
+    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
+    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
+    self._has_mpc_fcw = fcw_filtered_value > 0.5
+
+    # Slow down detection
     self._calculate_slow_down(md)
 
-    if self._standstill_count > WMACConstants.STANDSTILL_FRAMES or self._has_slow_down:
-      self._slowness_tracker.reset()
-      self._has_slowness = False
-    else:
+    # Slowness detection
+    if not (self._standstill_count > 5) and not self._has_slow_down:
       current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
-      self._has_slowness = self._slowness_tracker.update(current_slowness)
+      self._slowness_filter.add_data(current_slowness)
+      slowness_value = self._slowness_filter.get_value() or 0.0
 
-  def _calculate_slow_down(self, md) -> None:
+      # Hysteresis for slowness
+      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
+      self._has_slowness = slowness_value > threshold
+
+  def _calculate_slow_down(self, md):
+    """Calculate urgency based on trajectory endpoint vs expected distance."""
+
+    # Reset to safe defaults
+    urgency = 0.0
     self._endpoint_x = float('inf')
-    self._expected_distance = 0.0
     self._trajectory_valid = False
 
-    self._update_model_decel_trend(md)
-    urgency = self._model_action_urgency(md)
-    position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE
+    #Require exact trajectory size
+    position_valid = len(md.position.x) == TRAJECTORY_SIZE
+    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
 
-    if position_valid:
-      self._trajectory_valid = True
-      self._endpoint_x = md.position.x[WMACConstants.TRAJECTORY_SIZE - 1]
-      self._expected_distance = interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST)
-      urgency = max(urgency, self._endpoint_urgency(self._endpoint_x, self._expected_distance))
+    if not (position_valid and orientation_valid):
+      # Invalid trajectory - this itself might indicate a stop scenario
+      # Apply moderate urgency for incomplete trajectories at speed
+      if self._v_ego_kph > 20.0:
+        urgency = 0.3
 
-    self._raw_urgency = clip01(urgency)
-    self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
-    self._urgency = self._slow_down_tracker.value
-
-  def _update_model_decel_trend(self, md) -> None:
-    try:
-      desired_accel = float(md.action.desiredAcceleration)
-    except (AttributeError, OverflowError, TypeError, ValueError):
-      desired_accel = math.nan
-    if not math.isfinite(desired_accel):
-      self._reset_model_decel_trend()
-    else:
-      self._model_accel_samples.append(desired_accel)
-    history = tuple(self._model_accel_samples)
-    self._model_decel_trending = (len(history) == self._model_accel_samples.maxlen
-                                  and history[-1] <= WMACConstants.MODEL_DECEL_TREND_ACCEL
-                                  and (history[0] - history[-1]) / (DT_MDL * (len(history) - 1)) > WMACConstants.MODEL_DECEL_TREND_RATE
-                                  and all(after <= before for before, after in zip(history[:-1], history[1:], strict=True))
-                                  and sum(after < before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
-    if len(history) == self._model_accel_samples.maxlen and all(
-      accel >= WMACConstants.MODEL_DECEL_TREND_RELEASE_ACCEL for accel in history
-    ):
-      self._model_decel_latched = False
-
-  def _reset_model_decel_trend(self) -> None:
-    self._model_accel_samples.clear()
-    self._model_decel_trending = False
-    self._model_decel_latched = False
-
-  def _radar_acc_lead_score(self, lead_one) -> float:
-    radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
-    return float(lead_one.present and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
-
-  def _update_radar_acc_lead(self) -> None:
-    if self._has_current_radar_acc_lead:
-      self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES
-      self._has_radar_acc_lead = True
+      self._slow_down_filter.add_data(urgency)
+      urgency_filtered = self._slow_down_filter.get_value() or 0.0
+      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
+      self._urgency = urgency_filtered
       return
 
-    if not self._has_any_lead:
-      self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES)
+    # We have a valid full trajectory
+    self._trajectory_valid = True
 
-    self._has_radar_acc_lead = self._radar_acc_lead_frames > 0
-    self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1)
+    # Use the exact endpoint (33rd point, index 32)
+    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
+    self._endpoint_x = endpoint_x
 
-  def _model_action_urgency(self, md) -> float:
-    action = getattr(md, 'action', None)
-    if action is None:
-      return 0.0
+    # Get expected distance based on current speed using tuned constants
+    expected_distance = interp(self._v_ego_kph,
+                               WMACConstants.SLOW_DOWN_BP,
+                               WMACConstants.SLOW_DOWN_DIST)
+    self._expected_distance = expected_distance
 
-    urgency = 1.0 if getattr(action, 'shouldStop', False) else 0.0
-    desired_accel = getattr(action, 'desiredAcceleration', 0.0)
-    if desired_accel < WMACConstants.MODEL_DECEL_START:
-      urgency = max(urgency, min(1.0, (WMACConstants.MODEL_DECEL_START - desired_accel) / WMACConstants.MODEL_DECEL_RANGE))
-    return urgency
+    # Calculate urgency based on trajectory shortage
+    if endpoint_x < expected_distance:
+      shortage = expected_distance - endpoint_x
+      shortage_ratio = shortage / expected_distance
 
-  def _endpoint_urgency(self, endpoint_x: float, expected_distance: float) -> float:
-    if endpoint_x >= expected_distance:
-      return 0.0
+      # Base urgency on shortage ratio
+      urgency = min(1.0, shortage_ratio * 2.0)
 
-    shortage_ratio = (expected_distance - endpoint_x) / expected_distance
-    urgency = min(1.0, shortage_ratio * WMACConstants.ENDPOINT_URGENCY_GAIN)
+      # Increase urgency for very short trajectories (imminent stops)
+      critical_distance = expected_distance * 0.3
+      if endpoint_x < critical_distance:
+        urgency = min(1.0, urgency * 2.0)
 
-    if endpoint_x < expected_distance * WMACConstants.CRITICAL_ENDPOINT_FACTOR:
-      urgency = min(1.0, urgency * WMACConstants.CRITICAL_URGENCY_GAIN)
+      # Speed-based urgency adjustment
+      if self._v_ego_kph > 25.0:
+        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
+        urgency = min(1.0, urgency * speed_factor)
 
-    if self._v_ego_kph > WMACConstants.SPEED_URGENCY_MIN:
-      speed_factor = 1.0 + (self._v_ego_kph - WMACConstants.SPEED_URGENCY_MIN) / WMACConstants.SPEED_URGENCY_RANGE
-      urgency = min(1.0, urgency * speed_factor)
+    # Apply filtering but with less smoothing for stops
+    self._slow_down_filter.add_data(urgency)
+    urgency_filtered = self._slow_down_filter.get_value() or 0.0
 
-    return urgency
+    # Update state with lower threshold for better stop detection
+    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
+    self._urgency = urgency_filtered
 
-  def _model_decel_handoff_ready(self) -> bool:
-    try:
-      mpc_accel = float(self._mpc.a_solution[1])
-      return (math.isfinite(mpc_accel) and mpc_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
-              and math.isfinite(self._planner_accel) and self._planner_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
-              and self._planner_accel - self._model_accel_samples[-1] <= WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP)
-    except (AttributeError, IndexError, OverflowError, TypeError, ValueError):
-      return False
-
-  def _lead_dropout_model_handoff_ready(self) -> bool:
-    if (not self._active or self._CP.radarUnavailable or not self._radar_fresh or self._has_any_lead or not self._has_radar_acc_lead
-        or self._mode_manager.get_mode() != 'acc' or self._mpc.last_solution_status != 0):
-      return False
-    try:
-      model_accel = float(self._model_accel_samples[-1])
-    except (IndexError, OverflowError, TypeError, ValueError):
-      return False
-    return (math.isfinite(model_accel) and math.isfinite(self._planner_accel)
-            and self._planner_accel <= WMACConstants.MODEL_DECEL_START
-            and model_accel <= WMACConstants.MODEL_DECEL_START
-            and model_accel <= self._planner_accel + WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP)
-
-  def _desired_mode(self) -> tuple[ModeType, bool]:
-    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
-    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
-
-    if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
-      self._reset_model_decel_trend()
-      return 'acc', True
-
-    radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1
-    if (radar_stale or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down):
-      self._radar_acc_lead_frames = 0
-      self._has_radar_acc_lead = False
-      return 'blended', True
-
-    if self._lead_dropout_model_handoff_ready():
-      self._radar_acc_lead_frames = 0
-      self._has_radar_acc_lead = False
-      self._model_decel_latched = True
-      return 'blended', True
-
-    if not self._CP.radarUnavailable and self._has_radar_acc_lead:
-      self._reset_model_decel_trend()
-      return 'acc', True
-
-    entering_model_slowdown = self._model_decel_trending and self._model_decel_handoff_ready() and not self._model_decel_latched
-    self._model_decel_latched |= entering_model_slowdown
-    if self._model_decel_latched:
-      return 'blended', entering_model_slowdown
+  def _radarless_mode(self) -> None:
+    """Radarless mode decision logic with emergency handling."""
 
+    # EMERGENCY: MPC FCW - immediate blended mode
     if self._has_mpc_fcw:
-      return 'blended', True
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
+
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
+
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.5)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
+
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
+
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
+
+  def _radar_mode(self) -> None:
+    """Radar mode with emergency handling."""
+
+    # EMERGENCY: MPC FCW - immediate blended mode
+    if self._has_mpc_fcw:
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
+
+    # If lead detected and not in standstill: always use ACC
+    if self._has_lead_filtered and not (self._standstill_count > 3):
+      self._mode_manager.request_mode('acc', confidence=1.0)
+      return
+
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.3)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
+
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
+
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
+
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self._read_params()
+
+    self.set_mpc_fcw_crash_cnt()
+
+    self._update_calculations(sm)
 
     if self._CP.radarUnavailable:
-      if standstill or self._has_slow_down:
-        return 'blended', urgent_slow_down
-      return 'acc', False
+      self._radarless_mode()
+    else:
+      self._radar_mode()
 
-    if standstill or self._has_slow_down:
-      return 'blended', urgent_slow_down
-
-    return 'acc', False
-
-  def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True, planner_accel: float | None = None) -> None:
-    self._read_params()
-    self.set_mpc_fcw_crash_cnt()
-    try:
-      self._planner_accel = float(planner_accel)
-    except (OverflowError, TypeError, ValueError):
-      self._planner_accel = math.nan
-    self._update_calculations(sm, radar_fresh)
-    self._active = sm['selfdriveState'].experimentalMode and self._enabled
-    if not self._active:
-      model_decel_latched = self._model_decel_latched
-      self._reset_model_decel_trend()
-      if model_decel_latched:
-        self._mode_manager.request_mode('acc', immediate=True)
-
-    mode, immediate = self._desired_mode()
-    self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
-                                    cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead)
     self._mode_manager.update()
-
+    self._active = sm['selfdriveState'].experimentalMode and self._enabled
     self._frame += 1

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

@@ -0,0 +1,94 @@
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+
+class MockLeadOne:
+  def __init__(self, status=0.0):
+    self.status = status
+
+class MockRadarState:
+  def __init__(self, status=0.0):
+    self.leadOne = MockLeadOne(status=status)
+
+class MockCarState:
+  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
+    self.vEgo = vEgo
+    self.vCruise = vCruise
+    self.standstill = standstill
+
+class MockModelData:
+  def __init__(self, valid=True):
+    size = 33 if valid else 10  # incomplete if invalid
+    self.position = type("Pos", (), {"x": [0.0] * size})()
+    self.orientation = type("Ori", (), {"x": [0.0] * size})()
+
+class MockSelfDriveState:
+  def __init__(self, experimentalMode=False):
+    self.experimentalMode = experimentalMode
+
+class MockParams:
+  def get_bool(self, name):
+    return True
+
+@pytest.fixture
+def default_sm():
+  sm = {
+    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
+    'radarState': MockRadarState(status=1.0),
+    'modelV2': MockModelData(valid=True),
+    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+  }
+  return sm
+
+@pytest.fixture
+def mock_cp():
+  class CP:
+    radarUnavailable = False
+  return CP()
+
+@pytest.fixture
+def mock_mpc():
+  class MPC:
+    crash_cnt = 0
+  return MPC()
+
+# Fake Kalman Filter that always returns a given value
+class FakeKalman:
+  def __init__(self, value=1.0):
+    self.value = value
+  def add_data(self, v): pass
+  def get_value(self): return self.value
+  def get_confidence(self): return 1.0
+  def reset_data(self): pass
+
+def test_initial_mode_is_acc(mock_cp, mock_mpc):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  assert controller.mode() == "acc"
+
+def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['carState'].standstill = True
+  for _ in range(10):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  mock_mpc.crash_cnt = 1  # simulate FCW
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+
+  # Force conditions to simulate slowdown
+  controller._slow_down_filter = FakeKalman(value=1.0)  # ty: ignore[invalid-assignment]
+  controller._v_ego_kph = 35.0
+  default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert controller.mode() == "blended"

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -1,685 +0,0 @@
-import pytest
-
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
-
-
-class MockLeadOne:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1):
-    self.present = status
-    self.dRel = dRel
-    self.vRel = vRel
-    self.radar = radar
-    self.radarTrackId = radarTrackId
-
-
-class MockRadarState:
-  def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1, leadTwo=None):
-    self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId)
-    self.leadTwo = leadTwo if leadTwo is not None else MockLeadOne()
-
-
-class MockCarState:
-  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
-    self.vEgo = vEgo
-    self.vCruise = vCruise
-    self.standstill = standstill
-
-
-class MockAction:
-  def __init__(self, desiredAcceleration=0.0, shouldStop=False):
-    self.desiredAcceleration = desiredAcceleration
-    self.shouldStop = shouldStop
-
-
-class MockModelData:
-  def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False):
-    position_size = 33 if valid else 10
-    orientation_size = position_size if orientation_valid is None else (33 if orientation_valid else 10)
-    position_x = [0.0] * position_size
-    if position_x:
-      position_x[-1] = endpoint_x
-    self.position = type("Pos", (), {"x": position_x})()
-    self.orientation = type("Ori", (), {"x": [0.0] * orientation_size})()
-    self.acceleration = type("Accel", (), {"x": [0.0] * position_size})()
-    self.action = MockAction(desired_acceleration, should_stop)
-
-
-class MockSelfDriveState:
-  def __init__(self, experimentalMode=False):
-    self.experimentalMode = experimentalMode
-
-
-class MockParams:
-  def get_bool(self, name):
-    return True
-
-
-@pytest.fixture
-def default_sm():
-  sm = {
-    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
-    'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7),
-    'modelV2': MockModelData(valid=True),
-    'selfdriveState': MockSelfDriveState(experimentalMode=True),
-  }
-  return sm
-
-
-@pytest.fixture
-def mock_cp():
-  class CP:
-    radarUnavailable = False
-  return CP()
-
-
-@pytest.fixture
-def mock_mpc():
-  class MPC:
-    crash_cnt = 0
-    a_solution = [0.0, 0.0]
-    last_solution_status = 0
-  return MPC()
-
-
-def test_initial_mode_is_acc(mock_cp, mock_mpc):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  assert controller.mode() == "acc"
-
-
-def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['carState'].standstill = True
-  for _ in range(20):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-
-def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  mock_mpc.crash_cnt = 1
-  controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-
-def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  controller.update(default_sm)
-
-  assert controller.mode() == "blended"
-
-
-def test_valid_position_with_missing_orientation_can_trigger_slowdown(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, orientation_valid=False)
-
-  controller.update(default_sm)
-
-  assert controller._trajectory_valid
-  assert controller.mode() == "blended"
-
-
-def test_incomplete_position_does_not_trigger_slowdown(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=False, endpoint_x=0.0)
-
-  for _ in range(3):
-    controller.update(default_sm)
-
-  assert not controller._trajectory_valid
-  assert not controller._has_slow_down
-  assert controller.mode() == "acc"
-
-
-def test_slowdown_hysteresis_prevents_threshold_chatter():
-  signal = HysteresisSignal(enter_threshold=0.5, exit_threshold=0.4, rise_rate=1.0, fall_rate=1.0)
-
-  assert signal.update(0.55)
-  assert signal.update(0.45)
-  assert not signal.update(0.35)
-
-
-def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
-
-  controller.update(default_sm)
-
-  assert not controller._trajectory_valid
-  assert controller.mode() == "blended"
-
-
-def test_confirmed_model_decel_trend_enters_blended_before_a_large_command(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-
-  for desired_acceleration in (-0.02, -0.05, -0.08):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.0)
-    assert controller.mode() == "acc"
-
-  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
-  controller.update(default_sm, planner_accel=0.0)
-
-  assert controller._model_decel_trending
-  assert not controller._has_slow_down
-  assert controller.mode() == "blended"
-
-
-def test_confirmed_model_decel_handoff_stays_latched_through_a_plateau(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-
-  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.0)
-
-  for _ in range(WMACConstants.EMERGENCY_HOLD_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES + 1):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
-    controller.update(default_sm, planner_accel=0.0)
-
-  assert not controller._model_decel_trending
-  assert controller._model_decel_latched
-  assert controller.mode() == "blended"
-
-  for _ in range(WMACConstants.MODEL_DECEL_TREND_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
-    controller.update(default_sm, planner_accel=0.0)
-
-  assert not controller._model_decel_latched
-  assert controller.mode() == "acc"
-
-
-def test_model_decel_trend_never_overrides_a_radar_lead(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-
-  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm)
-
-  assert not controller._model_accel_samples
-  assert not controller._model_decel_latched
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_radar_acquisition_clears_a_latched_model_decel_handoff(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.0)
-  assert controller._model_decel_latched
-
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  controller.update(default_sm, planner_accel=0.0)
-
-  assert not controller._model_accel_samples
-  assert not controller._model_decel_latched
-  assert controller.mode() == "acc"
-
-
-def test_model_decel_trend_does_not_accumulate_while_dec_is_inactive(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['selfdriveState'].experimentalMode = False
-  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.0)
-
-  assert not controller._model_accel_samples
-  assert not controller._model_decel_latched
-
-  default_sm['selfdriveState'].experimentalMode = True
-  controller.update(default_sm, planner_accel=0.0)
-  assert not controller._model_decel_trending
-  assert controller.mode() == "acc"
-
-
-def test_disabling_dec_clears_a_latched_model_decel_mode(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.0)
-  assert controller._model_decel_latched
-  assert controller.mode() == "blended"
-
-  default_sm['selfdriveState'].experimentalMode = False
-  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
-  controller.update(default_sm, planner_accel=0.0)
-
-  assert not controller._model_decel_latched
-  assert controller.mode() == "acc"
-
-
-def test_model_decel_trend_waits_while_mpc_is_accelerating(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  mock_mpc.a_solution[1] = 0.5
-
-  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.0)
-
-  assert controller._model_decel_trending
-  assert controller.mode() == "acc"
-
-
-def test_steep_model_decel_trend_defers_to_the_existing_urgent_path(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-
-  for desired_acceleration in (0.0, -0.2, -0.4, -0.6):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.05)
-
-  assert controller._model_decel_trending
-  assert controller.mode() == "acc"
-
-
-def test_model_decel_trend_waits_while_the_planner_is_accelerating(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-
-  for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm, planner_accel=0.2)
-
-  assert controller._model_decel_trending
-  assert controller.mode() == "acc"
-
-
-def test_alternating_model_accel_noise_does_not_trigger_an_early_handoff(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-
-  for desired_acceleration in (0.0, -0.2, 0.0, -0.2):
-    default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
-    controller.update(default_sm)
-
-  assert not controller._model_decel_trending
-  assert controller.mode() == "acc"
-
-
-def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  for _ in range(3):
-    controller.update(default_sm)
-
-  assert controller._has_slow_down
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_far_radar_lead_always_uses_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0, radar=True)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  controller.update(default_sm)
-
-  assert controller._has_lead_filtered
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_radar_acquisition_immediately_returns_blended_to_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, radar=True, radarTrackId=7)
-  controller.update(default_sm)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True)
-  for _ in range(20):
-    controller.update(default_sm)
-  assert controller.mode() == "acc"
-
-
-def test_close_vision_only_lead_can_use_blended(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-
-def test_second_radar_lead_forces_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  lead_two = MockLeadOne(status=1.0, dRel=120.0, radar=True, radarTrackId=8)
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0, leadTwo=lead_two)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_second_vision_only_lead_does_not_force_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  lead_two = MockLeadOne(status=1.0, dRel=20.0, vRel=-10.0)
-  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-
-def test_inactive_lead_with_radar_marker_does_not_force_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0, radar=True, radarTrackId=7)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-
-def test_radarless_car_ignores_marked_radar_track(mock_cp, mock_mpc, default_sm):
-  mock_cp.radarUnavailable = True
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-
-def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0, radarTrackId=7)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  for _ in range(20):
-    controller.update(default_sm)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  default_sm['carState'].standstill = True
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True)
-  mock_mpc.crash_cnt = 1
-
-  for _ in range(10):
-    controller.update(default_sm)
-
-  assert controller._has_lead_filtered
-  assert controller._has_mpc_fcw
-  assert controller.mode() == "acc"
-
-
-def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
-  for _ in range(2):
-    controller.update(default_sm)
-  assert controller._has_slow_down
-
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  controller.update(default_sm)
-
-  assert controller._has_lead_filtered
-  assert controller.mode() == "acc"
-
-
-def test_braking_model_takes_over_on_the_first_fresh_full_lead_dropout(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1)
-  controller.update(default_sm, planner_accel=-1.2)
-
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  controller.update(default_sm, planner_accel=-1.2)
-
-  assert not controller._has_radar_acc_lead
-  assert controller._model_decel_latched
-  assert controller.mode() == "blended"
-
-
-@pytest.mark.parametrize(("model_accel", "planner_accel"), ((-0.75, -1.1), (-0.3, -1.1), (-1.1, -0.3)))
-def test_lead_dropout_guard_stays_active_without_a_matching_brake(model_accel, planner_accel, mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  controller.update(default_sm, planner_accel=-1.1)
-  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=model_accel)
-  default_sm['radarState'] = MockRadarState(status=0.0)
-
-  controller.update(default_sm, planner_accel=planner_accel)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_failed_mpc_keeps_the_lead_dropout_guard(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  controller.update(default_sm, planner_accel=-1.1)
-  mock_mpc.last_solution_status = 1
-  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1)
-  default_sm['radarState'] = MockRadarState(status=0.0)
-
-  controller.update(default_sm, planner_accel=-1.1)
-
-  assert controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-def test_matching_brake_without_a_prior_radar_lead_does_not_use_dropout_handoff(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1)
-
-  controller.update(default_sm, planner_accel=-1.1)
-
-  assert not controller._has_radar_acc_lead
-  assert not controller._model_decel_latched
-  assert controller.mode() == "acc"
-
-
-def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
-  for _ in range(2):
-    controller.update(default_sm)
-  assert controller._has_slow_down
-
-  default_sm['radarState'] = MockRadarState(status=1.0)
-  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
-    controller.update(default_sm)
-    assert controller._has_radar_acc_lead
-    assert controller.mode() == "acc"
-
-  controller.update(default_sm)
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "acc"
-
-  for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1):
-    controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-
-def test_radar_lead_short_dropout_guard_expires_without_any_lead(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  controller.update(default_sm)
-
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES):
-    controller.update(default_sm)
-    assert controller._has_radar_acc_lead
-
-  controller.update(default_sm)
-  assert not controller._has_radar_acc_lead
-
-
-def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  controller.update(default_sm)
-
-  controller.update(default_sm, radar_fresh=False)
-
-  assert not controller._has_current_radar_acc_lead
-  assert controller._has_radar_acc_lead
-  assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1
-  assert controller._radar_stale_frames == 1
-  assert controller.mode() == "acc"
-
-
-def test_one_stale_radar_frame_does_not_override_retained_lead_for_model_urgency(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  controller.update(default_sm)
-  default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
-
-  controller.update(default_sm, radar_fresh=False)
-  assert controller.mode() == "acc"
-
-  controller.update(default_sm, radar_fresh=False)
-  assert controller.mode() == "blended"
-
-
-def test_one_stale_radar_frame_does_not_delay_fcw(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  controller.update(default_sm)
-  mock_mpc.crash_cnt = 1
-
-  controller.update(default_sm, radar_fresh=False)
-
-  assert controller.mode() == "blended"
-
-
-def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  controller.update(default_sm)
-
-  for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1):
-    controller.update(default_sm, radar_fresh=False)
-    assert controller._has_radar_acc_lead
-
-  controller.update(default_sm, radar_fresh=False)
-
-  assert not controller._has_current_radar_acc_lead
-  assert not controller._has_radar_acc_lead
-  assert not controller._has_any_lead
-  assert not controller._has_lead_filtered
-
-
-def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  controller.update(default_sm)
-  for _ in range(WMACConstants.RADAR_STALE_FRAMES):
-    controller.update(default_sm, radar_fresh=False)
-
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm, radar_fresh=False)
-  assert controller.mode() == "blended"
-
-  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
-  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
-  controller.update(default_sm, radar_fresh=True)
-
-  assert controller._radar_stale_frames == 0
-  assert controller._has_current_radar_acc_lead
-  assert controller.mode() == "acc"
-
-
-@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"])
-def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  controller.update(default_sm)
-
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  if urgent_source == "fcw":
-    mock_mpc.crash_cnt = 1
-  else:
-    default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
-  controller.update(default_sm)
-
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-  mock_mpc.crash_cnt = 0
-  default_sm['modelV2'] = MockModelData(valid=True)
-  controller.update(default_sm)
-  assert controller.mode() == "blended"
-
-
-def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
-  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-  assert controller._has_current_radar_acc_lead
-  assert controller.mode() == "acc"
-
-  default_sm['radarState'] = MockRadarState(status=1.0)
-  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
-    controller.update(default_sm)
-    assert controller._has_radar_acc_lead
-    assert controller.mode() == "acc"
-
-
-def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-
-  for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2):
-    if frame % 2 == 0:
-      default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0))
-    else:
-      default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8))
-    controller.update(default_sm)
-
-    assert controller._has_current_radar_acc_lead
-    assert controller.mode() == "acc"
-
-
-def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(mock_cp, mock_mpc, default_sm):
-  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
-  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  controller.update(default_sm)
-
-  default_sm['radarState'] = MockRadarState(status=1.0)
-  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1):
-    controller.update(default_sm)
-  assert not controller._has_radar_acc_lead
-  assert controller.mode() == "blended"
-
-  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
-  default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two)
-  controller.update(default_sm)
-
-  assert controller._has_current_radar_acc_lead
-  assert controller.mode() == "acc"

openpilot/sunnypilot/selfdrive/controls/lib/longcontrol.py

@@ -1,51 +0,0 @@
-"""
-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 math
-
-STOPPING_DISTANCE = 0.75
-STOPPING_TIME = 2.5
-STOPPING_ACCEL_TOLERANCE = 0.1
-STOPPING_SPEED_TOLERANCE = 0.05
-STOPPING_SETTLE_FRAMES = 30
-
-
-class LongControlSP:
-  def __init__(self):
-    self._stopping_settle_frames: int | None = None
-
-  def update_state(self, stopping: bool) -> None:
-    if not stopping:
-      self._stopping_settle_frames = None
-
-  def stopping_decel_rate(self, CS, a_target: float) -> float:
-    if not all(math.isfinite(value) for value in (self.last_output_accel, a_target, CS.vEgo, CS.aEgo)):
-      return 1.0
-    can_hold = self.last_output_accel <= 0.0 and a_target >= self.last_output_accel
-    terminal_speed = (0.0 <= CS.vEgo <= STOPPING_SPEED_TOLERANCE
-                      or CS.standstill and abs(CS.vEgo) <= STOPPING_SPEED_TOLERANCE)
-    if self.last_output_accel > 0.0 or CS.vEgo < 0.0 and not terminal_speed:
-      return 1.0
-    if terminal_speed and self._stopping_settle_frames is None:
-      if not can_hold or self.last_output_accel > -STOPPING_ACCEL_TOLERANCE or CS.aEgo >= -STOPPING_ACCEL_TOLERANCE:
-        return 1.0
-      self._stopping_settle_frames = 0
-
-    time_decel = 0.0 if self._stopping_settle_frames is not None else CS.vEgo / STOPPING_TIME
-    required_decel = max(time_decel, CS.vEgo ** 2 / (2.0 * STOPPING_DISTANCE), 1e-3)
-    adequacy = min(max(-CS.aEgo / required_decel, 0.0), 1.0)
-    if not terminal_speed and self._stopping_settle_frames is None and can_hold and adequacy >= 1.0:
-      self._stopping_settle_frames = 0
-
-    motion_need = 1.0 - adequacy ** 2
-    planner_need = min(max((self.last_output_accel - a_target) / max(required_decel, STOPPING_ACCEL_TOLERANCE), 0.0), 1.0)
-    terminal_need = 0.0
-    if terminal_speed or self._stopping_settle_frames not in (None, 0):
-      self._stopping_settle_frames = min(self._stopping_settle_frames + 1, STOPPING_SETTLE_FRAMES)
-      terminal_need = (self._stopping_settle_frames / STOPPING_SETTLE_FRAMES) ** 2
-
-    return max(motion_need, planner_need, terminal_need)

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -1,46 +0,0 @@
-"""
-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 numpy as np
-
-from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
-
-
-class LongitudinalMpcSP:
-  def __init__(self) -> None:
-    self._accel_max_trajectory: tuple[float, ...] | None = None
-    self._cruise_accel_max: float | None = None
-    self._jerk_cost_multiplier = 1.0
-    self.last_solution_status = 0
-
-  def set_accel_controller_params(self, accel_max: tuple[float, ...] | None, jerk_cost_multiplier: float,
-                                  cruise_accel_max: float | None = None) -> None:
-    self._accel_max_trajectory = accel_max
-    self._cruise_accel_max = cruise_accel_max
-    self._jerk_cost_multiplier = jerk_cost_multiplier
-
-  def cruise_accel_max(self, stock_accel_max: float) -> float:
-    if self._cruise_accel_max is None or not np.isfinite(self._cruise_accel_max):
-      return stock_accel_max
-    return min(max(self._cruise_accel_max, 0.0), stock_accel_max)
-
-  def scale_jerk_cost(self, jerk_cost: float) -> float:
-    return jerk_cost * self._jerk_cost_multiplier
-
-  def apply_accel_limits(self) -> None:
-    if self._accel_max_trajectory is None:
-      return
-
-    accel_max = np.asarray(self._accel_max_trajectory)
-    if accel_max.shape != self.params[:, 1].shape or accel_max.dtype.kind not in "iuf" or not np.all(np.isfinite(accel_max)):
-      return
-
-    self.params[:, 1] = np.clip(accel_max, 0.0, ACCEL_MAX)
-    self.params[0, 1] = max(self.params[0, 1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX)))
-
-  def save_solution_status(self) -> None:
-    self.last_solution_status = self.solution_status

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -5,15 +5,10 @@ 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 math
-
 from openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -27,10 +22,9 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
-    self.mpc = mpc
-    self.accel_controller = AccelController(CP, dt=dt)
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
     self.events_sp = EventsSP()
+    self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
@@ -38,15 +32,9 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
-    self._radar_log_mono_time = None
-    self._radar_fresh_this_cycle = True
-    self._long_active_last_cycle = False
-    self._accel_controller_actuating = False
 
     self.output_v_target = 0.
     self.output_a_target = 0.
-    self.previous_plan_accel = 0.
-    self.mpc_accel_seed = 0.
 
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
@@ -55,44 +43,6 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
-  def update_accel_controller(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool,
-                              stock_accel_max: float, reset_state: bool) -> tuple[bool, float]:
-    is_e2e = self.is_e2e(sm)
-    force_decel = sm['controlsState'].forceDecel
-    previous_mpc_failed = self.mpc.last_solution_status != 0
-    previous_plan_accel = self.previous_plan_accel if self._long_active_last_cycle and not previous_mpc_failed else float("inf")
-    self.mpc_accel_seed = self.a_desired
-
-    self.accel_controller.update(
-      sm['radarState'], base_speed=self.output_v_target, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
-      follow_personality=sm['selfdriveState'].personality, acc_selected=not is_e2e,
-      engaged=not reset_state and not force_decel, cruise_initialized=sm['carState'].vCruise != V_CRUISE_UNSET,
-      stock_accel_max=max(stock_accel_max, 0.0),
-      radar_fresh=self._radar_fresh_this_cycle, previous_mpc_source=self.mpc.source, planner_speed=self.v_desired_filter.x,
-      planner_accel=self.a_desired, previous_plan_accel=previous_plan_accel,
-    )
-    controller = self.accel_controller
-    acc_handoff = (controller.is_active and not is_e2e and not previous_mpc_failed and self.mpc.source == MpcLongitudinalPlanSource.e2e
-                   and math.isfinite(previous_plan_accel))
-    if acc_handoff:
-      self.mpc_accel_seed = min(self.a_desired, previous_plan_accel)
-    actuating = controller.is_active and not is_e2e and not force_decel and not previous_mpc_failed
-    self._accel_controller_actuating = actuating
-    valid_lead_stop_hold = actuating and controller.state == AccelControllerState.stopHold and controller.selected_lead >= 0
-    controller_v_cruise = v_cruise if valid_lead_stop_hold else min(v_cruise, controller.output_v_target) if actuating else v_cruise
-    accel_max = controller.mpc_accel_max if actuating else None
-    cruise_accel_max = controller.cruise_accel_max if actuating else None
-    jerk_cost_multiplier = controller.get_jerk_cost_multiplier(
-      actuating, prev_accel_constraint, v_cruise - controller_v_cruise, previous_mpc_failed,
-    )
-    self.mpc.set_accel_controller_params(accel_max, jerk_cost_multiplier, cruise_accel_max)
-    self._long_active_last_cycle = not reset_state and not force_decel
-    return is_e2e, controller_v_cruise
-
-  def update_should_stop(self, should_stop: bool) -> bool:
-    departure_authorized = self._accel_controller_actuating and self._radar_fresh_this_cycle and self.mpc.last_solution_status == 0
-    return self.accel_controller.update_should_stop(should_stop, departure_authorized)
-
   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)
@@ -123,20 +73,9 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
-  def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
-    radar_log_mono_time = sm.logMonoTime['radarState']
-    radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
-    radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
-    if radar_advanced:
-      self._radar_log_mono_time = radar_log_mono_time
-    return radar_healthy and radar_advanced
-
   def update(self, sm: messaging.SubMaster) -> None:
-    self.previous_plan_accel = self.output_a_target
-    self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
-    self.accel_controller.update_params()
     self.events_sp.clear()
-    self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target)
+    self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
@@ -156,12 +95,6 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
-    accelController = longitudinalPlanSP.accelController
-    accelController.enabled = self.accel_controller.is_enabled
-    accelController.active = self.accel_controller.is_active
-    accelController.profile = self.accel_controller.profile
-    accelController.state = self.accel_controller.state
-
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

@@ -4,7 +4,6 @@ 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 types import SimpleNamespace
 from typing import Any
 
 import numpy as np
@@ -16,12 +15,8 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import (
-  _A_LAT_REG_MAX, _BELOW_EGO_TARGET_RELEASE_RATE, _ENTERING_PRED_LAT_ACC_TH, _MIN_ACTIVATION_SPEED,
-  _RELIEF_CONFIRMATION_FRAMES, _TARGET_RELEASE_RATE, SmartCruiseControlVision,
-)
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
 
 VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
 
@@ -125,21 +120,6 @@ class TestSmartCruiseControlVision:
   def reset_params(self):
     self.params.put_bool("SmartCruiseControlVision", True, block=True)
 
-  def set_lat_accels(self, current: float, predicted: float, v_ego: float = 20., model_speed: float = 20.) -> None:
-    self.sm['controlsState'].curvature = current / v_ego**2
-    self.sm['modelV2'].velocity.x = [model_speed] * len(ModelConstants.T_IDXS)
-    self.sm['modelV2'].orientationRate.z = [predicted / model_speed] * len(ModelConstants.T_IDXS)
-
-  def update_lat_accels(self, current: float, predicted: float, cruise: float = 30., a_ego: float = 0.,
-                        v_ego: float = 20., model_speed: float = 20.) -> None:
-    self.set_lat_accels(current, predicted, v_ego, model_speed)
-    self.scc_v.update(self.sm, True, False, v_ego, a_ego, cruise)
-
-  def enter_curve(self, predicted: float = 2.2) -> None:
-    self.update_lat_accels(0.5, predicted)
-    self.update_lat_accels(0.5, predicted)
-    assert self.scc_v.state == VisionState.entering
-
   def test_initial_state(self):
     assert self.scc_v.state == VisionState.disabled
     assert not self.scc_v.is_active
@@ -165,253 +145,6 @@ class TestSmartCruiseControlVision:
       self.scc_v.update(self.sm, True, False, 0., 0., 0.)
     assert self.scc_v.state == VisionState.enabled
 
-  def test_unconfirmed_leaving_and_reentry_only_shape_speed(self):
-    self.enter_curve()
-    targets = [self.scc_v.output_v_target]
-
-    self.update_lat_accels(2., 2.2, a_ego=-0.8)
-    assert self.scc_v.state == VisionState.turning
-    assert self.scc_v.output_a_target == -0.8
-    targets.append(self.scc_v.output_v_target)
-
-    self.update_lat_accels(1.2, 1.2, a_ego=0.3)
-    assert self.scc_v.state == VisionState.leaving
-    assert self.scc_v.output_a_target == 0.3
-    targets.append(self.scc_v.output_v_target)
-
-    self.update_lat_accels(1., 3., a_ego=-1.2)
-    assert self.scc_v.state == VisionState.entering
-    assert self.scc_v.output_a_target == -1.2
-    targets.append(self.scc_v.output_v_target)
-
-    entering, turning, leaving, reentering = targets
-    assert turning == pytest.approx(entering)
-    assert 0. < leaving - turning <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
-    assert reentering < leaving
-
-  def test_new_curve_interrupts_confirmed_release_immediately(self):
-    self.enter_curve()
-    for _ in range(_RELIEF_CONFIRMATION_FRAMES + 1):
-      self.update_lat_accels(0.8, 0.8)
-    releasing_v_target = self.scc_v.output_v_target
-    assert self.scc_v.state == VisionState.leaving
-
-    self.update_lat_accels(0.8, 3., a_ego=-0.7)
-    assert self.scc_v.state == VisionState.entering
-    assert self.scc_v.output_v_target < releasing_v_target
-    assert self.scc_v.output_a_target == -0.7
-
-  @pytest.mark.parametrize("planner_accel", (-2., -0.5, 0., 0.8))
-  def test_planner_acceleration_passes_through_exactly(self, planner_accel):
-    self.enter_curve()
-    self.update_lat_accels(0.5, 2.2, a_ego=planner_accel)
-    assert self.scc_v.output_a_target == planner_accel
-
-  def test_planner_acceleration_passes_through_all_states(self):
-    cases = (
-      (False, False, 0.5, 2.2, -0.2, VisionState.disabled),
-      (True, False, 0.5, 0.8, 0.1, VisionState.enabled),
-      (True, False, 0.5, 2.2, -0.4, VisionState.entering),
-      (True, False, 2., 2.2, -0.8, VisionState.turning),
-      (True, False, 1.2, 1.2, 0.3, VisionState.leaving),
-      (True, True, 1.2, 1.2, 0.6, VisionState.overriding),
-    )
-    for long_enabled, override, current, predicted, planner_accel, state in cases:
-      self.set_lat_accels(current, predicted)
-      self.scc_v.update(self.sm, long_enabled, override, 20., planner_accel, 30.)
-      assert self.scc_v.state == state
-      assert self.scc_v.output_a_target == planner_accel
-
-  def test_jitter_requires_confirmed_relief_then_releases_smoothly(self):
-    self.enter_curve()
-    previous_v_target = self.scc_v.output_v_target
-
-    for frame in range(_RELIEF_CONFIRMATION_FRAMES * 2):
-      self.update_lat_accels(1., 1.05 if frame % 2 == 0 else 1.15)
-      assert self.scc_v.state == VisionState.entering
-      assert self.scc_v.output_v_target >= previous_v_target
-      assert self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
-      previous_v_target = self.scc_v.output_v_target
-
-    for _ in range(_RELIEF_CONFIRMATION_FRAMES):
-      self.update_lat_accels(1.15, 0.8)
-      assert self.scc_v.state == VisionState.entering
-      assert 0. <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
-      previous_v_target = self.scc_v.output_v_target
-
-    release_cruise = 30.
-    for _ in range(_RELIEF_CONFIRMATION_FRAMES - 1):
-      self.update_lat_accels(0.8, 0.8, release_cruise)
-      assert self.scc_v.state == VisionState.entering
-      assert 0. <= self.scc_v.output_v_target - previous_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
-      previous_v_target = self.scc_v.output_v_target
-
-    active_v_targets = [previous_v_target]
-    for _ in range(int((release_cruise - previous_v_target) / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
-      self.update_lat_accels(0.8, 0.8, release_cruise)
-      if not self.scc_v.is_active:
-        break
-      assert self.scc_v.state == VisionState.leaving
-      assert self.scc_v.output_v_target != V_CRUISE_UNSET
-      active_v_targets.append(self.scc_v.output_v_target)
-
-    assert self.scc_v.state == VisionState.enabled
-    assert self.scc_v.output_v_target == V_CRUISE_UNSET
-    assert active_v_targets[-1] == pytest.approx(release_cruise)
-    assert np.all((np.diff(active_v_targets) >= 0.) &
-                  (np.diff(active_v_targets) <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9))
-
-  def test_target_release_slows_after_reaching_ego_speed(self):
-    self.enter_curve()
-
-    for _ in range(100):
-      previous_v_target = self.scc_v.output_v_target
-      self.update_lat_accels(0.8, 0.8)
-      if previous_v_target >= self.scc_v.v_ego:
-        rise = self.scc_v.output_v_target - previous_v_target
-        assert 0. < rise <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9
-        break
-    else:
-      pytest.fail("curve target did not release to ego speed")
-
-  def test_curve_target_is_independent_of_ego_speed(self):
-    model_speed = 24.
-    predicted_yaw_rate = 0.12
-    predicted_lat_accel = model_speed * predicted_yaw_rate
-    expected_v_target = (_A_LAT_REG_MAX / (predicted_yaw_rate / model_speed)) ** 0.5
-    targets = []
-
-    for v_ego in (18., 28.):
-      controller = SmartCruiseControlVision()
-      self.set_lat_accels(0.5, predicted_lat_accel, v_ego, model_speed)
-      controller.update(self.sm, True, False, v_ego, 0., 30.)
-      controller.update(self.sm, True, False, v_ego, 0., 30.)
-      assert controller.state == VisionState.entering
-      targets.append(controller.v_target)
-
-    assert targets[0] == pytest.approx(expected_v_target)
-    assert targets[1] == pytest.approx(expected_v_target)
-
-  def test_curve_target_respects_minimum_speed_floor(self):
-    model_speed = 10.
-    predicted_yaw_rate = 2.
-    self.set_lat_accels(0.5, model_speed * predicted_yaw_rate, model_speed=model_speed)
-    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
-    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
-
-    assert self.scc_v.state == VisionState.entering
-    assert self.scc_v.v_target < MIN_V
-    assert self.scc_v.output_v_target == pytest.approx(MIN_V)
-
-  @pytest.mark.parametrize(
-    ("velocities", "yaw_rates"),
-    [([], []), ([np.nan] * len(ModelConstants.T_IDXS), [np.nan] * len(ModelConstants.T_IDXS)), ([20.] * 5, [0.1] * 3)],
-    ids=("empty", "nonfinite", "mismatched"),
-  )
-  def test_model_vector_edges_remain_finite(self, velocities, yaw_rates):
-    self.sm['modelV2'].velocity.x = velocities
-    self.sm['modelV2'].orientationRate.z = yaw_rates
-    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
-    self.scc_v.update(self.sm, True, False, 20., 0., 30.)
-
-    assert all(np.isfinite(value) for value in (
-      self.scc_v.current_lat_acc, self.scc_v.max_pred_lat_acc, self.scc_v.v_target,
-      self.scc_v.output_v_target, self.scc_v.output_a_target,
-    ))
-
-  @pytest.mark.parametrize("launch_speed", (5.75, 9.9, _MIN_ACTIVATION_SPEED))
-  def test_vision_control_does_not_steal_launch(self, launch_speed):
-    self.set_lat_accels(0.5, 3., launch_speed)
-    self.scc_v.update(self.sm, True, False, launch_speed, 0., 30.)
-    self.scc_v.update(self.sm, True, False, launch_speed, 0., 30.)
-
-    assert launch_speed <= _MIN_ACTIVATION_SPEED
-    assert self.scc_v.state == VisionState.enabled
-    assert not self.scc_v.is_active
-    assert self.scc_v.output_v_target == V_CRUISE_UNSET
-
-  def test_vision_control_can_activate_above_launch_range(self):
-    speed = _MIN_ACTIVATION_SPEED + 0.01
-    self.set_lat_accels(0.5, 3., speed)
-    self.scc_v.update(self.sm, True, False, speed, 0., 30.)
-    self.scc_v.update(self.sm, True, False, speed, 0., 30.)
-
-    assert self.scc_v.state == VisionState.entering
-    assert self.scc_v.is_active
-
-  def test_sequential_curve_tightens_immediately_and_releases_bounded(self):
-    self.enter_curve(3.)
-    for _ in range(20):
-      self.update_lat_accels(0.5, 3.)
-    restrictive_v_target = self.scc_v.output_v_target
-
-    self.update_lat_accels(0.5, 1.4, a_ego=0.4)
-    first_relief_v_target = self.scc_v.output_v_target
-    assert self.scc_v.state == VisionState.entering
-    assert 0. < first_relief_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
-    assert self.scc_v.output_a_target == 0.4
-
-    self.update_lat_accels(0.5, 1.4)
-    assert 0. <= self.scc_v.output_v_target - first_relief_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
-
-    self.update_lat_accels(0.5, 3., a_ego=-0.6)
-    assert self.scc_v.state == VisionState.entering
-    assert self.scc_v.output_v_target == pytest.approx(restrictive_v_target)
-    assert self.scc_v.output_a_target == -0.6
-
-    for _ in range(4):
-      self.update_lat_accels(0.5, 1.4)
-      assert 0. < self.scc_v.output_v_target - restrictive_v_target <= _BELOW_EGO_TARGET_RELEASE_RATE * DT_MDL + 1e-9
-      self.update_lat_accels(0.5, 3.)
-      assert self.scc_v.output_v_target == pytest.approx(restrictive_v_target)
-
-  def test_acceleration_is_continuous_through_planner_arbitration(self):
-    car_control = messaging.new_message('carControl')
-    car_control.carControl.enabled = True
-    car_control.carControl.cruiseControl.override = False
-    self.sm['carControl'] = car_control.carControl
-    self.sm['carState'].vCruiseCluster = 108.
-
-    planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-    planner.scc = SimpleNamespace(
-      vision=self.scc_v,
-      map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=0.),
-      update=lambda sm, enabled, override, v_ego, a_ego, v_cruise: self.scc_v.update(
-        sm, enabled, override, v_ego, a_ego, v_cruise),
-    )
-    planner.resolver = SimpleNamespace(
-      speed_limit_valid=False, speed_limit_last_valid=False, speed_limit=0., speed_limit_final_last=0., distance=0.,
-      update=lambda _v_ego, _sm: None,
-    )
-    planner.sla = SimpleNamespace(
-      output_v_target=V_CRUISE_UNSET, output_a_target=0., update=lambda *_args: None,
-    )
-    planner.events_sp = SimpleNamespace()
-
-    self.set_lat_accels(0.5, 2.2)
-    planner.update_targets(self.sm, 20., -0.8, 30.)
-    planner.update_targets(self.sm, 20., -0.8, 30.)
-    assert planner.source == LongitudinalPlanSource.sccVision
-    assert planner.output_a_target == -0.8
-
-    for planner_accel in (-2., 0.5, -0.2):
-      planner.update_targets(self.sm, 20., planner_accel, 30.)
-      assert planner.source == LongitudinalPlanSource.sccVision
-      assert planner.output_a_target == planner_accel
-
-    self.set_lat_accels(0.8, 0.8)
-    for _ in range(int(30. / (_TARGET_RELEASE_RATE * DT_MDL)) + 10):
-      planner.update_targets(self.sm, 20., 0.4, 30.)
-      assert planner.output_a_target == 0.4
-      if planner.source == LongitudinalPlanSource.cruise:
-        break
-    else:
-      pytest.fail("SCC Vision did not release to cruise")
-
-    planner.update_targets(self.sm, 20., 0.4, 30.)
-    assert self.scc_v.state == VisionState.enabled
-    assert planner.source == LongitudinalPlanSource.cruise
-
   @pytest.mark.parametrize(
     "case, should_enter",
     [

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller_closed_loop.py

@@ -1,82 +0,0 @@
-"""
-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 gc
-
-import numpy as np
-
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import _A_LAT_REG_MAX
-from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP as Plant
-
-
-def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.) -> dict[str, np.ndarray]:
-  gc.collect()
-  curvature = 0.005
-  plant = Plant(lead_relevancy=False, speed=30., actuator_delay=0.15, actuator_lag=0.20)
-  planner = plant.planner
-  planner.accel_controller.enabled = False
-  planner.accel_controller.update_params = lambda: None
-  planner.dec._enabled = False
-  planner.dec._read_params = lambda: None
-  planner.scc.map.enabled = False
-  planner.scc.map.update_params = lambda: None
-  planner.scc.vision.enabled = scc_enabled
-  planner.scc.vision._update_params = lambda: None
-
-  if scc_enabled:
-    original_update_calculations = planner.scc.vision._update_calculations
-
-    def inject_constant_curvature(sm):
-      velocities = np.asarray(sm['modelV2'].velocity.x, dtype=float)
-      sm['modelV2'].orientationRate.z = (curvature * velocities).tolist()
-      sm['controlsState'].curvature = curvature
-      original_update_calculations(sm)
-
-    planner.scc.vision._update_calculations = inject_constant_curvature
-
-  original_update = planner.update
-
-  def enable_longitudinal(sm):
-    sm['carControl'].enabled = True
-    sm['carControl'].longActive = True
-    original_update(sm)
-
-  planner.update = enable_longitudinal
-  rows = []
-  while plant.current_time < duration:
-    output = plant.step(v_cruise=cruise)
-    rows.append((
-      plant.current_time, output['speed'], planner.mpc.last_solution_status, output['should_stop'],
-      planner.scc.vision.is_active, planner.source == LongitudinalPlanSource.sccVision,
-      planner.scc.vision.output_v_target,
-    ))
-
-  data = np.asarray(rows, dtype=float)
-  gc.collect()
-  return {
-    'time': data[:, 0], 'speed': data[:, 1], 'solver_status': data[:, 2], 'should_stop': data[:, 3],
-    'active': data[:, 4], 'scc_source': data[:, 5], 'target': data[:, 6],
-  }
-
-
-def test_constant_curve_recovers_like_stock_speed_cap():
-  target = (_A_LAT_REG_MAX / 0.005) ** 0.5
-  scc = _run_constant_curve(scc_enabled=True, cruise=30.)
-  stock = _run_constant_curve(scc_enabled=False, cruise=target)
-  scc_final = scc['speed'][scc['time'] >= 60.]
-  stock_final = stock['speed'][stock['time'] >= 60.]
-
-  assert not scc['solver_status'].any()
-  assert not stock['solver_status'].any()
-  assert not scc['should_stop'].any()
-  assert np.all(scc['active'][scc['time'] >= 60.])
-  assert np.all(scc['scc_source'][scc['time'] >= 60.])
-  assert np.allclose(scc['target'][scc['time'] >= 60.], target)
-  assert scc_final.min() >= target - 1.
-  assert abs(scc_final.mean() - stock_final.mean()) < 0.5
-  assert abs(scc_final.min() - stock_final.min()) < 1.
-  assert abs(scc_final.max() - stock_final.max()) < 1.

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -29,11 +29,19 @@ _FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cyc
 
 _A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
 
-_RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL)))
-_TARGET_RELEASE_RATE = 1.  # m/s^2
-_BELOW_EGO_TARGET_RELEASE_RATE = 3.  # m/s^2
-_MIN_PRED_SPEED = 1.  # m/s
-_MIN_ACTIVATION_SPEED = 10.  # m/s
+_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
+
+# Lookup table for the minimum smooth deceleration during the ENTERING state
+# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
+_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
+_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
+
+# Lookup table for the acceleration for the TURNING state
+# depending on the current lateral acceleration of the vehicle.
+_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
+_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
+
+_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
 
 
 class SmartCruiseControlVision:
@@ -57,26 +65,13 @@ class SmartCruiseControlVision:
     self.state = VisionState.disabled
     self.current_lat_acc = 0.
     self.max_pred_lat_acc = 0.
-    self.relief_frames = 0
-
-  def _v_demand(self) -> float:
-    return max(MIN_V, min(self.v_target, self.v_cruise_setpoint))
-
-  def _released_v_target(self) -> float:
-    demand = self._v_demand()
-    if demand < self.output_v_target:
-      return demand
-    release_rate = _BELOW_EGO_TARGET_RELEASE_RATE if self.output_v_target < min(self.v_ego, demand) else _TARGET_RELEASE_RATE
-    return min(demand, self.output_v_target + release_rate * DT_MDL)
 
   def get_a_target_from_control(self) -> float:
-    return self.a_ego
+    return self.a_target
 
   def get_v_target_from_control(self) -> float:
     if self.is_active:
-      if self.output_v_target == V_CRUISE_UNSET:
-        return self._v_demand()
-      return self._released_v_target()
+      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
 
     return V_CRUISE_UNSET
 
@@ -87,27 +82,25 @@ class SmartCruiseControlVision:
   def _update_calculations(self, sm: messaging.SubMaster) -> None:
     if not self.long_enabled:
       return
+    else:
+      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
+      vel_plan = np.array(sm['modelV2'].velocity.x)
 
-    rate_plan = np.asarray(np.abs(sm['modelV2'].orientationRate.z), dtype=float)
-    vel_plan = np.asarray(sm['modelV2'].velocity.x, dtype=float)
-    size = min(len(rate_plan), len(vel_plan))
-    rate_plan, vel_plan = rate_plan[:size], vel_plan[:size]
-    valid = np.isfinite(rate_plan) & np.isfinite(vel_plan) & (vel_plan >= _MIN_PRED_SPEED)
+      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
 
-    self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
-    self.max_pred_lat_acc = 0.
-    self.v_target = V_CRUISE_UNSET
-    if np.any(valid):
-      self.max_pred_lat_acc = float(np.percentile(rate_plan[valid] * vel_plan[valid], 97))
-      max_pred_curvature = float(np.percentile(rate_plan[valid] / vel_plan[valid], 97))
-      if max_pred_curvature > 0.:
-        self.v_target = min(float((_A_LAT_REG_MAX / max_pred_curvature) ** 0.5), V_CRUISE_UNSET)
+      # get the maximum lat accel from the model
+      predicted_lat_accels = rate_plan * vel_plan
+      self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
+
+      # get the maximum curve based on the current velocity
+      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
+      max_curve = self.max_pred_lat_acc / (v_ego**2)
+
+      # Get the target velocity for the maximum curve
+      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
 
   def _update_state_machine(self) -> tuple[bool, bool]:
     # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
-    relief = self.current_lat_acc < _FINISH_LAT_ACC_TH and self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH
-    self.relief_frames = self.relief_frames + 1 if self.state in ACTIVE_STATES and relief else 0
-
     if self.state != VisionState.disabled:
       # longitudinal and feature disable always have priority in a non-disabled state
       if not self.long_enabled or not self.enabled:
@@ -119,7 +112,7 @@ class SmartCruiseControlVision:
         # ENABLED
         if self.state == VisionState.enabled:
           # Do not enter a turn control cycle if the speed is low.
-          if self.v_ego <= _MIN_ACTIVATION_SPEED:
+          if self.v_ego <= MIN_V:
             pass
           # If significant lateral acceleration is predicted ahead, then move to Entering turn state.
           elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
@@ -135,26 +128,23 @@ class SmartCruiseControlVision:
           # Transition to Turning if current lateral acceleration is over the threshold.
           if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
             self.state = VisionState.turning
-          # Begin releasing only after both current and predicted lateral acceleration stay clear.
-          elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES:
-            self.state = VisionState.leaving
+          # Abort if the predicted lateral acceleration drops
+          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.enabled
 
         # TURNING
         elif self.state == VisionState.turning:
-          # Transition out of Turning if current lateral acceleration drops below a threshold.
+          # Transition to Leaving if current lateral acceleration drops below a threshold.
           if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
-            self.state = VisionState.entering if self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH else VisionState.leaving
+            self.state = VisionState.leaving
 
         # LEAVING
         elif self.state == VisionState.leaving:
           # Transition back to Turning if current lateral acceleration goes back over the threshold.
           if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
             self.state = VisionState.turning
-          # Start a new turn cycle immediately if another curve is predicted.
-          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.entering
-          # Finish after confirmed relief and a gradual release to the cruise setpoint.
-          elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and self.output_v_target >= self.v_cruise_setpoint:
+          # Finish if current lateral acceleration goes below a threshold.
+          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
             self.state = VisionState.enabled
 
     # DISABLED
@@ -167,11 +157,32 @@ class SmartCruiseControlVision:
 
     enabled = self.state in ENABLED_STATES
     active = self.state in ACTIVE_STATES
-    if not active:
-      self.relief_frames = 0
 
     return enabled, active
 
+  def _update_solution(self) -> float:
+    # DISABLED, ENABLED, OVERRIDING
+    if self.state not in ACTIVE_STATES:
+      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
+      # the smooth deceleration.
+      a_target = self.a_ego
+    # ENTERING
+    elif self.state == VisionState.entering:
+      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
+      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
+    # TURNING
+    elif self.state == VisionState.turning:
+      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
+      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
+    # LEAVING
+    elif self.state == VisionState.leaving:
+      # When leaving, we provide a comfortable acceleration to regain speed.
+      a_target = _LEAVING_ACC
+    else:
+      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
+
+    return a_target
+
   def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
              v_cruise_setpoint: float) -> None:
     self.long_enabled = long_enabled
@@ -184,7 +195,7 @@ class SmartCruiseControlVision:
     self._update_calculations(sm)
 
     self.is_enabled, self.is_active = self._update_state_machine()
-    self.a_target = self.a_ego
+    self.a_target = self._update_solution()
 
     self.output_v_target = self.get_v_target_from_control()
     self.output_a_target = self.get_a_target_from_control()

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py

@@ -1,391 +0,0 @@
-import numpy as np
-import pytest
-
-from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
-from opendbc.car.car_helpers import interfaces
-from opendbc.car.gm.values import CAR as GM
-from opendbc.car.honda.values import CAR as HONDA
-from opendbc.car.hyundai.values import CAR as HYUNDAI
-from opendbc.car.rivian.values import CAR as RIVIAN
-from opendbc.car.toyota.values import CAR as TOYOTA
-from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
-from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
-from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
-from openpilot.sunnypilot.selfdrive.controls.lib.longcontrol import STOPPING_SETTLE_FRAMES
-from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
-
-
-STOP_ACCEL_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1, RIVIAN.RIVIAN_R1)
-SETTLE_VEHICLES = (TOYOTA.TOYOTA_RAV4_TSS2, HONDA.HONDA_CIVIC_2022, VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1)
-ROUTE_STOP_ONSETS = (
-  (0.280, -0.290, -0.220, -0.220), (0.290, -0.497, -0.270, -0.302), (0.464, -0.223, -0.264, -0.292),
-  (0.467, -0.582, -0.316, -0.359),
-  (0.530, -0.311, -0.309, -0.333), (0.581, -0.467, -0.312, -0.352), (0.398, -0.557, -0.311, -0.348),
-  (0.517, -0.290, -0.301, -0.327), (0.312, -0.420, -0.271, -0.304), (0.474, -0.509, -0.303, -0.347),
-  (0.241, -0.554, -0.573, -0.617), (0.292, -0.154, -0.302, -0.326),
-)
-
-
-def get_car_params(candidate):
-  fingerprint = gen_empty_fingerprint()
-  interface = interfaces[candidate]
-  CP = interface.get_params(candidate, fingerprint, [], True, False, False)
-  return CP, interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False)
-
-
-def make_car_state(v_ego=0.2, a_ego=0.0, standstill=False) -> structs.CarState:
-  state = structs.CarState(vEgo=float(v_ego), aEgo=float(a_ego), standstill=standstill)
-  state.cruiseState.standstill = standstill
-  return state
-
-
-def make_control(candidate, initial_accel=-0.33):
-  CP, CP_SP = get_car_params(candidate)
-  control = LongControl(CP, CP_SP)
-  control.long_control_state = LongCtrlState.pid
-  control.last_output_accel = initial_accel
-  return CP, control
-
-
-def stock_stopping_output(output_accel, stop_accel):
-  return min(output_accel, 0.0) - DT_CTRL if output_accel > stop_accel else output_accel
-
-
-def test_stop_threshold_remains_unchanged():
-  assert should_stop(0.24, 0.0)
-  assert not should_stop(0.26, 0.0)
-  assert not should_stop(0.24, 0.1)
-
-
-@pytest.mark.parametrize(("v_ego", "a_ego", "a_target", "initial_accel"), ROUTE_STOP_ONSETS)
-def test_logged_stop_onsets_hold_the_existing_brake(v_ego, a_ego, a_target, initial_accel):
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
-  output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
-  assert control.long_control_state == LongCtrlState.stopping
-  assert output == pytest.approx(initial_accel)
-
-
-def test_glide_hold_survives_a_soft_deceleration_sample():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
-  samples = ((0.388, -0.201, -0.164), (0.330, -0.120, -0.140), (0.283, -0.0675, -0.120))
-  outputs = [control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
-             for v_ego, a_ego, a_target in samples]
-
-  assert outputs == pytest.approx([-0.166] * len(samples))
-
-
-def test_glide_response_reaches_the_stock_rate_when_deceleration_stops():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
-  control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
-  output = control.update(True, make_car_state(0.330, -0.01), -0.140, True, (-3.5, 2.0))
-
-  assert -0.176 < output < -0.175
-
-
-def test_glide_response_increases_with_stopping_distance_error():
-  _, nominal = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
-  _, distance_error = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
-  for control in (nominal, distance_error):
-    control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
-  nominal_output = nominal.update(True, make_car_state(0.330, -0.050), -0.140, True, (-3.5, 2.0))
-  distance_error_output = distance_error.update(True, make_car_state(0.400, -0.050), -0.140, True, (-3.5, 2.0))
-
-  assert -0.176 < distance_error_output < nominal_output
-
-
-@pytest.mark.parametrize(("decel_fraction", "expected_rate"), ((1.0, 0.0), (0.75, 0.4375), (0.5, 0.75), (0.0, 1.0)))
-def test_stopping_rate_scales_with_realized_deceleration(decel_fraction, expected_rate):
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  output = control.update(True, make_car_state(0.3, -0.12 * decel_fraction), 0.0, True, (-3.5, 2.0))
-
-  assert (-0.33 - output) / DT_CTRL == pytest.approx(expected_rate, abs=1e-6)
-
-
-def test_stopping_rate_scales_with_planner_demand():
-  _, gentle = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  _, urgent = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-
-  gentle_output = gentle.update(True, make_car_state(0.3, -0.12), -0.34, True, (-3.5, 2.0))
-  urgent_output = urgent.update(True, make_car_state(0.3, -0.12), -1.0, True, (-3.5, 2.0))
-
-  assert -0.331 < gentle_output < -0.33
-  assert urgent_output == pytest.approx(-0.34)
-
-
-def test_glide_hold_yields_to_stronger_planner_braking():
-  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -0.166)
-  control.update(True, make_car_state(0.388, -0.201), -0.164, True, (-3.5, 2.0))
-  output = control.update(True, make_car_state(0.330, -0.120), -1.0, True, (-3.5, 2.0))
-
-  assert output == pytest.approx(stock_stopping_output(-0.166, CP.stopAccel))
-
-
-@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
-def test_urgent_braking_matches_the_stock_ramp(candidate):
-  CP, control = make_control(candidate)
-  CS = make_car_state(0.8, -0.1)
-  output = control.last_output_accel
-
-  for _ in range(round(1.0 / DT_CTRL)):
-    output = control.update(True, CS, -3.0, True, (-3.5, 2.0))
-
-  expected = -0.33
-  for _ in range(round(1.0 / DT_CTRL)):
-    expected = stock_stopping_output(expected, CP.stopAccel)
-  assert output == pytest.approx(expected)
-
-
-@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
-def test_stronger_planner_brake_matches_the_stock_ramp(candidate):
-  CP, control = make_control(candidate)
-  outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0, True, (-3.5, 2.0)) for _ in range(10)]
-  expected = []
-  output = -0.33
-  for _ in range(10):
-    output = stock_stopping_output(output, CP.stopAccel)
-    expected.append(output)
-  assert outputs == pytest.approx(expected)
-
-
-@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
-def test_insufficient_deceleration_uses_most_of_the_stock_ramp(candidate):
-  CP, control = make_control(candidate)
-  output = control.update(True, make_car_state(0.6, -0.1), -0.1, True, (-3.5, 2.0))
-  if -0.33 > CP.stopAccel:
-    assert -0.34 < output < -0.338
-  else:
-    assert output == pytest.approx(-0.33)
-
-
-def test_deceleration_noise_cannot_release_the_brake():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  outputs = [control.update(True, make_car_state(0.3, -0.3 if frame % 2 else 0.0), -0.1, True, (-3.5, 2.0)) for frame in range(40)]
-  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
-
-
-def test_planner_noise_cannot_release_the_brake():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  outputs = [control.update(True, make_car_state(0.3, -0.3), -1.0 if frame % 2 else -0.1, True, (-3.5, 2.0)) for frame in range(40)]
-  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
-
-
-@pytest.mark.parametrize(("v_ego", "a_ego", "a_target"), (
-  (float("nan"), -0.3, -0.1), (0.3, float("nan"), -0.1), (0.3, -0.3, float("nan")),
-  (float("inf"), -0.3, -0.1), (0.3, -float("inf"), -0.1), (0.3, -0.3, float("inf")),
-))
-def test_invalid_state_uses_the_stock_ramp(v_ego, a_ego, a_target):
-  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  output = control.update(True, make_car_state(v_ego, a_ego), a_target, True, (-3.5, 2.0))
-  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
-
-
-@pytest.mark.parametrize(("speed", "initial_accel", "grade_accel", "actuator_lag", "actuator_delay"), (
-  (0.24, 0.0, -0.49, 0.15, 0.0), (0.53, -0.31, -0.49, 0.35, 0.1),
-  (0.24, 0.0, 0.0, 0.15, 0.0), (0.464, -0.223, 0.0, 0.25, 0.05), (0.53, -0.31, 0.0, 0.35, 0.1),
-  (0.24, 0.0, 0.49, 0.15, 0.0), (0.53, -0.31, 0.49, 0.25, 0.05), (0.6, -0.3, 0.49, 0.35, 0.1), (0.6, -0.3, 0.49, 0.5, 0.1),
-))
-def test_smooth_stop_distance_is_bounded(speed, initial_accel, grade_accel, actuator_lag, actuator_delay):
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
-  applied_accel = initial_accel
-  delay = [initial_accel] * round(actuator_delay / DT_CTRL)
-  distance = 0.0
-  outputs = []
-
-  for _ in range(round(4.0 / DT_CTRL)):
-    command = control.update(True, make_car_state(speed, applied_accel), -0.1, True, (-3.5, 2.0))
-    outputs.append(command)
-    delayed_command = command
-    if delay:
-      delay.append(command)
-      delayed_command = delay.pop(0)
-    applied_accel += DT_CTRL / actuator_lag * (delayed_command + grade_accel - applied_accel)
-    speed = max(0.0, speed + applied_accel * DT_CTRL)
-    distance += speed * DT_CTRL
-    if speed == 0.0:
-      break
-
-  assert speed == 0.0
-  assert distance < 1.0
-  assert all(current <= previous for previous, current in zip(outputs[:-1], outputs[1:], strict=True))
-
-
-@pytest.mark.parametrize("candidate", STOP_ACCEL_VEHICLES)
-def test_standstill_uses_the_stock_ramp(candidate):
-  CP, control = make_control(candidate)
-  control.long_control_state = LongCtrlState.off
-  CS = make_car_state(0.0, 0.0, standstill=True)
-  outputs = [control.update(True, CS, 0.0, False, (-3.5, 2.0)) for _ in range(round(2.0 / DT_CTRL))]
-  expected = -0.33
-  for _ in range(round(2.0 / DT_CTRL)):
-    expected = stock_stopping_output(expected, CP.stopAccel)
-  assert outputs[0] == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
-  assert outputs[-1] == pytest.approx(expected)
-
-
-@pytest.mark.parametrize("candidate", SETTLE_VEHICLES)
-def test_final_stop_builds_brake_smoothly_while_vehicle_settles(candidate):
-  _, control = make_control(candidate)
-  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
-  outputs = [control.update(True, make_car_state(0.0006, a_ego, standstill=True), -0.032, True, (-3.5, 2.0))
-             for a_ego in (-1.098, -0.950, -0.609, -0.286)]
-  changes = -np.diff([-0.33, *outputs])
-  assert np.all(changes > 0.0)
-  assert np.all(np.diff(changes) > 0.0)
-  assert changes[-1] < 0.001
-
-
-@pytest.mark.parametrize("a_ego", (-0.09, 0.0, 0.1))
-def test_settled_vehicle_uses_the_stock_hold_ramp(a_ego):
-  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  output = control.update(True, make_car_state(0.0, a_ego, standstill=True), -0.1, True, (-3.5, 2.0))
-  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
-
-
-@pytest.mark.parametrize("candidate", SETTLE_VEHICLES)
-def test_direct_terminal_entry_builds_brake_smoothly(candidate):
-  _, control = make_control(candidate)
-  CS = make_car_state(0.0006, -0.3, standstill=True)
-  outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(4)]
-
-  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
-  assert rates == pytest.approx([(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, 5)])
-
-
-def test_direct_terminal_entry_keeps_urgent_stock_braking():
-  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), -1.0, True, (-3.5, 2.0))
-
-  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
-
-
-@pytest.mark.parametrize("initial_accel", (0.0, -0.05))
-def test_direct_terminal_entry_first_builds_meaningful_brake(initial_accel):
-  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, initial_accel)
-  output = control.update(True, make_car_state(0.0006, -0.3, standstill=True), 0.0, True, (-3.5, 2.0))
-
-  assert output == pytest.approx(stock_stopping_output(initial_accel, CP.stopAccel))
-
-
-@pytest.mark.parametrize("candidate", SETTLE_VEHICLES)
-def test_final_settling_ramp_is_bounded(candidate):
-  _, control = make_control(candidate)
-  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
-  CS = make_car_state(0.0, -0.3, standstill=True)
-  outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
-
-  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
-  expected = [(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)] + [1.0]
-  assert rates == pytest.approx(expected)
-
-
-@pytest.mark.parametrize(("v_ego", "a_ego", "standstill"), ((0.6, -0.1, False), (0.0, 0.0, True)))
-def test_stopping_never_releases_a_stronger_command(v_ego, a_ego, standstill):
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2, -3.0)
-  output = control.update(True, make_car_state(v_ego, a_ego, standstill), 0.0, True, (-3.5, 2.0))
-  assert output == pytest.approx(-3.0)
-
-
-def test_reported_standstill_while_moving_can_hold_the_brake():
-  _, control = make_control(GM.CHEVROLET_BOLT_EUV)
-  control.long_control_state = LongCtrlState.off
-  output = control.update(True, make_car_state(0.3, -0.3, standstill=True), -0.1, False, (-3.5, 2.0))
-  assert output == pytest.approx(-0.33)
-
-
-def test_stopping_removes_positive_acceleration_immediately():
-  _, control = make_control(HYUNDAI.HYUNDAI_SONATA, 0.2)
-  output = control.update(True, make_car_state(0.2, -0.2), -0.1, True, (-3.5, 2.0))
-  assert output == pytest.approx(-DT_CTRL)
-
-
-def test_rollback_uses_the_stock_ramp():
-  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  output = control.update(True, make_car_state(-0.1, 0.1), -0.1, True, (-3.5, 2.0))
-  assert output == pytest.approx(stock_stopping_output(-0.33, CP.stopAccel))
-
-
-def test_rollback_after_settling_arms_uses_the_stock_ramp():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
-  control.update(True, make_car_state(0.01, -0.3), -0.1, True, (-3.5, 2.0))
-  previous = control.last_output_accel
-  output = control.update(True, make_car_state(-0.04, -0.3), -0.1, True, (-3.5, 2.0))
-
-  assert output == pytest.approx(previous - DT_CTRL)
-
-
-def test_small_velocity_noise_does_not_trigger_the_stock_rate():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
-  output = control.update(True, make_car_state(-0.04, -0.3, standstill=True), -0.1, True, (-3.5, 2.0))
-  assert -0.331 < output < -0.33
-
-
-def test_terminal_speed_chatter_cannot_extend_settling_ramp():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
-  outputs = [control.update(True, make_car_state(0.049 if frame % 2 == 0 else 0.051, -0.3), -0.1, True, (-3.5, 2.0))
-             for frame in range(STOPPING_SETTLE_FRAMES + 2)]
-
-  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
-  assert rates[:STOPPING_SETTLE_FRAMES] == pytest.approx([(frame / STOPPING_SETTLE_FRAMES) ** 2 for frame in range(1, STOPPING_SETTLE_FRAMES + 1)])
-  assert rates[-2:] == pytest.approx([1.0, 1.0])
-
-
-def test_terminal_speed_plateau_cannot_extend_settling_ramp():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
-  CS = make_car_state(0.03, -0.3)
-  outputs = [control.update(True, CS, -0.1, True, (-3.5, 2.0)) for _ in range(STOPPING_SETTLE_FRAMES + 1)]
-
-  rates = -np.diff([-0.33, *outputs]) / DT_CTRL
-  assert rates[-2:] == pytest.approx([1.0, 1.0])
-
-
-def test_interrupted_stop_cannot_reuse_settling_hold():
-  CP, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  control.update(True, make_car_state(0.28, -0.29), -0.22, True, (-3.5, 2.0))
-  control.update(False, make_car_state(0.0, 0.0, standstill=True), 0.0, False, (-3.5, 2.0))
-  output = control.update(True, make_car_state(0.0, -0.3, standstill=True), -0.1, True, (-3.5, 2.0))
-
-  assert output == pytest.approx(stock_stopping_output(0.0, CP.stopAccel))
-
-
-def test_departure_uses_the_stock_pid_path():
-  _, control = make_control(TOYOTA.TOYOTA_RAV4_TSS2)
-  control.long_control_state = LongCtrlState.stopping
-  output = control.update(True, make_car_state(0.0), 0.6, False, (-3.5, 2.0))
-  assert control.long_control_state == LongCtrlState.pid
-  assert output > 0.0
-
-
-def test_planner_mpc_and_longcontrol_complete_a_smooth_stop():
-  plant = PlantSP(
-    lead_relevancy=True, speed=0.6, distance_lead=3.6, run_long_control=True,
-    actuator_model=PRIUS_TSS2_ROUTE_MODEL,
-  )
-  plant.planner.accel_controller.enabled = True
-  plant.planner.accel_controller.profile = 1
-  plant.planner.accel_controller.update_params = lambda: None
-  plant.planner.dec._enabled = False
-  plant.planner.dec._read_params = lambda: None
-  commands = []
-  speeds = []
-  states = []
-  solver_statuses = []
-
-  while plant.current_time < 5.0:
-    result = plant.step(v_lead=0.0, v_cruise=8.0)
-    commands.append(result["actuator_command"])
-    speeds.append(result["speed"])
-    states.append(result["long_control_state"])
-    solver_statuses.append(plant.planner.mpc.last_solution_status)
-
-  stopping = states.index(LongCtrlState.stopping)
-  moving_stop_commands = [command for command, state, speed in zip(commands, states, speeds, strict=True)
-                          if state == LongCtrlState.stopping and speed > 0.02]
-  assert all(current <= previous + 1e-9 for previous, current in zip(commands[stopping:-1], commands[stopping + 1:], strict=True))
-  assert len(moving_stop_commands) > 1 and max(moving_stop_commands) - min(moving_stop_commands) < 1e-9
-  assert plant.speed == 0.0 and plant.distance < 1.0
-  assert plant.distance_lead - plant.distance > 3.0
-  assert all(status == 0 for status in solver_statuses)

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -1,395 +0,0 @@
-"""
-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 collections import deque
-from collections.abc import Callable
-from dataclasses import dataclass
-import math
-import time
-from typing import Any
-
-import numpy as np
-
-from openpilot.cereal import log, messaging
-from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
-from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
-from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
-from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
-from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
-
-
-LeadObservation = dict[str, Any]
-LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
-ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
-EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
-
-
-@dataclass(frozen=True)
-class ActuatorModel:
-  planner_delay: float
-  transport_delay: float
-  actuator_lag: float
-  command_rate_limit: float
-  stopping_acceleration: float
-  standstill_breakaway_acceleration: float
-  standstill_breakaway_time: float
-
-  def __post_init__(self):
-    nonnegative_fields = {
-      "planner_delay": self.planner_delay,
-      "transport_delay": self.transport_delay,
-      "actuator_lag": self.actuator_lag,
-      "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
-      "standstill_breakaway_time": self.standstill_breakaway_time,
-    }
-    if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
-      raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
-    if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
-      raise ValueError("command_rate_limit must be finite and positive")
-    if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
-      raise ValueError("stopping_acceleration must be finite and non-positive")
-
-
-# Conservative Prius TSS2 actuator model.
-PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
-  planner_delay=0.05,
-  transport_delay=0.0,
-  actuator_lag=0.20,
-  command_rate_limit=4.0,
-  stopping_acceleration=-2.0,
-  standstill_breakaway_acceleration=1.0,
-  standstill_breakaway_time=0.05,
-)
-
-
-class PlantSP(Plant):
-  """Closed-loop plant with configurable observations and actuator response."""
-
-  def __init__(
-    self,
-    lead_relevancy=False,
-    speed=0.0,
-    distance_lead=2.0,
-    enabled=True,
-    only_lead2=False,
-    only_radar=False,
-    e2e=False,
-    personality=0,
-    force_decel=False,
-    lead_observation_fn: LeadObservationFn | None = None,
-    model_action_fn: ModelActionFn | None = None,
-    ego_observation_fn: EgoObservationFn | None = None,
-    actuator_delay: float | None = None,
-    actuator_lag: float = 0.0,
-    actuator_model: ActuatorModel | None = None,
-    run_long_control: bool = False,
-  ):
-    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
-      raise ValueError("actuator_delay must be finite and non-negative")
-    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
-      raise ValueError("actuator_lag must be finite and non-negative")
-
-    self.rate = 1.0 / DT_MDL
-
-    if not Plant.messaging_initialized:
-      Plant.radar = messaging.pub_sock('radarState')
-      Plant.controls_state = messaging.pub_sock('controlsState')
-      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
-      Plant.car_state = messaging.pub_sock('carState')
-      Plant.plan = messaging.sub_sock('longitudinalPlan')
-      Plant.messaging_initialized = True
-
-    self.v_lead_prev = 0.0
-
-    self.distance = 0.0
-    self.speed = speed
-    self.should_stop = False
-    self.acceleration = 0.0
-    self.a_target = 0.0
-    self.actuator_command = 0.0
-    self.applied_actuator_command = 0.0
-    self.breakaway_confirmed = False
-    self._breakaway_timer = 0.0
-
-    # lead car
-    self.lead_relevancy = lead_relevancy
-    self.distance_lead = distance_lead
-    self.enabled = enabled
-    self.only_lead2 = only_lead2
-    self.only_radar = only_radar
-    self.e2e = e2e
-    self.personality = personality
-    self.force_decel = force_decel
-    self.lead_observation_fn = lead_observation_fn
-    self.model_action_fn = model_action_fn
-    self.ego_observation_fn = ego_observation_fn
-    self.actuator_model = actuator_model
-    self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
-    self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
-    self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
-    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
-                                      actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
-
-    self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
-    self.ts = 1.0 / self.rate
-    time.sleep(0.1)
-    self.sm = messaging.SubMaster(['longitudinalPlan'])
-
-    from opendbc.car.honda.values import CAR
-    from opendbc.car.honda.interface import CarInterface
-
-    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
-    if self.actuator_delay is not None:
-      CP.longitudinalActuatorDelay = self.actuator_delay
-    CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
-    self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
-    self.long_control = LongControl(CP, CP_SP) if run_long_control else None
-
-    if self.actuator_model is not None and self.speed >= 0.01:
-      self.breakaway_confirmed = True
-    self.integration_dt = DT_CTRL if run_long_control else self.ts
-    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
-    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
-
-  @staticmethod
-  def _lead_message(observation: LeadObservation):
-    lead = log.RadarState.LeadData.new_message()
-    for field, value in observation.items():
-      setattr(lead, field, value)
-    return lead
-
-  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
-    if self.lead_observation_fn is None:
-      return dict(truth) if present_by_default else None
-
-    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
-    if observed is None:
-      return None
-
-    complete_observation = dict(truth)
-    complete_observation.update(observed)
-    return complete_observation
-
-  def _update_actuator(self, command: float) -> tuple[float, float]:
-    if self._actuator_delay_queue:
-      self._actuator_delay_queue.append(command)
-      delayed_command = self._actuator_delay_queue.popleft()
-    else:
-      delayed_command = command
-
-    if self.actuator_model is not None:
-      max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
-      self.applied_actuator_command = float(np.clip(delayed_command,
-                                                   self.applied_actuator_command - max_command_delta,
-                                                   self.applied_actuator_command + max_command_delta))
-
-      if self.speed < 0.01:
-        if self.applied_actuator_command <= 0.0:
-          self.breakaway_confirmed = False
-          self._breakaway_timer = 0.0
-        elif not self.breakaway_confirmed:
-          breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
-          if breakaway_ready:
-            self._breakaway_timer += self.integration_dt
-          else:
-            self._breakaway_timer = 0.0
-
-          self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
-        if not self.breakaway_confirmed:
-          self.acceleration = 0.0
-          return delayed_command, self.acceleration
-      else:
-        self.breakaway_confirmed = True
-
-      response_command = self.applied_actuator_command
-    else:
-      self.applied_actuator_command = delayed_command
-      response_command = delayed_command
-
-    if self.actuator_lag > 0.0:
-      alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
-      self.acceleration += alpha * (response_command - self.acceleration)
-    else:
-      self.acceleration = response_command
-    return delayed_command, self.acceleration
-
-  def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
-    self.speed += self.acceleration * dt
-    if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
-      self.speed = self.acceleration = 0.0
-    self.distance += self.speed * dt
-
-  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
-    # ******** publish a fake model going straight and fake calibration ********
-    # note that this is worst case for MPC, since model will delay long mpc by one time step
-    radar = messaging.new_message('radarState')
-    control = messaging.new_message('controlsState')
-    ss = messaging.new_message('selfdriveState')
-    car_state = messaging.new_message('carState')
-    lp = messaging.new_message('liveParameters')
-    car_control = messaging.new_message('carControl')
-    model = messaging.new_message('modelV2')
-    car_state_sp = messaging.new_message('carStateSP')
-    live_map_data_sp = messaging.new_message('liveMapDataSP')
-    gps_data = messaging.new_message('gpsLocation')
-    a_lead = (v_lead - self.v_lead_prev) / self.ts
-    self.v_lead_prev = v_lead
-
-    if self.lead_relevancy:
-      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
-      v_rel = v_lead - self.speed
-      if self.only_radar:
-        status = True
-      elif prob_lead > 0.5:
-        status = True
-      else:
-        status = False
-    else:
-      d_rel = 200.0
-      v_rel = 0.0
-      prob_lead = 0.0
-      status = False
-
-    truth_lead: LeadObservation = {
-      "dRel": float(d_rel),
-      "yRel": 0.0,
-      "vRel": float(v_rel),
-      "vLead": float(v_lead),
-      "vLeadK": float(v_lead),
-      "aLeadK": float(a_lead),
-      "present": bool(status),
-      # TODO use real radard logic for this
-      "aLeadTau": float(_LEAD_ACCEL_TAU),
-      "modelProb": float(prob_lead),
-      "radar": bool(self.only_radar),
-      "radarTrackId": -1,
-    }
-    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
-    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
-    if lead_one_observation is not None:
-      radar.radarState.leadOne = self._lead_message(lead_one_observation)
-    if lead_two_observation is not None:
-      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
-
-    # Simulate model predicting slightly faster speed
-    # this is to ensure lead policy is effective when model
-    # does not predict slowdown in e2e mode
-    position = log.XYZTData.new_message()
-    position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
-    model.modelV2.position = position
-    if self.model_action_fn is None:
-      model_acceleration, model_should_stop = self.acceleration + 0.1, False
-    else:
-      model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
-    model.modelV2.action.desiredAcceleration = float(model_acceleration)
-    model.modelV2.action.shouldStop = bool(model_should_stop)
-    velocity = log.XYZTData.new_message()
-    velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
-    velocity.x[0] = float(self.speed)  # always start at current speed
-    model.modelV2.velocity = velocity
-    acceleration = log.XYZTData.new_message()
-    acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
-    model.modelV2.acceleration = acceleration
-    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
-
-    control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
-      LongCtrlState.pid if self.enabled else LongCtrlState.off)
-    ss.selfdriveState.experimentalMode = self.e2e
-    ss.selfdriveState.personality = self.personality
-    control.controlsState.forceDecel = self.force_decel
-    true_v_ego = self.speed
-    true_a_ego = self.acceleration
-    published_v_ego = true_v_ego
-    published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
-    if self.ego_observation_fn is not None:
-      published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
-    car_state.carState.vEgo = float(published_v_ego)
-    car_state.carState.aEgo = float(published_a_ego)
-    car_state.carState.standstill = bool(self.speed < 0.01)
-    car_state.carState.vCruise = float(v_cruise * 3.6)
-    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
-
-    # ******** get controlsState messages for plotting ***
-    sm = PlannerSM(self.rk.frame, {
-      'radarState': radar.radarState,
-      'carState': car_state.carState,
-      'carControl': car_control.carControl,
-      'controlsState': control.controlsState,
-      'selfdriveState': ss.selfdriveState,
-      'liveParameters': lp.liveParameters,
-      'modelV2': model.modelV2,
-      'carStateSP': car_state_sp.carStateSP,
-      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-      'gpsLocation': gps_data.gpsLocation,
-    })
-    self.planner.update(sm)
-    self.a_target = self.planner.output_a_target
-    if self.long_control is None:
-      self.actuator_command = self.a_target
-      if self.planner.output_should_stop:
-        stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
-        self.actuator_command = min(stopping_acceleration, self.actuator_command)
-      self._update_actuator(self.actuator_command)
-      self._integrate_ego(self.ts)
-    else:
-      for _ in range(round(self.ts / DT_CTRL)):
-        car_state.carState.vEgo = self.speed
-        car_state.carState.aEgo = self.acceleration
-        car_state.carState.standstill = self.speed < 0.01
-        self.actuator_command = self.long_control.update(
-          self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
-        )
-        self._update_actuator(self.actuator_command)
-        self._integrate_ego(DT_CTRL, stop_at_standstill=True)
-    self.should_stop = self.planner.output_should_stop
-    fcw = self.planner.fcw
-    self.distance_lead = self.distance_lead + v_lead * self.ts
-
-    # *** radar model ***
-    if self.lead_relevancy:
-      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
-      v_rel = v_lead - self.speed
-    else:
-      d_rel = 200.0
-      v_rel = 0.0
-
-    # print at 5hz
-    # if (self.rk.frame % (self.rate // 5)) == 0:
-    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
-    #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
-
-    # ******** update prevs ********
-    self.rk.monitor_time()
-
-    accel_controller = self.planner.accel_controller
-    return {
-      "distance": self.distance,
-      "speed": self.speed,
-      "acceleration": self.acceleration,
-      "realized_acceleration": self.acceleration,
-      "a_target": self.a_target,
-      "actuator_command": self.actuator_command,
-      "published_a_ego": published_a_ego,
-      "published_v_ego": published_v_ego,
-      "should_stop": self.should_stop,
-      "long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
-                             else control.controlsState.longControlState.raw),
-      "distance_lead": self.distance_lead,
-      "fcw": fcw,
-      "mpc_source": self.planner.mpc.source,
-      "dec_mode": self.planner.dec.mode(),
-      "controller_target": accel_controller.output_v_target,
-      "model_action": {
-        "desiredAcceleration": float(model_acceleration),
-        "shouldStop": bool(model_should_stop),
-      },
-      "truth_lead": dict(truth_lead),
-      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
-      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
-    }

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py

@@ -1,157 +0,0 @@
-from collections.abc import Callable
-import math
-from typing import cast
-
-import pytest
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
-from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
-
-STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
-
-
-def departing_lead(current_time: float) -> float:
-  return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
-
-
-def stopped_lead(_current_time: float) -> float:
-  return 0.0
-
-
-PARITY_SCENARIOS = {
-  "approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80},
-  "stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120},
-}
-
-
-def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs):
-  plant = cls(**kwargs)
-  plant.v_lead_prev = v_lead(0.0)
-  solver_failures = 0
-  original_reset = plant.planner.mpc.reset
-
-  def counting_reset(*args, **kw):
-    nonlocal solver_failures
-    if plant.planner.mpc.solution_status != 0:
-      solver_failures += 1
-    return original_reset(*args, **kw)
-
-  plant.planner.mpc.reset = counting_reset
-  results = []
-  for _ in range(steps):
-    lead_speed = v_lead(plant.current_time)
-    result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
-    results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
-  return results, solver_failures
-
-
-@pytest.mark.parametrize("scenario", PARITY_SCENARIOS, ids=list(PARITY_SCENARIOS))
-def test_plant_sp_matches_stock_plant_on_shared_kwargs(scenario: str):
-  kwargs = dict(PARITY_SCENARIOS[scenario])
-  v_cruise = cast(float, kwargs.pop("v_cruise"))
-  v_lead = cast(Callable[[float], float], kwargs.pop("v_lead"))
-  steps = cast(int, kwargs.pop("steps"))
-
-  stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
-  sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
-
-  assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
-  assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
-
-  for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
-    zip(stock_results, sp_results, strict=True),
-  ):
-    for key in STOCK_STEP_KEYS:
-      if isinstance(stock_result[key], float):
-        assert sp_result[key] == pytest.approx(stock_result[key]), f"{scenario} frame {frame} key {key}"
-      else:
-        assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
-    assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
-    assert sp_a_target == pytest.approx(stock_a_target), f"{scenario} frame {frame} output_a_target"
-
-  if scenario == "stop_then_depart":
-    departure_frame = round(1.0 / DT_MDL)
-    for results in (stock_results, sp_results):
-      assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
-      assert results[departure_frame - 1][0]["should_stop"]
-      assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
-      assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
-    stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and not result["should_stop"])
-    sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and not result["should_stop"])
-    stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and result["speed"] > 0.05)
-    sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and result["speed"] > 0.05)
-    assert sp_release == stock_release
-    assert sp_motion == stock_motion
-
-
-def test_full_lead_observation_is_independent_from_truth():
-  callback_inputs = []
-
-  def observe_lead(current_time, lead_name, truth):
-    callback_inputs.append((current_time, lead_name, truth))
-    if lead_name == "leadOne":
-      return {
-        "dRel": 12.5,
-        "vRel": -4.0,
-        "vLead": 6.0,
-        "vLeadK": 5.5,
-        "aLeadK": -1.25,
-        "aLeadTau": 0.7,
-        "present": True,
-        "modelProb": 0.9,
-        "radarTrackId": 42,
-      }
-    return None
-
-  plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
-  result = plant.step(v_lead=8.0)
-
-  assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
-  assert callback_inputs[0][2]["dRel"] == pytest.approx(50.0)
-  assert result["truth_lead"]["dRel"] == pytest.approx(50.0)
-  assert result["lead_one_observation"]["dRel"] == pytest.approx(12.5)
-  assert result["lead_one_observation"]["radarTrackId"] == 42
-  assert result["lead_two_observation"] is None
-  assert result["distance_lead"] == pytest.approx(50.0 + 8.0 * DT_MDL)
-
-
-def test_model_action_realized_acceleration_and_source_logging():
-  def model_action(current_time, v_ego, a_ego):
-    return -1.25, True
-
-  plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
-  first = plant.step()
-  second = plant.step()
-
-  assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
-  assert first["published_a_ego"] == pytest.approx(0.0)
-  assert second["published_a_ego"] == pytest.approx(first["realized_acceleration"])
-  assert first["acceleration"] == first["realized_acceleration"]
-  assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
-  assert first["mpc_source"] is not None
-  assert first["dec_mode"] in ("acc", "blended")
-  assert "controller_target" in first
-  assert first["lead_one_observation"] is not None
-  assert first["truth_lead"] == first["lead_one_observation"]
-
-
-def test_configurable_transport_delay_and_first_order_lag():
-  plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
-
-  assert plant.planner.CP.longitudinalActuatorDelay == pytest.approx(2 * DT_MDL)
-  delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
-  assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
-
-  expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
-  assert delayed_commands[2][0] == -1.0
-  assert delayed_commands[2][1] == pytest.approx(expected_acceleration)
-
-
-@pytest.mark.parametrize(
-  ("delay", "lag"),
-  [(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
-)
-def test_invalid_actuator_dynamics(delay, lag):
-  with pytest.raises(ValueError):
-    PlantSP(actuator_delay=delay, actuator_lag=lag)

openpilot/sunnypilot/sunnylink/capabilities.py

@@ -11,7 +11,6 @@ from opendbc.car.structs import car
 from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, UNSUPPORTED_LONGITUDINAL_CAR
 from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
 from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
-from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP, VIRTUAL_CRUISE_SPEED_CAR
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.hardware import HARDWARE
@@ -20,7 +19,6 @@ from openpilot.common.hardware import HARDWARE
 # Wire-protocol version for the capabilities payload. Bump on breaking changes
 # only; additive fields are backward-compatible and do not require a bump.
 PROTOCOL_VERSION = 1
-TOYOTA_VIRTUAL_CRUISE_SPEED_PLATFORMS = {str(platform) for platform in VIRTUAL_CRUISE_SPEED_CAR}
 
 # All capability fields that rules may reference.
 # Non-boolean fields must have defaults in CAPABILITY_DEFAULTS.
@@ -44,7 +42,6 @@ CAPABILITY_FIELDS = (
   "device_type",
   "subaru_has_sng",
   "hyundai_alpha_long_available",
-  "toyota_virtual_cruise_speed_available",
 )
 
 CAPABILITY_LABELS: dict[str, str] = {
@@ -67,7 +64,6 @@ CAPABILITY_LABELS: dict[str, str] = {
   "device_type": "Device type",
   "subaru_has_sng": "Subaru Stop-and-Go available",
   "hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
-  "toyota_virtual_cruise_speed_available": "Toyota Virtual Cruise Speed available",
 }
 
 # Explicit defaults for non-boolean capability fields
@@ -114,12 +110,6 @@ def _resolve_brand_capabilities(caps: dict, bundle_platform: str, CP) -> None:
       caps["subaru_has_sng"] = not bool(CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID))
       caps["has_stop_and_go"] = caps["subaru_has_sng"]
 
-  elif brand == "toyota":
-    if bundle_platform:
-      caps["toyota_virtual_cruise_speed_available"] = bundle_platform in TOYOTA_VIRTUAL_CRUISE_SPEED_PLATFORMS
-    elif CP is not None:
-      caps["toyota_virtual_cruise_speed_available"] = str(CP.carFingerprint) in TOYOTA_VIRTUAL_CRUISE_SPEED_PLATFORMS
-
 
 def generate_capabilities(params: Params | None = None) -> dict:
   """Generate a SettingsCapabilities dict from CarParams + boolean params.
@@ -184,8 +174,6 @@ def generate_capabilities(params: Params | None = None) -> dict:
       caps["icbm_available"] = bool(CP_SP.intelligentCruiseButtonManagementAvailable)
       caps["has_icbm"] = bool(CP_SP.intelligentCruiseButtonManagementAvailable) and params.get_bool("IntelligentCruiseButtonManagement")
       caps["tesla_has_vehicle_bus"] = bool(CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS)
-      if caps["brand"] == "toyota":
-        caps["toyota_virtual_cruise_speed_available"] = bool(CP_SP.flags & ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE)
     except Exception:
       cloudlog.exception("capabilities: failed to deserialize CarParamsSPPersistent")
 

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -652,58 +652,6 @@
                 }
               ]
             },
-            {
-              "key": "AccelPersonalityEnabled",
-              "widget": "toggle",
-              "title": "Enable Accel Controller",
-              "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.",
-              "visibility": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ],
-              "enablement": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                }
-              ]
-            },
-            {
-              "key": "AccelPersonality",
-              "widget": "multiple_button",
-              "title": "Acceleration Profile",
-              "description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
-              "options": [
-                {
-                  "value": 0,
-                  "label": "Eco"
-                },
-                {
-                  "value": 1,
-                  "label": "Normal"
-                },
-                {
-                  "value": 2,
-                  "label": "Sport"
-                }
-              ],
-              "enablement": [
-                {
-                  "type": "capability",
-                  "field": "has_longitudinal_control",
-                  "equals": true
-                },
-                {
-                  "type": "param",
-                  "key": "AccelPersonalityEnabled",
-                  "equals": true
-                }
-              ]
-            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",
@@ -764,21 +712,6 @@
                   "type": "capability",
                   "field": "has_icbm",
                   "equals": true
-                },
-                {
-                  "type": "all",
-                  "conditions": [
-                    {
-                      "type": "capability",
-                      "field": "toyota_virtual_cruise_speed_available",
-                      "equals": true
-                    },
-                    {
-                      "type": "param",
-                      "key": "ToyotaVirtualCruiseSpeed",
-                      "equals": true
-                    }
-                  ]
                 }
               ]
             }
@@ -817,21 +750,6 @@
                       "type": "capability",
                       "field": "has_icbm",
                       "equals": true
-                    },
-                    {
-                      "type": "all",
-                      "conditions": [
-                        {
-                          "type": "capability",
-                          "field": "toyota_virtual_cruise_speed_available",
-                          "equals": true
-                        },
-                        {
-                          "type": "param",
-                          "key": "ToyotaVirtualCruiseSpeed",
-                          "equals": true
-                        }
-                      ]
                     }
                   ]
                 }
@@ -2176,22 +2094,6 @@
                   "equals": true
                 }
               ]
-            },
-            {
-              "key": "PlanplusControl",
-              "widget": "option",
-              "title": "Plan Plus Controls",
-              "description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.",
-              "min": 0.0,
-              "max": 2.0,
-              "step": 0.1,
-              "enablement": [
-                {
-                  "type": "param",
-                  "key": "ShowAdvancedControls",
-                  "equals": true
-                }
-              ]
             }
           ]
         },
@@ -2400,50 +2302,6 @@
       "title": "Toyota / Lexus Settings",
       "description": "",
       "items": [
-        {
-          "key": "ToyotaAutoHold",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
-        {
-          "key": "ToyotaEnhancedBsm",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Toyota: Prius TSS2 BSM and some tssp",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
-        {
-          "key": "ToyotaTSS2Long",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Toyota: custom longitudinal for TSS2",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
-        {
-          "key": "ToyotaDriveMode",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Enable drive mode btn link",
-          "enablement": [
-            {
-              "type": "not_engaged"
-            }
-          ]
-        },
         {
           "key": "ToyotaEnforceStockLongitudinal",
           "widget": "toggle",
@@ -2453,40 +2311,6 @@
           "enablement": [
             {
               "type": "not_engaged"
-            },
-            {
-              "type": "param",
-              "key": "ToyotaVirtualCruiseSpeed",
-              "equals": false
-            }
-          ]
-        },
-        {
-          "key": "ToyotaVirtualCruiseSpeed",
-          "widget": "toggle",
-          "needs_onroad_cycle": true,
-          "title": "Toyota: Virtual Cruise Speed (Alpha)",
-          "description": "Uses a sunnypilot-owned cruise target with the Toyota RES/SET buttons and unlocks Custom ACC Speed Intervals. Set the short interval to 5 for next-5-unit tap behavior. The Toyota cluster continues to show the factory target and may differ from sunnypilot. The direct button signals are route-validated on Corolla Cross and Prius TSS2, but held-button timing differs by platform. Validate acceleration above the factory target in a controlled setting.",
-          "visibility": [
-            {
-              "type": "capability",
-              "field": "toyota_virtual_cruise_speed_available",
-              "equals": true
-            }
-          ],
-          "enablement": [
-            {
-              "type": "not_engaged"
-            },
-            {
-              "type": "capability",
-              "field": "has_longitudinal_control",
-              "equals": true
-            },
-            {
-              "type": "param",
-              "key": "ToyotaEnforceStockLongitudinal",
-              "equals": false
             }
           ]
         },

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -43,32 +43,6 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
-  - key: AccelPersonalityEnabled
-    widget: toggle
-    title: Enable Accel Controller
-    description: Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking
-      and stopping authority.
-    visibility:
-    - $ref: '#/macros/longitudinal'
-    enablement:
-    - $ref: '#/macros/longitudinal'
-  - key: AccelPersonality
-    widget: multiple_button
-    title: Acceleration Profile
-    description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts
-      and recovers more quickly.
-    options:
-    - value: 0
-      label: Eco
-    - value: 1
-      label: Normal
-    - value: 2
-      label: Sport
-    enablement:
-    - $ref: '#/macros/longitudinal'
-    - type: param
-      key: AccelPersonalityEnabled
-      equals: true
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)
@@ -103,14 +77,6 @@ sections:
     - type: capability
       field: has_icbm
       equals: true
-    - type: all
-      conditions:
-      - type: capability
-        field: toyota_virtual_cruise_speed_available
-        equals: true
-      - type: param
-        key: ToyotaVirtualCruiseSpeed
-        equals: true
   items:
   - key: CustomAccIncrementsEnabled
     widget: toggle
@@ -132,14 +98,6 @@ sections:
       - type: capability
         field: has_icbm
         equals: true
-      - type: all
-        conditions:
-        - type: capability
-          field: toyota_virtual_cruise_speed_available
-          equals: true
-        - type: param
-          key: ToyotaVirtualCruiseSpeed
-          equals: true
   sub_panels:
   - id: custom_acc_intervals
     label: Custom ACC Speed Intervals Settings

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml

@@ -51,16 +51,6 @@ sections:
         key: LagdToggle
         equals: true
     - $ref: '#/macros/advanced_only'
-  - key: PlanplusControl
-    widget: option
-    title: Plan Plus Controls
-    description: Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover
-      to lane center; too high and it will ping-pong.
-    min: 0.0
-    max: 2.0
-    step: 0.1
-    enablement:
-    - $ref: '#/macros/advanced_only'
 - id: lateral_control
   title: Lateral Control
   description: Neural network lateral control for supported models

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -82,30 +82,6 @@ sections:
   title: Toyota / Lexus Settings
   description: ''
   items:
-  - key: ToyotaAutoHold
-    widget: toggle
-    needs_onroad_cycle: true
-    title: 'Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS'
-    enablement:
-    - $ref: '#/macros/not_engaged'
-  - key: ToyotaEnhancedBsm
-    widget: toggle
-    needs_onroad_cycle: true
-    title: 'Toyota: Prius TSS2 BSM and some tssp'
-    enablement:
-    - $ref: '#/macros/not_engaged'
-  - key: ToyotaTSS2Long
-    widget: toggle
-    needs_onroad_cycle: true
-    title: 'Toyota: custom longitudinal for TSS2'
-    enablement:
-    - $ref: '#/macros/not_engaged'
-  - key: ToyotaDriveMode
-    widget: toggle
-    needs_onroad_cycle: true
-    title: Enable drive mode btn link
-    enablement:
-    - $ref: '#/macros/not_engaged'
   - key: ToyotaEnforceStockLongitudinal
     widget: toggle
     needs_onroad_cycle: true
@@ -113,28 +89,6 @@ sections:
     description: sunnypilot will not take over control of gas and brakes. Factory Toyota longitudinal control will be used.
     enablement:
     - $ref: '#/macros/not_engaged'
-    - type: param
-      key: ToyotaVirtualCruiseSpeed
-      equals: false
-  - key: ToyotaVirtualCruiseSpeed
-    widget: toggle
-    needs_onroad_cycle: true
-    title: 'Toyota: Virtual Cruise Speed (Alpha)'
-    description: Uses a sunnypilot-owned cruise target with the Toyota RES/SET buttons and unlocks Custom ACC Speed
-      Intervals. Set the short interval to 5 for next-5-unit tap behavior. The Toyota cluster continues to show the
-      factory target and may differ from sunnypilot. The direct button signals are route-validated on Corolla Cross
-      and Prius TSS2, but held-button timing differs by platform. Validate acceleration above the factory target in a
-      controlled setting.
-    visibility:
-    - type: capability
-      field: toyota_virtual_cruise_speed_available
-      equals: true
-    enablement:
-    - $ref: '#/macros/not_engaged'
-    - $ref: '#/macros/longitudinal'
-    - type: param
-      key: ToyotaEnforceStockLongitudinal
-      equals: false
   - key: ToyotaStopAndGoHack
     widget: toggle
     needs_onroad_cycle: true

openpilot/sunnypilot/sunnylink/tests/test_capabilities.py

@@ -14,10 +14,6 @@ from __future__ import annotations
 
 import pytest
 
-from openpilot.cereal import custom
-from opendbc.car.structs import car
-from opendbc.car.toyota.values import CAR as TOYOTA_CAR
-from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
 from openpilot.sunnypilot.sunnylink.capabilities import (
   CAPABILITY_DEFAULTS,
   CAPABILITY_FIELDS,
@@ -31,33 +27,6 @@ KNOWN_PROTOCOL_VERSIONS = (1,)
 LATEST_KNOWN = max(KNOWN_PROTOCOL_VERSIONS)
 
 
-class FakeParams:
-  def __init__(self, values=None):
-    self.values = values or {}
-
-  def get(self, key, *args, **kwargs):
-    return self.values.get(key)
-
-  def get_bool(self, key):
-    return bool(self.values.get(key, False))
-
-
-def build_persistent_toyota_params(platform, *, sp_flags=0):
-  CP = car.CarParams.new_message()
-  CP.brand = "toyota"
-  CP.carFingerprint = str(platform)
-  CP.pcmCruise = True
-  CP.openpilotLongitudinalControl = True
-
-  CP_SP = custom.CarParamsSP.new_message()
-  CP_SP.flags = int(sp_flags)
-
-  return FakeParams({
-    "CarParamsPersistent": CP.to_bytes(),
-    "CarParamsSPPersistent": CP_SP.to_bytes(),
-  })
-
-
 @pytest.fixture(scope="module")
 def caps():
   return generate_capabilities()
@@ -108,58 +77,6 @@ class TestOpaquePerBrandFlags:
     assert caps["hyundai_alpha_long_available"] is False
 
 
-class TestToyotaVirtualCruiseSpeedCapability:
-  def test_field_present_and_labeled(self):
-    assert "toyota_virtual_cruise_speed_available" in CAPABILITY_FIELDS
-    assert "toyota_virtual_cruise_speed_available" in CAPABILITY_LABELS
-
-  def test_default_false(self):
-    caps = generate_capabilities(FakeParams())
-    assert caps["toyota_virtual_cruise_speed_available"] is False
-
-  @pytest.mark.parametrize(("platform", "expected"), (
-    (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, True),
-    (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, True),
-    (TOYOTA_CAR.TOYOTA_RAV4_TSS2, False),
-  ))
-  def test_bundle_platform_gating(self, platform, expected):
-    params = FakeParams({
-      "CarPlatformBundle": {
-        "brand": "toyota",
-        "platform": str(platform),
-      },
-    })
-    caps = generate_capabilities(params)
-    assert caps["toyota_virtual_cruise_speed_available"] is expected
-
-  @pytest.mark.parametrize(("platform", "sp_flags", "expected"), (
-    (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE, True),
-    (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, 0, True),
-    (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE, True),
-    (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, 0, True),
-    (TOYOTA_CAR.TOYOTA_RAV4_TSS2, ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE, False),
-  ))
-  def test_persistent_car_params_platform_gating(self, platform, sp_flags, expected):
-    caps = generate_capabilities(build_persistent_toyota_params(platform, sp_flags=sp_flags))
-    assert caps["toyota_virtual_cruise_speed_available"] is expected
-
-  @pytest.mark.parametrize(("bundle_platform", "persistent_platform", "expected"), (
-    (TOYOTA_CAR.TOYOTA_COROLLA_TSS2, TOYOTA_CAR.TOYOTA_RAV4_TSS2, True),
-    (TOYOTA_CAR.TOYOTA_PRIUS_TSS2, TOYOTA_CAR.TOYOTA_RAV4_TSS2, True),
-    (TOYOTA_CAR.TOYOTA_RAV4_TSS2, TOYOTA_CAR.TOYOTA_COROLLA_TSS2, False),
-    (TOYOTA_CAR.TOYOTA_RAV4_TSS2, TOYOTA_CAR.TOYOTA_PRIUS_TSS2, False),
-  ))
-  def test_bundle_platform_takes_precedence_over_stale_persistent_params(self, bundle_platform, persistent_platform, expected):
-    params = build_persistent_toyota_params(persistent_platform, sp_flags=ToyotaFlagsSP.VIRTUAL_CRUISE_SPEED_AVAILABLE)
-    params.values["CarPlatformBundle"] = {
-      "brand": "toyota",
-      "platform": str(bundle_platform),
-    }
-
-    caps = generate_capabilities(params)
-    assert caps["toyota_virtual_cruise_speed_available"] is expected
-
-
 class TestCapabilitiesShape:
   def test_all_fields_present(self, caps):
     for field in CAPABILITY_FIELDS:

openpilot/sunnypilot/sunnylink/tests/test_settings_changes.py

@@ -105,34 +105,6 @@ def _references_capability_field(rules: list[dict[str, Any]] | None, field: str)
   return found
 
 
-def _has_toyota_virtual_cruise_gate(rules: list[dict[str, Any]] | None) -> bool:
-  def _walk(rule: dict[str, Any]) -> bool:
-    if rule.get("type") == "all":
-      conditions = rule.get("conditions", [])
-      has_capability = any(
-        c.get("type") == "capability" and
-        c.get("field") == "toyota_virtual_cruise_speed_available" and
-        c.get("equals") is True
-        for c in conditions
-      )
-      has_param = any(
-        c.get("type") == "param" and
-        c.get("key") == "ToyotaVirtualCruiseSpeed" and
-        c.get("equals") is True
-        for c in conditions
-      )
-      if has_capability and has_param:
-        return True
-
-    if rule.get("type") == "not" and "condition" in rule:
-      return _walk(rule["condition"])
-    if rule.get("type") in ("any", "all"):
-      return any(_walk(c) for c in rule.get("conditions", []))
-    return False
-
-  return any(_walk(rule) for rule in rules or [])
-
-
 @pytest.fixture(scope="module")
 def schema():
   return generate_schema()
@@ -245,25 +217,3 @@ class TestNotEngagedReplacement:
     rule_types = _flatten_rule_types(item.get("enablement"))
     assert "offroad_only" not in rule_types, f"{key} still uses offroad_only"
     assert "not_engaged" in rule_types, f"{key} missing not_engaged"
-
-
-class TestToyotaVirtualCruiseSpeed:
-  def test_vehicle_toggle_contract(self, schema):
-    toyota = schema["vehicle_settings"]["toyota"]
-    item = next((item for item in toyota["items"] if item.get("key") == "ToyotaVirtualCruiseSpeed"), None)
-
-    assert item is not None
-    assert item["widget"] == "toggle"
-    assert item.get("needs_onroad_cycle") is True
-    assert _references_capability_field(item.get("visibility"), "toyota_virtual_cruise_speed_available")
-    assert _references_capability_field(item.get("enablement"), "has_longitudinal_control")
-    assert "not_engaged" in _flatten_rule_types(item.get("enablement"))
-
-  def test_custom_acc_section_links_virtual_cruise_opt_in(self, schema):
-    section = _find_section(schema, "cruise", "custom_acc_increments")
-    assert section is not None
-    assert _has_toyota_virtual_cruise_gate(section.get("enablement"))
-
-    item = _find_item(schema, "CustomAccIncrementsEnabled")
-    assert item is not None
-    assert _has_toyota_virtual_cruise_gate(item.get("enablement"))

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -278,33 +278,16 @@ class TestKnownPanels:
     enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
     assert "NeuralNetworkLateralControl" in enhanced_enable_keys
 
-  def test_accel_controller_profile_mapping_and_enablement(self, schema):
-    cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
-    items = {item["key"]: item for item in _iter_panel_items(cruise)}
-
-    assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
-    assert items["AccelPersonality"]["options"] == [
-      {"value": 0, "label": "Eco"},
-      {"value": 1, "label": "Normal"},
-      {"value": 2, "label": "Sport"},
-    ]
-    assert {
-      "type": "param",
-      "key": "AccelPersonalityEnabled",
-      "equals": True,
-    } in items["AccelPersonality"]["enablement"]
-
 
 class TestKnownVehicleSettings:
   def test_hyundai_has_longitudinal_tuning(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
     assert "HyundaiLongitudinalTuning" in keys
 
-  def test_toyota_has_enforce_stock_stop_go_and_virtual_cruise(self, schema):
+  def test_toyota_has_enforce_stock_and_stop_go(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("toyota"))}
     assert "ToyotaEnforceStockLongitudinal" in keys
     assert "ToyotaStopAndGoHack" in keys
-    assert "ToyotaVirtualCruiseSpeed" in keys
 
   def test_tesla_has_coop_steering(self, schema):
     keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("tesla"))}

openpilot/system/ui/lib/scroll_panel2.py

@@ -45,9 +45,8 @@ class ScrollState(Enum):
 
 
 class GuiScrollPanel2:
-  def __init__(self, horizontal: bool = True, handle_out_of_bounds: bool = True) -> None:
+  def __init__(self, horizontal: bool = True) -> None:
     self._horizontal = horizontal
-    self._handle_out_of_bounds = handle_out_of_bounds
     self._state = ScrollState.STEADY
     self._offset: rl.Vector2 = rl.Vector2(0, 0)
     self._initial_click_event: MouseEvent | None = None
@@ -86,20 +85,6 @@ class GuiScrollPanel2:
     """Returns (max_offset, min_offset) for the given bounds and content size."""
     return 0.0, min(0.0, bounds_size - content_size)
 
-  def _clamp_offset(self, bounds_size: float, content_size: float) -> None:
-    if self._handle_out_of_bounds:
-      return
-
-    max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)
-    offset = self.get_offset()
-    clamped_offset = max(min_offset, min(max_offset, offset))
-    if clamped_offset == offset:
-      return
-
-    self.set_offset(clamped_offset)
-    if (clamped_offset == max_offset and self._velocity > 0) or (clamped_offset == min_offset and self._velocity < 0):
-      self._velocity = 0.0
-
   def _update_state(self, bounds_size: float, content_size: float, snap_target: float | None) -> None:
     """Runs per render frame, independent of mouse events. Updates auto-scrolling state and velocity."""
     max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)
@@ -153,8 +138,6 @@ class GuiScrollPanel2:
         factor = 1.0 - math.exp(-SNAP_RATE * dt)
         self.set_offset(self.get_offset() + dist * factor)
 
-    self._clamp_offset(bounds_size, content_size)
-
   def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
                           content_size: float) -> None:
     max_offset, min_offset = self._get_offset_bounds(bounds_size, content_size)

openpilot/system/ui/widgets/scroller.py

@@ -75,6 +75,7 @@ class _Scroller(Widget):
     self._items: list[Widget] = []
     self._horizontal = horizontal
     self._snap_items = snap_items
+    assert not self._snap_items or self._horizontal, "Snapping is only supported for horizontal scrolling"
     self._spacing = spacing
     self._pad = pad
 
@@ -190,20 +191,12 @@ class _Scroller(Widget):
     snap_target: float | None = None
     if self._snap_items and visible_items and self._scrolling_to[0] is None:
       # TODO: this doesn't handle two small buttons at the edges well
-      center_pos = (self._rect.x + self._rect.width / 2) if self._horizontal else (self._rect.y + self._rect.height / 2)
-      closest_delta_pos = min(
-        (self._item_center_pos(item) - center_pos for item in visible_items),
-        key=abs,
-      )
+      center_pos = self._rect.x + self._rect.width / 2
+      closest_delta_pos = min((((item.rect.x + item.rect.width / 2) - center_pos) for item in visible_items), key=abs)
       snap_target = self.scroll_panel.get_offset() - closest_delta_pos
 
     return self.scroll_panel.update(self._rect, content_size, snap_target=snap_target)
 
-  def _item_center_pos(self, item: Widget) -> float:
-    if self._horizontal:
-      return item.rect.x + item.rect.width / 2
-    return item.rect.y + item.rect.height / 2
-
   @property
   def moving_items(self) -> bool:
     return len(self._move_animations) > 0 or len(self._move_lift) > 0