Update compile_modeld.py

.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/commaai/msgq.git

cereal/custom.capnp

@@ -194,7 +194,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
-  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -297,47 +296,6 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
-
-  struct AccelController {
-    enabled @0 :Bool;
-    active @1 :Bool;
-    shadowOnly @2 :Bool;
-    profile @3 :Profile;
-    state @4 :State;
-    vTargetBase @5 :Float32;
-    vTargetRaw @6 :Float32;
-    vTargetFiltered @7 :Float32;
-    vTargetShadow @8 :Float32;
-    leadIndex @9 :Int8 = -1;
-    usableGap @10 :Float32;
-    closingSpeed @11 :Float32;
-    requiredDecel @12 :Float32;
-    aMaxProfile @13 :Float32;
-    aMaxEffective @14 :Float32;
-
-    enum Profile {
-      eco @0;
-      normal @1;
-      sport @2;
-    }
-
-    enum State {
-      inactive @0;
-      free @1;
-      restrict @2;
-      hold @3;
-      release @4;
-      stopHold @5;
-    }
-  }
-
-  # Compatibility type for vehicle integrations that map physical drive modes
-  # onto AccelPersonality. New controller telemetry uses AccelController.Profile.
-  enum AccelerationPersonality {
-    eco @0;
-    normal @1;
-    sport @2;
-  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {
@@ -384,7 +342,6 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
-    laneChangeRoadEdge @24;
   }
 }
 
@@ -491,8 +448,6 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
 
 struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
-  leftLaneChangeEdgeBlock @1 :Bool;
-  rightLaneChangeEdgeBlock @2 :Bool;
 
   enum TurnDirection {
     none @0;

common/params_keys.h

@@ -179,19 +179,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"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"}},
@@ -235,10 +228,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"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"}},

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("LiveParameters")
 
     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("LiveParameters") is None
     assert self.params.get("LiveParameters", return_default=True) is None
 

selfdrive/car/card.py

@@ -10,7 +10,7 @@ from cereal import car, log, custom
 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
@@ -121,13 +121,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)

selfdrive/controls/lib/desire_helper.py

@@ -56,7 +56,7 @@ class DesireHelper:
   def get_lane_change_direction(CS):
     return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
 
-  def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
+  def update(self, carstate, lateral_active, lane_change_prob):
     self.alc.update_params()
     self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
@@ -88,8 +88,8 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
-                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
+                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
         self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -217,7 +217,6 @@ class LongitudinalMpc:
   def __init__(self, dt=DT_MDL):
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
-    self.last_solution_status = 0
     self.reset()
     self.source = LongitudinalPlanSource.cruise
 
@@ -268,11 +267,10 @@ class LongitudinalMpc:
     for i in range(N):
       self.solver.cost_set(i, 'Zl', Zl)
 
-  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, *, jerk_cost_multiplier=1.0):
+  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,
-                    jerk_factor * J_EGO_COST * jerk_cost_multiplier]
+    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)
 
@@ -315,7 +313,7 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard, accel_max=None):
+  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -347,14 +345,6 @@ class LongitudinalMpc:
 
     self.params[:,0] = ACCEL_MIN
     self.params[:,1] = ACCEL_MAX
-    if accel_max is not None:
-      try:
-        accel_max_trajectory = np.asarray(accel_max, dtype=float)
-      except (OverflowError, TypeError, ValueError):
-        accel_max_trajectory = np.empty(0)
-      if accel_max_trajectory.shape == (N + 1,) and np.all(np.isfinite(accel_max_trajectory)):
-        self.params[:,1] = np.clip(accel_max_trajectory, 0.0, ACCEL_MAX)
-        self.params[0,1] = max(self.params[0,1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX)))
     self.params[:,2] = np.min(x_obstacles, axis=1)
     self.params[:,3] = np.copy(self.a_prev)
     self.params[:,4] = t_follow
@@ -374,7 +364,6 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "ubx", self.x0)
 
     self.solution_status = self.solver.solve()
-    self.last_solution_status = self.solution_status
     self.solve_time = float(self.solver.get_stats('time_tot')[0])
     self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
     self.time_linearization = float(self.solver.get_stats('time_lin')[0])

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
@@ -129,18 +129,16 @@ 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)
-    base_v_cruise = v_cruise
 
     if force_slow_decel:
       v_cruise = 0.0
 
-    is_e2e = LongitudinalPlannerSP.update_accel_controller_mpc(
-      self, sm, base_v_cruise, v_cruise, prev_accel_constraint, reset_state=reset_state,
-      cruise_initialized=v_cruise_initialized, available_accel_max=accel_clip[1] if self.allow_throttle else 0.0,
-      previous_should_stop=self.output_should_stop, force_decel=force_slow_decel,
-    )
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -156,14 +154,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
-    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, vEgoStopping=self.CP.vEgoStopping,
-    )
+    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, vEgoStopping=self.CP.vEgoStopping)
     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:
@@ -171,7 +168,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
-    self.output_should_stop = self.accel_controller_should_stop(self.output_should_stop, is_e2e)
 
     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)

selfdrive/selfdrived/selfdrived.py

@@ -321,16 +321,9 @@ class SelfdriveD(CruiseHelper):
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
-      mdv2sp = self.sm['modelDataV2SP']
-
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
-            (CS.rightBlindspot and direction == LaneChangeDirection.right):
+         (CS.rightBlindspot and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
-
-      elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
-           (mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
-        self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
-
       else:
         if direction == LaneChangeDirection.left:
           self.events.add(EventName.preLaneChangeLeft)

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -1,11 +1,5 @@
 #!/usr/bin/env python3
-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 cereal import log
@@ -17,105 +11,12 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
-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")
-
-
-# Route-derived conservative Prius TSS2 stress model for the acceleration-controller
-# regression suite. The 1.0 m/s² gate represents prompt takeoffs, not a universal
-# physical threshold: the supplied routes also contain low-command creep departures.
-# This models vehicle response only and does not emulate Toyota's CAN controller.
-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 Plant:
   messaging_initialized = False
 
-  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,
-  ):
-    """Closed-loop longitudinal planner plant.
-
-    ``lead_observation_fn(time, lead_name, truth)`` may return a complete or partial
-    observed LeadData mapping, or ``None`` for an absent lead. It is called separately
-    for ``leadOne`` and ``leadTwo``. The supplied truth mapping is a copy, and observed
-    values never affect the physical lead trajectory.
-
-    ``model_action_fn(time, v_ego, a_ego)`` returns
-    ``(desired_acceleration, should_stop)``.
-
-    ``ego_observation_fn(time, true_v_ego, true_a_ego)`` returns the observed
-    ``(v_ego, a_ego)`` published in ``carState``. It can inject measurement noise
-    without changing the physical plant state.
-
-    Passing ``actuator_delay`` both overrides ``CP.longitudinalActuatorDelay`` and
-    adds the corresponding command transport delay to the plant. ``None`` keeps the
-    historical Honda planner delay with instantaneous plant response. ``actuator_lag``
-    is an optional first-order acceleration-response time constant. Both defaults keep
-    historical plant dynamics unchanged.
-
-    ``actuator_model`` opts into a staged vehicle-response model. Its planner delay
-    is used by MPC, while its independent transport delay is used by the command
-    queue before rate limiting, standstill breakaway confirmation, and first-order
-    lag. Leaving it unset preserves the historical actuator path.
-    """
-    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
+  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):
+    self.rate = 1. / DT_MDL
 
     if not Plant.messaging_initialized:
       Plant.radar = messaging.pub_sock('radarState')
@@ -127,15 +28,10 @@ class Plant:
 
     self.v_lead_prev = 0.0
 
-    self.distance = 0.0
+    self.distance = 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
@@ -146,18 +42,9 @@ class Plant:
     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))
 
     self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
-    self.ts = 1.0 / self.rate
+    self.ts = 1. / self.rate
     time.sleep(0.1)
     self.sm = messaging.SubMaster(['longitudinalPlan'])
 
@@ -165,86 +52,14 @@ class Plant:
     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)
 
-    if self.actuator_model is not None and self.speed >= 0.01:
-      self.breakaway_confirmed = True
-    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.ts)
-    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
-
   @property
   def current_time(self):
     return float(self.rk.frame) / self.rate
 
-  @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
-
-    # Partial overrides are convenient for individual sensor glitches, while copying
-    # from truth ensures every field written to cereal is deterministic.
-    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.ts
-      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.ts
-          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:
-      # Preserve the historical response path exactly when no staged model is used.
-      self.applied_actuator_command = delayed_command
-      response_command = delayed_command
-
-    if self.actuator_lag > 0.0:
-      alpha = 1.0 - math.exp(-self.ts / self.actuator_lag)
-      self.acceleration += alpha * (response_command - self.acceleration)
-    else:
-      self.acceleration = response_command
-    return delayed_command, self.acceleration
-
-  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50., 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')
@@ -257,48 +72,39 @@ class Plant:
     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
+    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)
+      d_rel = np.maximum(0., self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
       if self.only_radar:
         status = True
-      elif prob_lead > 0.5:
+      elif prob_lead > .5:
         status = True
       else:
         status = False
     else:
-      d_rel = 200.0
-      v_rel = 0.0
+      d_rel = 200.
+      v_rel = 0.
       prob_lead = 0.0
       status = False
 
-    truth_lead: LeadObservation = {
-      "dRel": float(d_rel),
-      "yRel": 0.0,
-      "vRel": float(v_rel),
-      "aRel": float(a_lead - self.acceleration),
-      "vLead": float(v_lead),
-      "dPath": 0.0,
-      "vLat": 0.0,
-      "vLeadK": float(v_lead),
-      "aLeadK": float(a_lead),
-      "fcw": False,
-      "status": 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)
+    lead = log.RadarState.LeadData.new_message()
+    lead.dRel = float(d_rel)
+    lead.yRel = 0.0
+    lead.vRel = float(v_rel)
+    lead.aRel = float(a_lead - self.acceleration)
+    lead.vLead = float(v_lead)
+    lead.vLeadK = float(v_lead)
+    lead.aLeadK = float(a_lead)
+    # TODO use real radard logic for this
+    lead.aLeadTau = float(_LEAD_ACCEL_TAU)
+    lead.status = status
+    lead.modelProb = float(prob_lead)
+    if not self.only_lead2:
+      radar.radarState.leadOne = lead
+    radar.radarState.leadTwo = lead
 
     # Simulate model predicting slightly faster speed
     # this is to ensure lead policy is effective when model
@@ -306,15 +112,10 @@ class Plant:
     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)
+    model.modelV2.action.desiredAcceleration = float(self.acceleration + 0.1)
     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
+    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)]
@@ -325,45 +126,33 @@ class Plant:
     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.vEgo = float(self.speed)
     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]
+    car_control.carControl.orientationNED = [0., float(pitch), 0.]
 
     # ******** get controlsState messages for plotting ***
-    sm = {
-      '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,
-    }
+    sm = {'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
-    self.actuator_command = self.a_target
+    self.acceleration = self.planner.output_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)
-    delayed_actuator_command, _ = self._update_actuator(self.actuator_command)
+      self.acceleration = min(-0.5, self.acceleration)
     self.speed = self.speed + self.acceleration * self.ts
     self.should_stop = self.planner.output_should_stop
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts
 
     # ******** run the car ********
-    # print(self.distance, speed)
+    #print(self.distance, speed)
     if self.speed <= 0:
       self.speed = 0
       self.acceleration = 0
@@ -371,64 +160,30 @@ class Plant:
 
     # *** radar model ***
     if self.lead_relevancy:
-      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      d_rel = np.maximum(0., self.distance_lead - self.distance)
       v_rel = v_lead - self.speed
     else:
-      d_rel = 200.0
-      v_rel = 0.0
+      d_rel = 200.
+      v_rel = 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_result = getattr(self.planner, "accel_controller_result", None)
     return {
       "distance": self.distance,
       "speed": self.speed,
       "acceleration": self.acceleration,
-      "realized_acceleration": self.acceleration,
-      "a_target": self.a_target,
-      "planner_acceleration": self.a_target,
-      "actuator_command": self.actuator_command,
-      "stop_clamped_actuator_command": self.actuator_command,
-      "delayed_actuator_command": delayed_actuator_command,
-      "applied_actuator_command": self.applied_actuator_command,
-      "vehicle_actuator_command": self.applied_actuator_command,
-      "true_v_ego": true_v_ego,
-      "true_a_ego": true_a_ego,
-      "published_a_ego": published_a_ego,
-      "published_v_ego": published_v_ego,
-      "observed_a_ego": published_a_ego,
-      "observed_v_ego": published_v_ego,
-      "planner_delay": self.actuator_delay,
-      "transport_delay": self.transport_delay,
-      "breakaway_confirmed": self.breakaway_confirmed,
-      "breakaway_time": self._breakaway_timer,
       "should_stop": self.should_stop,
       "distance_lead": self.distance_lead,
       "fcw": fcw,
-      "mpc_source": self.planner.mpc.source,
-      "dec_mode": self.planner.dec.mode(),
-      "pace_cap": getattr(accel_controller_result, "target_speed", None),
-      "base_target": getattr(accel_controller_result, "base_speed", None),
-      "raw_energy_cap": getattr(accel_controller_result, "raw_energy_cap", None),
-      "live_filtered_cap": getattr(accel_controller_result, "live_filtered_cap", None),
-      "shadow_filtered_cap": getattr(accel_controller_result, "shadow_filtered_cap", None),
-      "accel_controller_selected_lead": getattr(accel_controller_result, "selected_lead", None),
-      "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),
     }
 
-
 # simple engage in standalone mode
 def plant_thread():
   plant = Plant()

selfdrive/test/longitudinal_maneuvers/test_plant.py

@@ -1,80 +0,0 @@
-import math
-
-import pytest
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
-
-
-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,
-        "status": True,
-        "modelProb": 0.9,
-        "radarTrackId": 42,
-      }
-    return None
-
-  plant = Plant(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 = Plant(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 "pace_cap" in first
-  assert "raw_energy_cap" in first
-  assert "live_filtered_cap" 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 = Plant(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):
-    Plant(actuator_delay=delay, actuator_lag=lag)

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)

selfdrive/ui/mici/layouts/main.py

@@ -13,7 +13,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.onroad import OnroadViewContainerSP as AugmentedRoadView
 
 ONROAD_DELAY = 2.5  # seconds
 
@@ -119,15 +118,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):

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

selfdrive/ui/mici/widgets/button.py

@@ -382,18 +382,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):

selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py

@@ -51,17 +51,11 @@ class LaneChangeSettingsLayout(Widget):
       description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
                              "(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
     )
-    self._road_edge_block = toggle_item_sp(
-      param="RoadEdgeLaneChangeEnabled",
-      title=lambda: tr("Block Lane Change: Road Edge Detection"),
-      description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
-    )
 
     items = [
       self._lane_change_timer,
       LineSeparatorSP(40),
       self._bsm_delay,
-      self._road_edge_block,
     ]
 
     return items

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

@@ -1,13 +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
-
-
-class MiciMainLayoutSP(MiciMainLayout):
-  def _should_auto_scroll_to_onroad(self) -> bool:
-    return not self._onroad_layout.is_on_info_panel()

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

@@ -1,63 +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 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, callback) -> None:
-    self.road_view.set_click_callback(callback)
-    self.onroad_info_panel.set_click_callback(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)

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

@@ -1,324 +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
-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
-from openpilot.system.ui.lib.multilang import tr
-from openpilot.system.ui.lib.text_measure import measure_text_cached
-from openpilot.system.ui.lib.application import MousePos
-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
-
-
-@dataclass(frozen=True)
-class OnroadInfoPanelColors:
-  white: rl.Color = rl.WHITE
-  black: rl.Color = rl.BLACK
-  red: rl.Color = rl.Color(255, 0, 0, 255)
-  green: rl.Color = rl.Color(0, 255, 0, 255)
-  grey: rl.Color = rl.Color(190, 195, 190, 255)
-  light_grey: rl.Color = rl.Color(200, 200, 200, 255)
-  dark_grey: rl.Color = rl.Color(100, 100, 100, 255)
-  bg_dark: rl.Color = rl.Color(0, 0, 0, 255)
-  card_bg: rl.Color = rl.Color(50, 50, 50, 200)
-  badge_bg: rl.Color = 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)
-    margin = 20
-    mid_y = rect.y + rect.height / 2
-
-    left_x = rect.x + 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
-
-    speed_val = str(round(display_speed))
-    if self.speed_limit_valid and display_speed > self.speed_limit:
-      speed_color = COLORS.red
-    else:
-      speed_color = COLORS.white
-
-    rl.draw_text_ex(self._font_semi_bold, unit, rl.Vector2(left_x, mid_y - 95), 38, 0, COLORS.grey)
-    rl.draw_text_ex(self._font_bold, speed_val, rl.Vector2(left_x, mid_y - 60), 110, 0, speed_color)
-
-    sign_width = 135
-    sign_height = 135 if ui_state.is_metric else 175
-
-    has_next = self.next_speed_limit > 0 and self.next_speed_limit != self.speed_limit
-    target_slide = 1.0 if has_next else 0.0
-    slide_speed = 3.0 * rl.get_frame_time()
-    if self._sign_slide < target_slide:
-      self._sign_slide = min(self._sign_slide + slide_speed, target_slide)
-    elif self._sign_slide > target_slide:
-      self._sign_slide = max(self._sign_slide - slide_speed, target_slide)
-
-    next_w = int(sign_width * 0.7)
-    next_h = int(sign_height * 0.7)
-    next_peek = int(next_w * 0.85) + 5
-    centered_x = rect.x + rect.width - sign_width - margin
-    shifted_x = rect.x + rect.width - sign_width - margin - next_peek
-    sign_x = centered_x + (shifted_x - centered_x) * self._sign_slide
-    sign_y = rect.y + (rect.height - sign_height) / 2
-
-    road_y = mid_y + 55
-    road_width = sign_x - left_x - margin
-    self._draw_road_name(left_x, road_y, road_width)
-
-    if has_next and self._sign_slide > 0.01:
-      next_val = str(round(self.next_speed_limit))
-      dist_str = self._format_distance(self.next_speed_limit_distance)
-      next_x = sign_x + sign_width - int(next_w * 0.15)
-      next_y = sign_y + (sign_height - next_h) / 2
-
-      next_speed_color = COLORS.black
-      if ui_state.is_metric:
-        self._draw_vienna_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
-      else:
-        self._draw_mutcd_sign(next_x, next_y, next_w, next_h, next_val, next_speed_color, is_upcoming=True)
-
-      dist_size = measure_text_cached(self._font_medium, dist_str, 24)
-      rl.draw_text_ex(self._font_medium, dist_str, rl.Vector2(next_x + next_w / 2 - dist_size.x / 2, next_y + next_h + 4), 24, 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_val = str(abs(round(self.speed_limit_offset)))
-      badge_sz = 42
-      badge_x = sign_x + sign_width - badge_sz * 0.85
-      badge_y = sign_y - badge_sz * 0.25
-
-      if ui_state.is_metric:
-        badge_r = badge_sz / 2
-        badge_cx = badge_x + badge_r
-        badge_cy = badge_y + badge_r
-        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r + 2, COLORS.dark_grey)
-        rl.draw_circle(int(badge_cx), int(badge_cy), badge_r, COLORS.badge_bg)
-        self._draw_text_centered(self._font_bold, offset_val, 24, rl.Vector2(badge_cx, badge_cy), COLORS.white)
-      else:
-        mutcd_badge_x = sign_x + sign_width - badge_sz * 0.65
-        mutcd_badge_y = sign_y - badge_sz * 0.50
-        badge_rect = rl.Rectangle(mutcd_badge_x, mutcd_badge_y, badge_sz, badge_sz)
-        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(self._font_bold, offset_val, 24, rl.Vector2(mutcd_badge_x + badge_sz / 2, mutcd_badge_y + badge_sz / 2), COLORS.white)
-
-    # SCC
-    speed_size = measure_text_cached(self._font_bold, speed_val, 110)
-    scc_x = left_x + speed_size.x + 30
-    scc_y = mid_y - 50
-    self._draw_scc_icons(scc_x, scc_y)
-
-    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) -> None:
-    sm = ui_state.sm
-    if not sm.valid["longitudinalPlanSP"]:
-      return
-    scc = sm["longitudinalPlanSP"].smartCruiseControl
-
-    box_w, box_h = 100, 36
-    gap = 6
-    drawn = 0
-
-    for label, active in [("SCC-V", scc.vision.active), ("SCC-M", scc.map.active)]:
-      if not active:
-        continue
-      bx = x
-      by = y + drawn * (box_h + gap)
-      rl.draw_rectangle_rounded(rl.Rectangle(bx, by, box_w, box_h), 0.3, 10, COLORS.green)
-      self._draw_text_centered(self._font_bold, label, 20, rl.Vector2(bx + box_w / 2, by + box_h / 2), COLORS.black)
-      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:
-    road_display = self.road_name if self.road_name else "--"
-    font_size = 30
-    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)
-    text_size = measure_text_cached(self._font_bold, speed_str, int(font_size))
-    text_pos = rl.Vector2(center.x - text_size.x / 2, center.y - text_size.y / 2)
-    rl.draw_text_ex(self._font_bold, speed_str, text_pos, font_size, 0, speed_color)
-
-  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(self._font_bold, tr("AHEAD"), label_size, rl.Vector2(mid_x, y + height * 0.27), COLORS.black)
-    else:
-      self._draw_text_centered(self._font_bold, tr("SPEED"), label_size, rl.Vector2(mid_x, y + height * 0.20), COLORS.black)
-      self._draw_text_centered(self._font_bold, tr("LIMIT"), label_size, rl.Vector2(mid_x, y + height * 0.40), COLORS.black)
-
-    speed_font_size = int(width * 0.52) if len(speed_str) >= 3 else int(width * 0.62)
-    self._draw_text_centered(self._font_bold, speed_str, speed_font_size, rl.Vector2(mid_x, y + height * 0.72), speed_color)
-
-  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 _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")

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

@@ -1,30 +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.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, rect: rl.Rectangle) -> None:
-    super()._render(rect)
-    if self._show_confidence_ball:
-      self._real_confidence_ball.render(self.rect)

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

@@ -1,34 +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):
-  """Reject orthogonal-dominant drags so nested scrollers (outer horizontal +
-  inner vertical) don't both engage on a slightly diagonal swipe.
-
-  Implemented as a post-super state rollback rather than reimplementing the
-  PRESSED state machine — keeps stock behaviour authoritative."""
-
-  def _handle_mouse_event(self, mouse_event: MouseEvent, bounds: rl.Rectangle, bounds_size: float,
-                          content_size: float) -> None:
-    pre_state = self._state
-    super()._handle_mouse_event(mouse_event, bounds, bounds_size, content_size)
-
-    if self._state == ScrollState.MANUAL_SCROLL and pre_state == ScrollState.PRESSED and \
-       self._initial_click_event is not None:
-      diff_x = abs(mouse_event.pos.x - self._initial_click_event.pos.x)
-      diff_y = abs(mouse_event.pos.y - self._initial_click_event.pos.y)
-      along = diff_x if self._horizontal else diff_y
-      anti = diff_y if self._horizontal else diff_x
-      if anti > along:
-        self._state = ScrollState.STEADY
-        self._velocity = 0.0
-        self._velocity_buffer.clear()

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)

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

sunnypilot/modeld_v2/compile_modeld.py

@@ -10,471 +10,291 @@ import argparse
 import os
 import pickle
 import time
-from functools import partial
 from collections import defaultdict
-
+from functools import partial
 import numpy as np
-from tinygrad.tensor import Tensor
+os.environ['GMMU'] = '0'
+
+def _patch_tinygrad_fetch_fw():
+  import hashlib
+  import pathlib
+  import zstandard
+  from tinygrad import helpers
+  _orig_fetch_fw = helpers.fetch_fw
+  def fetch_fw(path, name, sha256):
+    p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
+    if p.is_file():
+      blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
+      if hashlib.sha256(blob).hexdigest() == sha256:
+        return blob
+    return _orig_fetch_fw(path, name, sha256)
+  helpers.fetch_fw = fetch_fw
+_patch_tinygrad_fetch_fw()
+
+from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare, sample_desire, sample_skip, shift_and_sample
+from tinygrad import dtypes
 from tinygrad.device import Device
 from tinygrad.engine.jit import TinyJit
-
-from openpilot.selfdrive.modeld.compile_modeld import (
-  NV12Frame, make_frame_prepare,
-  shift_and_sample, sample_skip, sample_desire,
-)
+from tinygrad.tensor import Tensor
 
 MODEL_TYPES = ('vision_policy', 'supercombo', 'vision_multi_policy')
 
 
-def _detect_desire_key(policy_input_shapes):
-  for k in policy_input_shapes:
-    if k.startswith('desire'):
-      return k
-  return None
+def _detect_desire_key(shapes: dict) -> str | None:
+  return next((key for key in shapes if key.startswith('desire')), None)
 
 
-def _detect_vision_keys(vision_input_shapes):
-  img_keys = sorted([k for k in vision_input_shapes if 'img' in k])
-  road_key = next((k for k in img_keys if 'big' not in k), None)
-  wide_key = next((k for k in img_keys if 'big' in k), None)
-  if road_key is None or wide_key is None:
-    raise ValueError(f"Cannot determine road/wide image keys from {list(vision_input_shapes.keys())}")
-  return road_key, wide_key
+def _detect_vision_keys(shapes: dict) -> tuple[str | None, str | None]:
+  img_keys = sorted(key for key in shapes if 'img' in key)
+  return (
+    next((key for key in img_keys if 'big' not in key), None),
+    next((key for key in img_keys if 'big' in key), None)
+  )
 
 
-def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
-  road_key, _ = _detect_vision_keys(vision_input_shapes)
-  img = vision_input_shapes[road_key]
-  n_frames = img[1] // 6
-  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
-
-  fb = policy_input_shapes['features_buffer']
-  desire_key = _detect_desire_key(policy_input_shapes)
-  dp = policy_input_shapes[desire_key]
-  tc = policy_input_shapes.get('traffic_convention', (1, 2))
-
-  npy = {
-    'desire': np.zeros(dp[2], dtype=np.float32),
-    'traffic_convention': np.zeros(tc, dtype=np.float32),
-    'tfm': np.zeros((3, 3), dtype=np.float32),
-    'big_tfm': np.zeros((3, 3), dtype=np.float32),
-  }
-
-  handled = {'features_buffer', desire_key, 'traffic_convention'}
-  for key, shape in policy_input_shapes.items():
-    if key in handled:
-      continue
-    npy[key] = np.zeros(shape, dtype=np.float32)
-
-  input_queues = {
-    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    'feat_q': Tensor(np.zeros((frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
-    'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
-    **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
-  }
-  return input_queues, npy
+def derive_frame_skip(vision_input_shapes: dict, policy_input_shapes: dict) -> int:
+  features_buffer = policy_input_shapes.get('features_buffer')
+  return 1 if not features_buffer or features_buffer[1] >= 99 else 4
 
 
-def make_run_split_policy(vision_runner, policy_runner, nv12: NV12Frame, model_w, model_h,
-                          vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                          vision_road_key, vision_wide_key, prepare_only=False):
-  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
-  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
-  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
-
-  def run_policy(img_q, big_img_q, feat_q, desire_q, desire, traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
-    npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
-                   desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
-    extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
-    Tensor.realize(*npy_tensors, *extra_device.values())
-    tfm, big_tfm, desire, traffic_convention = npy_tensors
-
-    img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
-    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
-
-    if prepare_only:
-      return img, big_img
-
-    vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
-
-    new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
-    feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
-    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-
-    inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
-    policy_out = next(iter(policy_runner(inputs).values())).cast('float32')
-
-    return vision_out, policy_out
-  return run_policy
-
-
-def compile_split_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
-                         vision_runner, policy_runner, vision_metadata, policy_metadata):
-  print(f"Compiling combined policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
-
-  vision_features_slice = vision_metadata['output_slices']['hidden_state']
-  vision_input_shapes = vision_metadata['input_shapes']
-  policy_input_shapes = policy_metadata['input_shapes']
-  desire_key = _detect_desire_key(policy_input_shapes)
-  extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
-  vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
-
-  _run = make_run_split_policy(vision_runner, policy_runner, nv12, model_w, model_h,
-                               vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                               vision_road_key, vision_wide_key, prepare_only)
-  run_policy_jit = TinyJit(_run, prune=True)
-
-  SEED = 42
-
-  def random_inputs_run_fn(fn, seed, test_val=None, test_buffers=None, expect_match=True):
-    input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
-    np.random.seed(seed)
-    Tensor.manual_seed(seed)
-
-    testing = test_val is not None or test_buffers is not None
-    n_runs = 1 if testing else 3
-
-    for i in range(n_runs):
-      frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-      big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-      for v in npy.values():
-        v[:] = np.random.randn(*v.shape).astype(v.dtype)
-      Device.default.synchronize()
-      st = time.perf_counter()
-      outs = fn(**input_queues, frame=frame, big_frame=big_frame)
-      mt = time.perf_counter()
-      Device.default.synchronize()
-      et = time.perf_counter()
-      print(f"  [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
-
-      if i == 0:
-        val = [np.copy(v.numpy()) for v in outs]
-        buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
-
-    if test_val is not None:
-      match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True))
-      assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
-    if test_buffers is not None:
-      match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True))
-      assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
-    return fn, val, buffers
-
-  print('capture + replay')
-  run_policy_jit, test_val, test_buffers = random_inputs_run_fn(run_policy_jit, SEED)
-
-  print('pickle round trip')
-  run_policy_jit = pickle.loads(pickle.dumps(run_policy_jit))
-  random_inputs_run_fn(run_policy_jit, SEED, test_val, test_buffers, expect_match=True)
-  random_inputs_run_fn(run_policy_jit, SEED+1, test_val, test_buffers, expect_match=False)
-  return run_policy_jit
-
-
-def derive_frame_skip(vision_input_shapes, policy_input_shapes):
-  fb = policy_input_shapes.get('features_buffer')
-  if fb is None:
-    return 1
-  fb_history = fb[1]
-  if fb_history >= 99:
-    return 1
-  return 4
-
-
-def make_supercombo_input_queues(input_shapes, frame_skip, device):
-  img_shape = input_shapes.get('img', input_shapes.get('input_imgs'))
-  if img_shape is None:
-    raise ValueError("No img input found in model shapes")
+def generate_queues_and_npy(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
+  road_key, _ = _detect_vision_keys(input_shapes)
+  if not road_key:
+    raise ValueError("Vision road key missing from input shapes.")
 
+  img_shape = input_shapes[road_key]
   n_frames = img_shape[1] // 6
   img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
 
-  npy_keys = {}
-  queue_keys = {}
+  desire_key = _detect_desire_key(input_shapes)
+  if not desire_key:
+    raise ValueError("Desire key missing from input shapes.")
+
+  desire_shape = input_shapes[desire_key]
+  features_buffer = input_shapes.get('features_buffer')
+
+  npy_arrays = {
+    'desire': np.zeros(desire_shape[2], dtype=np.float32),
+    'tfm': np.zeros((3, 3), dtype=np.float32),
+    'big_tfm': np.zeros((3, 3), dtype=np.float32)
+  }
 
   for key, shape in input_shapes.items():
-    if 'img' in key:
-      continue
-    if len(shape) == 3 and shape[1] > 1:
-      if key.startswith('desire'):
-        npy_keys[key] = np.zeros(shape[2], dtype=np.float32)
-        queue_keys[f'{key}_q'] = Tensor(
-          np.zeros((frame_skip * shape[1], shape[0], shape[2]), dtype=np.float32),
-          device=device).contiguous().realize()
-      elif key == 'features_buffer':
-        queue_keys['feat_q'] = Tensor(
-          np.zeros((frame_skip * (shape[1] - 1) + 1, shape[0], shape[2]), dtype=np.float32),
-          device=device).contiguous().realize()
-      else:
-        npy_keys[key] = np.zeros(shape, dtype=np.float32)
-    elif len(shape) == 2:
-      npy_keys[key] = np.zeros(shape, dtype=np.float32)
+    if key not in npy_arrays and 'img' not in key and key not in ('features_buffer', desire_key):
+      npy_arrays[key] = np.zeros(shape, dtype=np.float32)
 
-  if 'traffic_convention' not in npy_keys:
-    tc_shape = input_shapes.get('traffic_convention', (1, 2))
-    npy_keys['traffic_convention'] = np.zeros(tc_shape, dtype=np.float32)
-
-  npy_keys['tfm'] = np.zeros((3, 3), dtype=np.float32)
-  npy_keys['big_tfm'] = np.zeros((3, 3), dtype=np.float32)
-
-  input_queues = {
+  queues = {
     'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
     'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    **queue_keys,
-    **{k: Tensor(v, device='NPY').realize() for k, v in npy_keys.items()},
+    'desire_q': Tensor(np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]),
+                  dtype=np.float32), device=device).contiguous().realize()
   }
-  return input_queues, npy_keys
+
+  if features_buffer:
+    queues['feat_q'] = Tensor(np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]),
+                       dtype=np.float32), device=device).contiguous().realize()
+
+  queues.update({key: Tensor(value, device='NPY').realize() for key, value in npy_arrays.items()})
+  return queues, npy_arrays
 
 
-def make_run_supercombo(model_runner, nv12: NV12Frame, model_w, model_h,
-                        features_slice, frame_skip, input_shapes, prepare_only=False):
-  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+def make_split_input_queues(vision_input_shapes: dict, policy_input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
+  return generate_queues_and_npy({**vision_input_shapes, **policy_input_shapes}, frame_skip, device)
+
+
+def make_supercombo_input_queues(input_shapes: dict, frame_skip: int, device: str = Device.DEFAULT) -> tuple[dict, dict]:
+  return generate_queues_and_npy(input_shapes, frame_skip, device)
+
+
+def create_jit_runner(vision_runner, policy_runners: list, nv12: NV12Frame, model_size: tuple[int, int],
+            features_slice: slice, frame_skip: int, input_shapes: dict, prepare_only: bool):
+  frame_prepare = make_frame_prepare(nv12, *model_size)
   sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
   sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
 
   desire_key = _detect_desire_key(input_shapes)
-  if desire_key is None:
-    raise ValueError(f"No desire* key found in input_shapes: {list(input_shapes.keys())}")
-  road_img_key, wide_img_key = _detect_vision_keys(input_shapes)
-  extra_policy_keys = [k for k in input_shapes
-                       if k not in (desire_key, 'features_buffer', 'traffic_convention')
-                       and 'img' not in k]
+  road_key, wide_key = _detect_vision_keys(input_shapes)
 
-  def run_supercombo(img_q, big_img_q, feat_q, desire_q,
-                     frame, big_frame, **kwargs):
-    desire = kwargs.get(desire_key)
+  if not desire_key or not road_key or not wide_key:
+    raise ValueError("Missing required vision or desire keys in input shapes.")
+
+  extra_keys = [key for key in input_shapes if key not in (desire_key, 'features_buffer', 'traffic_convention') and 'img' not in key]
+
+  def runner(img_q, big_img_q, feat_q, frame, big_frame, tfm, big_tfm, **kwargs):
+    desire_q = kwargs['desire_q']
+    desire = kwargs['desire']
     traffic_convention = kwargs.get('traffic_convention')
-    tfm = kwargs['tfm']
-    big_tfm = kwargs['big_tfm']
 
-    tfm = tfm.to(Device.DEFAULT)
-    big_tfm = big_tfm.to(Device.DEFAULT)
-    desire = desire.to(Device.DEFAULT)
-    traffic_convention = traffic_convention.to(Device.DEFAULT)
-    Tensor.realize(tfm, big_tfm, desire, traffic_convention)
+    npys = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT), desire.to(Device.DEFAULT)]
+    if traffic_convention is not None:
+      npys.append(traffic_convention.to(Device.DEFAULT))
 
-    img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
-    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
+    extra_tensors = {key: kwargs[key].to(Device.DEFAULT) for key in extra_keys if key in kwargs}
+    Tensor.realize(*npys, *extra_tensors.values())
+
+    tfm_dev, big_tfm_dev, desire_dev = npys[:3]
+    traffic_conv_dev = npys[3] if traffic_convention is not None else None
+
+    img = shift_and_sample(img_q, frame_prepare(frame, tfm_dev).unsqueeze(0), sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm_dev).unsqueeze(0), sample_skip_fn)
 
     if prepare_only:
       return img, big_img
 
-    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-    feat_buf = sample_skip_fn(feat_q)
+    desire_buf = shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)
+    inputs = {desire_key: desire_buf, **extra_tensors}
 
-    inputs = {road_img_key: img, wide_img_key: big_img,
-              desire_key: desire_buf, 'features_buffer': feat_buf,
-              'traffic_convention': traffic_convention}
-    for k in extra_policy_keys:
-      if k in kwargs:
-        inputs[k] = kwargs[k].to(Device.DEFAULT)
+    if traffic_conv_dev is not None:
+      inputs['traffic_convention'] = traffic_conv_dev
 
-    model_out = next(iter(model_runner(inputs).values())).cast('float32')
+    if vision_runner:
+      vision_out = next(iter(vision_runner({road_key: img, wide_key: big_img}).values()))
+      vision_out_cast = vision_out.cast('float32')
+      new_feat = vision_out_cast[:, features_slice].reshape(1, -1).unsqueeze(0)
+      inputs['features_buffer'] = shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+      policy_outs = [next(iter(pol_runner(inputs).values())).cast('float32') for pol_runner in policy_runners]
+      return (vision_out_cast, *policy_outs) if len(policy_outs) > 1 else (vision_out_cast, policy_outs[0])
+    inputs.update({road_key: img, wide_key: big_img, 'features_buffer': sample_skip_fn(feat_q)})
+    policy_out = next(iter(policy_runners[0](inputs).values())).cast('float32')
+    new_feat = policy_out[:, features_slice].reshape(1, -1).unsqueeze(0)
+    shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+    return policy_out
 
-    new_feat = model_out[:, features_slice].reshape(1, -1).unsqueeze(0)
-    shift_and_sample(feat_q, new_feat, sample_skip_fn)
-
-    return model_out
-
-  return run_supercombo
+  return runner
 
 
-def make_run_vision_multi_policy(vision_runner, policy_runners, nv12: NV12Frame, model_w, model_h,
-                                 vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                                 vision_road_key, vision_wide_key, prepare_only=False):
-  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
-  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
-  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
+def compile_and_warmup(nv12: NV12Frame, model_size: tuple[int, int], prepare_only: bool, frame_skip: int, vision_runner, policy_runners: list, metadata: dict):
+  print(f"Compiling combined JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
 
-  def run_multi_policy(img_q, big_img_q, feat_q, desire_q, desire,
-                       traffic_convention, tfm, big_tfm, frame, big_frame, **extra):
-    npy_tensors = [tfm.to(Device.DEFAULT), big_tfm.to(Device.DEFAULT),
-                   desire.to(Device.DEFAULT), traffic_convention.to(Device.DEFAULT)]
-    extra_device = {k: extra[k].to(Device.DEFAULT) for k in extra_policy_keys}
-    Tensor.realize(*npy_tensors, *extra_device.values())
-    tfm, big_tfm, desire, traffic_convention = npy_tensors
+  all_shapes = {key: value for meta in metadata.values() for key, value in meta['input_shapes'].items()}
 
-    img = shift_and_sample(img_q, frame_prepare(frame, tfm).unsqueeze(0), sample_skip_fn)
-    big_img = shift_and_sample(big_img_q, frame_prepare(big_frame, big_tfm).unsqueeze(0), sample_skip_fn)
+  feat_meta = metadata.get('vision') or metadata.get('model') or metadata.get('policy')
+  if not feat_meta:
+    raise ValueError("Could not find vision, model, or policy metadata.")
 
-    if prepare_only:
-      return img, big_img
+  features_slice = feat_meta['output_slices']['hidden_state']
+  WARP_DEV = 'CPU' if "USBGPU" in os.environ else Device.DEFAULT
 
-    vision_out = next(iter(vision_runner({vision_road_key: img, vision_wide_key: big_img}).values())).cast('float32')
+  run_func = create_jit_runner(vision_runner, policy_runners, nv12, model_size, features_slice, frame_skip, all_shapes, prepare_only)
+  run_jit = TinyJit(run_func, prune=True)
+  queues, npy_arrays = generate_queues_and_npy(all_shapes, frame_skip, Device.DEFAULT)
 
-    new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
-    feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
-    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-
-    inputs = {'features_buffer': feat_buf, desire_key: desire_buf, 'traffic_convention': traffic_convention, **extra_device}
-
-    policy_outputs = []
-    for runner in policy_runners:
-      policy_out = next(iter(runner(inputs).values())).cast('float32')
-      policy_outputs.append(policy_out)
-
-    return (vision_out, *policy_outputs)
-
-  return run_multi_policy
-
-
-def _warmup_and_serialize(run_jit, input_queues, npy, nv12):
   for i in range(3):
     np.random.seed(42 + i)
-    frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-    big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype='uint8').realize()
-    for v in npy.values():
-      v[:] = np.random.randn(*v.shape).astype(v.dtype)
+    frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
+    big_frame = Tensor.randint(nv12.size, low=0, high=256, dtype=dtypes.uint8, device=WARP_DEV).realize()
+    for arr in npy_arrays.values():
+      arr[:] = np.random.randn(*arr.shape).astype(arr.dtype)
+
     Device.default.synchronize()
-    st = time.perf_counter()
-    run_jit(**input_queues, frame=frame, big_frame=big_frame)
-    mt = time.perf_counter()
+    start_time = time.perf_counter()
+    run_jit(**queues, frame=frame, big_frame=big_frame)
+    mid_time = time.perf_counter()
     Device.default.synchronize()
-    et = time.perf_counter()
-    print(f"  [{i + 1}/3] enqueue {(mt - st) * 1e3:6.2f} ms -- total {(et - st) * 1e3:6.2f} ms")
-  return pickle.loads(pickle.dumps(run_jit))
+    print(f"  [{i + 1}/3] enqueue {(mid_time - start_time) * 1e3:6.2f} ms -- total {(time.perf_counter() - start_time) * 1e3:6.2f} ms")
+
+  return pickle.loads(pickle.dumps(run_jit)) if not prepare_only else run_jit
 
 
-def compile_supercombo(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
-                       model_runner, metadata):
-  print(f"Compiling combined supercombo JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
-
-  features_slice = metadata['output_slices']['hidden_state']
-  input_shapes = metadata['input_shapes']
-
-  _run = make_run_supercombo(model_runner, nv12, model_w, model_h,
-                             features_slice, frame_skip, input_shapes, prepare_only)
-  run_jit = TinyJit(_run, prune=True)
-
-  input_queues, npy = make_supercombo_input_queues(input_shapes, frame_skip, Device.DEFAULT)
-
-  run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
-  return run_jit
+def _parse_size(size_str: str) -> tuple[int, int]:
+  width, height = size_str.lower().split('x')
+  return int(width), int(height)
 
 
-def compile_multi_policy(nv12: NV12Frame, model_w, model_h, prepare_only, frame_skip,
-                         vision_runner, policy_runners, vision_metadata, policy_metadata):
-  print(f"Compiling combined multi-policy JIT for {nv12.width}x{nv12.height} (prepare_only={prepare_only})...")
-
-  vision_features_slice = vision_metadata['output_slices']['hidden_state']
-  vision_input_shapes = vision_metadata['input_shapes']
-  policy_input_shapes = policy_metadata['input_shapes']
-  desire_key = _detect_desire_key(policy_input_shapes)
-  extra_policy_keys = [k for k in policy_input_shapes if k not in ('features_buffer', desire_key, 'traffic_convention')]
-  vision_road_key, vision_wide_key = _detect_vision_keys(vision_input_shapes)
-
-  _run = make_run_vision_multi_policy(vision_runner, policy_runners, nv12, model_w, model_h,
-                                      vision_features_slice, frame_skip, desire_key, extra_policy_keys,
-                                      vision_road_key, vision_wide_key, prepare_only)
-  run_jit = TinyJit(_run, prune=True)
-
-  input_queues, npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
-
-  run_jit = _warmup_and_serialize(run_jit, input_queues, npy, nv12)
-  return run_jit
+def read_file_chunked_to_shm(path):
+  if not path:
+    return None
+  import atexit
+  from openpilot.common.file_chunker import read_file_chunked
+  from openpilot.system.hardware.hw import Paths
+  shm_path = os.path.join(Paths.shm_path(), os.path.basename(path))
+  atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path))
+  with open(shm_path, 'wb') as f:
+    f.write(read_file_chunked(path))
+  return shm_path
 
 
-def _parse_size(s):
-  w, h = s.lower().split('x')
-  return int(w), int(h)
+def _compile_for_resolutions(camera_resolutions: list, model_size: tuple[int, int], frame_skip: int,
+               vision_runner, policy_runners: list, metadata: dict) -> dict:
+  from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  return {
+    (cam_w, cam_h): {
+      name: compile_and_warmup(NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)), model_size, prepare_only,
+                   frame_skip, vision_runner, policy_runners, metadata)
+      for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
+    }
+    for cam_w, cam_h in camera_resolutions
+  }
+
+
+def _load_policy_runners(args: argparse.Namespace) -> tuple[list, list]:
+  runners, keys = [], []
+  for name, onnx_arg in [('policy', args.policy_onnx), ('off_policy', args.off_policy_onnx), ('on_policy', args.on_policy_onnx)]:
+    if onnx_arg:
+      runners.append(OnnxRunner(onnx_arg))
+      keys.append(name)
+  return runners, keys
 
 
 if __name__ == "__main__":
-  from tinygrad.nn.onnx import OnnxRunner
-  from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
   from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
+  from tinygrad.nn.onnx import OnnxRunner
 
-  p = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
-  p.add_argument('--model-type', choices=MODEL_TYPES, required=True)
-  p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
-  p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
-  p.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
-  p.add_argument('--output', required=True)
+  parser = argparse.ArgumentParser(description="Compile combined JIT pkl for sunnypilot modeld_v2")
+  parser.add_argument('--model-type', choices=MODEL_TYPES, required=True)
+  parser.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
+  parser.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
+  parser.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
+  parser.add_argument('--output', required=True)
 
-  p.add_argument('--vision-onnx', help='vision ONNX (for split models)')
-  p.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
-  p.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
-  p.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
-  p.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
+  parser.add_argument('--vision-onnx', help='vision ONNX (for split models)')
+  parser.add_argument('--policy-onnx', help='policy ONNX (for vision_policy)')
+  parser.add_argument('--off-policy-onnx', help='off-policy ONNX (for vision_multi_policy)')
+  parser.add_argument('--on-policy-onnx', help='on-policy ONNX (for vision_multi_policy)')
+  parser.add_argument('--supercombo-onnx', help='supercombo ONNX (for supercombo)')
 
-  args = p.parse_args()
-  out = defaultdict(dict)
+  args = parser.parse_args()
+  output_data = defaultdict(dict)
+
+  args.vision_onnx = read_file_chunked_to_shm(args.vision_onnx)
+  args.policy_onnx = read_file_chunked_to_shm(args.policy_onnx)
+  args.off_policy_onnx = read_file_chunked_to_shm(args.off_policy_onnx)
+  args.on_policy_onnx = read_file_chunked_to_shm(args.on_policy_onnx)
+  args.supercombo_onnx = read_file_chunked_to_shm(args.supercombo_onnx)
+
+  vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
 
   if args.model_type == 'vision_policy':
-    assert args.vision_onnx and args.policy_onnx
-    vision_runner = OnnxRunner(args.vision_onnx)
-    policy_runner = OnnxRunner(args.policy_onnx)
-    out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
-    out['metadata']['policy'] = make_metadata_dict(args.policy_onnx)
-
-    frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
-                                                                                       out['metadata']['policy']['input_shapes'])
-
-    for cam_w, cam_h in args.camera_resolutions:
-      nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      model_w, model_h = args.model_size
-      out[(cam_w, cam_h)] = {
-        name: compile_split_policy(nv12, model_w, model_h, prepare_only, frame_skip,
-                                   vision_runner, policy_runner,
-                                   out['metadata']['vision'], out['metadata']['policy'])
-        for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
-      }
-
+    assert vision_runner and args.policy_onnx
+    policy_runners = [OnnxRunner(args.policy_onnx)]
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
   elif args.model_type == 'supercombo':
     assert args.supercombo_onnx
-    model_runner = OnnxRunner(args.supercombo_onnx)
-    out['metadata']['model'] = make_metadata_dict(args.supercombo_onnx)
-
-    frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip({}, out['metadata']['model']['input_shapes'])
-
-    for cam_w, cam_h in args.camera_resolutions:
-      nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      model_w, model_h = args.model_size
-      out[(cam_w, cam_h)] = {
-        name: compile_supercombo(nv12, model_w, model_h, prepare_only, frame_skip,
-                                 model_runner, out['metadata']['model'])
-        for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
-      }
-
+    policy_runners = [OnnxRunner(args.supercombo_onnx)]
+    output_data['metadata'] = {'model': make_metadata_dict(args.supercombo_onnx)}
   elif args.model_type == 'vision_multi_policy':
-    assert args.vision_onnx
-    vision_runner = OnnxRunner(args.vision_onnx)
-    out['metadata']['vision'] = make_metadata_dict(args.vision_onnx)
+    assert vision_runner
+    policy_runners, policy_names = _load_policy_runners(args)
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
+    for name in policy_names:
+      runner_arg = getattr(args, f"{name}_onnx")
+      output_data['metadata'][name] = make_metadata_dict(runner_arg)
 
-    policy_runners = []
-    policy_onnxes = []
-    if args.policy_onnx:
-      policy_onnxes.append(('policy', args.policy_onnx))
-    if args.off_policy_onnx:
-      policy_onnxes.append(('off_policy', args.off_policy_onnx))
-    if args.on_policy_onnx:
-      policy_onnxes.append(('on_policy', args.on_policy_onnx))
+  policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
+  first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
+  vision_meta = output_data['metadata'].get('vision', {})
 
-    for name, onnx_path in policy_onnxes:
-      runner = OnnxRunner(onnx_path)
-      policy_runners.append(runner)
-      out['metadata'][name] = make_metadata_dict(onnx_path)
+  derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
+  output_data.update(_compile_for_resolutions(args.camera_resolutions, args.model_size, derived_frame_skip,
+                        vision_runner, policy_runners, output_data['metadata']))
 
-    first_policy_key = policy_onnxes[0][0]
-    frame_skip = args.frame_skip if args.frame_skip is not None else derive_frame_skip(out['metadata']['vision']['input_shapes'],
-                                                                                       out['metadata'][first_policy_key]['input_shapes'])
+  with open(args.output, "wb") as file:
+    pickle.dump(output_data, file)
 
-    for cam_w, cam_h in args.camera_resolutions:
-      nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      model_w, model_h = args.model_size
-      out[(cam_w, cam_h)] = {
-        name: compile_multi_policy(nv12, model_w, model_h, prepare_only, frame_skip,
-                                   vision_runner, policy_runners,
-                                   out['metadata']['vision'], out['metadata'][first_policy_key])
-        for name, prepare_only in [('warp_enqueue', True), ('run_policy', False)]
-      }
-
-  with open(args.output, "wb") as f:
-    pickle.dump(out, f)
   pkl_size = os.path.getsize(args.output)
   print(f"Saved combined JIT to {args.output} ({pkl_size / 1e6:.2f} MB)")
 
   from openpilot.common.file_chunker import chunk_file, get_chunk_targets
   chunk_targets = get_chunk_targets(args.output, pkl_size)
   chunk_file(args.output, chunk_targets)
-  num_chunks = len(chunk_targets) - 1
-  print(f"Chunked into {num_chunks} file(s)")
+  print(f"Chunked into {len(chunk_targets) - 1} file(s)")

sunnypilot/modeld_v2/modeld.py

@@ -40,7 +40,6 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
-from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "selfdrive.modeld.modeld_tinygrad"
 
@@ -63,11 +62,6 @@ def _find_driving_pkl(bundle):
   if _pkl_exists(pkl_path):
     return pkl_path
 
-  fallback = os.path.join(model_root, 'driving_tinygrad.pkl')
-  if _pkl_exists(fallback):
-    return fallback
-  return None
-
 
 class FrameMeta:
   frame_id: int = 0
@@ -126,7 +120,7 @@ class ModelState(ModelStateBase):
       self._vision_input_names = [k for k in model_metadata['input_shapes'] if 'img' in k]
       from openpilot.sunnypilot.modeld_v2.compile_modeld import make_supercombo_input_queues
       frame_skip = derive_frame_skip({}, model_metadata['input_shapes'])
-      self.input_queues, self.npy = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.DEV)
+      self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'], frame_skip, device=self.DEV)
     else:
       vision_metadata = metadata['vision']
       policy_keys = [k for k in metadata if k != 'vision']
@@ -144,7 +138,7 @@ class ModelState(ModelStateBase):
       policy_input_shapes = first_policy_metadata['input_shapes']
       self._vision_input_names = [k for k in vision_input_shapes if 'img' in k]
       frame_skip = derive_frame_skip(vision_input_shapes, policy_input_shapes)
-      self.input_queues, self.npy = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.DEV)
+      self.input_queues, self.numpy_inputs = make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device=self.DEV)
 
     from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
     from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
@@ -184,7 +178,7 @@ class ModelState(ModelStateBase):
 
   @property
   def desire_key(self) -> str:
-    return next(k for k in self.npy if k.startswith('desire'))
+    return next(k for k in self.numpy_inputs if k.startswith('desire'))
 
   def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
                 inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
@@ -200,16 +194,16 @@ class ModelState(ModelStateBase):
 
     desire_key = self.desire_key
     inputs[desire_key][0] = 0
-    self.npy[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
+    self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
     self.prev_desire[:] = inputs[desire_key]
     for key in ('traffic_convention', 'lateral_control_params'):
-      if key in self.npy and key in inputs:
-        self.npy[key][:] = inputs[key]
+      if key in self.numpy_inputs and key in inputs:
+        self.numpy_inputs[key][:] = inputs[key]
 
     road_key = next(n for n in bufs if 'big' not in n)
     wide_key = next(n for n in bufs if 'big' in n)
-    self.npy['tfm'][:, :] = transforms[road_key].reshape(3, 3)
-    self.npy['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)
+    self.numpy_inputs['tfm'][:, :] = transforms[road_key].reshape(3, 3)
+    self.numpy_inputs['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)
 
     if prepare_only:
       self._warp_enqueue(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
@@ -237,8 +231,8 @@ class ModelState(ModelStateBase):
       if 'planplus' in outputs and 'plan' in outputs:
         outputs['plan'] = outputs['plan'] + outputs['planplus']
 
-    if 'desired_curvature' in outputs and 'prev_desired_curv' in self.npy:
-      buf = self.npy['prev_desired_curv']
+    if 'desired_curvature' in outputs and 'prev_desired_curv' in self.numpy_inputs:
+      buf = self.numpy_inputs['prev_desired_curv']
       buf[0, :-1] = buf[0, 1:]
       buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0
 
@@ -330,7 +324,6 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
-  RELC = RoadEdgeLaneChangeController(DH)
   meta_constants = load_meta_constants()
 
   while True:
@@ -411,7 +404,7 @@ def main(demo=False):
       'traffic_convention': traffic_convention,
     }
 
-    if 'lateral_control_params' in model.npy:
+    if 'lateral_control_params' in model.numpy_inputs:
       inputs['lateral_control_params'] = np.array([v_ego, lat_delay], dtype=np.float32)
 
     mt1 = time.perf_counter()
@@ -435,10 +428,7 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      RELC.update(modelv2_send.modelV2.roadEdgeStds, modelv2_send.modelV2.laneLineProbs, v_ego)
-      mdv2sp_send.modelDataV2SP.leftLaneChangeEdgeBlock = RELC.left_edge_detected
-      mdv2sp_send.modelDataV2SP.rightLaneChangeEdgeBlock = RELC.right_edge_detected
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, RELC.left_edge_detected, RELC.right_edge_detected)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

sunnypilot/modeld_v2/models/README.md

@@ -1,62 +0,0 @@
-## Neural networks in openpilot
-To view the architecture of the ONNX networks, you can use [netron](https://netron.app/)
-
-## Supercombo
-### Supercombo input format (Full size: 799906 x float32)
-* **image stream**
-  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
-    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
-      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
-      * Channel 4 represents the half-res U channel
-      * Channel 5 represents the half-res V channel
-* **wide image stream**
-  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
-    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
-      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
-      * Channel 4 represents the half-res U channel
-      * Channel 5 represents the half-res V channel
-* **desire**
-  * one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8
-* **traffic convention**
-  * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
-* **feature buffer**
-  * A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512
-
-
-### Supercombo output format (Full size: XXX x float32)
-Read [here](https://github.com/commaai/openpilot/blob/90af436a121164a51da9fa48d093c29f738adf6a/selfdrive/modeld/models/driving.h#L236) for more.
-
-
-## Driver Monitoring Model
-* .onnx model can be run with onnx runtimes
-* .dlc file is a pre-quantized model and only runs on qualcomm DSPs
-
-### input format
-* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format:
-  * full input size is 1440 * 960 = 1382400
-  * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner)
-* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs
-
-### output format
-* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33))
-  * for each person in the front seats (2 * 41)
-    * face pose: 12 = 6 + 6
-      * face orientation [pitch, yaw, roll] in camera frame: 3
-      * face position [dx, dy] relative to image center: 2
-      * normalized face size: 1
-      * standard deviations for above outputs: 6
-    * face visible probability: 1
-    * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1
-      * eye position and size, and their standard deviations: 8
-      * eye visible probability: 1
-      * eye closed probability: 1
-    * wearing sunglasses probability: 1
-    * face occluded probability: 1
-    * touching wheel probability: 1
-    * paying attention probability: 1
-    * (deprecated) distracted probabilities: 2
-    * using phone probability: 1
-    * distracted probability: 1
-  * common outputs 2
-    * poor camera vision probability: 1
-    * left hand drive probability: 1

sunnypilot/modeld_v2/tests/dmon_lag/repro.cc

@@ -1,101 +0,0 @@
-// clang++ -O2 repro.cc && ./a.out
-
-#include <sched.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-#include <cstdint>
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-#include <ctime>
-
-static inline double millis_since_boot() {
-  struct timespec t;
-  clock_gettime(CLOCK_BOOTTIME, &t);
-  return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6;
-}
-
-#define MODEL_WIDTH 320
-#define MODEL_HEIGHT 640
-
-// null function still breaks it
-#define input_lambda(x) x
-
-// this is copied from models/dmonitoring.cc, and is the code that triggers the issue
-void inner(uint8_t *resized_buf, float *net_input_buf) {
-  int resized_width = MODEL_WIDTH;
-  int resized_height = MODEL_HEIGHT;
-
-  // one shot conversion, O(n) anyway
-  // yuvframe2tensor, normalize
-  for (int r = 0; r < MODEL_HEIGHT/2; r++) {
-    for (int c = 0; c < MODEL_WIDTH/2; c++) {
-      // Y_ul
-      net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]);
-      // Y_ur
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]);
-      // Y_dl
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]);
-      // Y_dr
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]);
-      // U
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]);
-      // V
-      net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]);
-    }
-  }
-}
-
-float trial() {
-  int resized_width = MODEL_WIDTH;
-  int resized_height = MODEL_HEIGHT;
-
-  int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
-
-  // allocate the buffers
-  uint8_t *resized_buf = (uint8_t*)malloc(resized_width*resized_height*3/2);
-  float *net_input_buf = (float*)malloc(yuv_buf_len*sizeof(float));
-  printf("allocate -- %p 0x%x -- %p 0x%lx\n", resized_buf, resized_width*resized_height*3/2, net_input_buf, yuv_buf_len*sizeof(float));
-
-  // test for bad buffers
-  static int CNT = 20;
-  float avg = 0.0;
-  for (int i = 0; i < CNT; i++) {
-    double s4 = millis_since_boot();
-    inner(resized_buf, net_input_buf);
-    double s5 = millis_since_boot();
-    avg += s5-s4;
-  }
-  avg /= CNT;
-
-  // once it's bad, it's reliably bad
-  if (avg > 10) {
-    printf("HIT %f\n", avg);
-    printf("BAD\n");
-
-    for (int i = 0; i < 200; i++) {
-      double s4 = millis_since_boot();
-      inner(resized_buf, net_input_buf);
-      double s5 = millis_since_boot();
-      printf("%.2f   ", s5-s4);
-    }
-    printf("\n");
-
-    exit(0);
-  }
-
-  // don't free so we get a different buffer each time
-  //free(resized_buf);
-  //free(net_input_buf);
-
-  return avg;
-}
-
-int main() {
-  while (true) {
-    float ret = trial();
-    printf("got %f\n", ret);
-  }
-}
-

sunnypilot/modeld_v2/tests/test_combined_pkl_loader.py

@@ -46,16 +46,6 @@ class TestFindDrivingPkl:
     assert result is not None
     assert 'driving_fof_tinygrad.pkl' in result
 
-  def test_finds_fallback_driving_tinygrad(self, tmp_path, monkeypatch):
-    (tmp_path / 'driving_tinygrad.pkl').write_bytes(b'fake')
-    from openpilot.system.hardware import hw
-    monkeypatch.setattr(hw.Paths, 'model_root', staticmethod(lambda: str(tmp_path)))
-
-    bundle = DummyBundle(models=[DummyModel('vision', 'nonexistent.pkl')])
-    result = _find_driving_pkl(bundle)
-    assert result is not None
-    assert 'driving_tinygrad.pkl' in result
-
 
 # Init — assertion guard
 
@@ -84,8 +74,8 @@ class TestStockEquivalence:
     skip_keys = {'action_t'}
     assert set(state.input_queues.keys()) == set(stock_queues.keys()) - skip_keys, \
       f"Queue keys differ: v2={set(state.input_queues.keys())}, stock={set(stock_queues.keys())}"
-    assert set(state.npy.keys()) == set(stock_npy.keys()) - skip_keys, \
-      f"Npy keys differ: v2={set(state.npy.keys())}, stock={set(stock_npy.keys())}"
+    assert set(state.numpy_inputs.keys()) == set(stock_npy.keys()) - skip_keys, \
+      f"Npy keys differ: v2={set(state.numpy_inputs.keys())}, stock={set(stock_npy.keys())}"
 
   def test_split_queue_keys_work_with_desire_key(self, model_state_factory):
     from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues
@@ -188,16 +178,16 @@ class TestInputQueueCreation:
   def test_npy_contains_transforms(self, archetype_name, model_state_factory):
     arch = ARCHETYPES[archetype_name]
     state = model_state_factory(arch)
-    assert 'tfm' in state.npy, f"{arch.name}: 'tfm' missing from npy"
-    assert 'big_tfm' in state.npy, f"{arch.name}: 'big_tfm' missing from npy"
-    assert state.npy['tfm'].shape == (3, 3)
-    assert state.npy['big_tfm'].shape == (3, 3)
+    assert 'tfm' in state.numpy_inputs, f"{arch.name}: 'tfm' missing from npy"
+    assert 'big_tfm' in state.numpy_inputs, f"{arch.name}: 'big_tfm' missing from npy"
+    assert state.numpy_inputs['tfm'].shape == (3, 3)
+    assert state.numpy_inputs['big_tfm'].shape == (3, 3)
 
   @pytest.mark.parametrize("archetype_name", ARCHETYPE_NAMES)
   def test_npy_contains_desire(self, archetype_name, model_state_factory):
     arch = ARCHETYPES[archetype_name]
     state = model_state_factory(arch)
-    assert arch.expected_desire_key in state.npy, \
+    assert arch.expected_desire_key in state.numpy_inputs, \
       f"{arch.name}: '{arch.expected_desire_key}' missing from npy"
 
 

sunnypilot/modeld_v2/tests/test_warp.py

@@ -1,103 +0,0 @@
-import os
-os.environ['DEV'] = 'CPU'
-import pytest
-import numpy as np
-from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
-from openpilot.sunnypilot.modeld_v2.warp import CAMERA_CONFIGS
-from openpilot.sunnypilot.modeld_v2.warp import Warp, MODEL_W, MODEL_H
-
-VISION_NAME_PAIRS = [  # needed to account for supercombos input_imgs
-  ('img', 'big_img'),
-  ('input_imgs', 'big_input_imgs'),
-]
-
-
-class MockVisionBuf:
-  def __init__(self, w, h):
-    self.width = w
-    self.height = h
-    _, _, _, yuv_size = get_nv12_info(w, h)
-    self.data = np.zeros(yuv_size, dtype=np.uint8)
-
-
-@pytest.mark.parametrize("buffer_length", [2, 5])
-def test_warp_initialization(buffer_length):
-  warp = Warp(buffer_length)
-  assert warp.buffer_length == buffer_length
-  assert warp.img_buffer_shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-
-
-@pytest.mark.parametrize("buffer_length", [2, 5])
-@pytest.mark.parametrize("cam_w, cam_h", CAMERA_CONFIGS)
-@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
-def test_warp_process(buffer_length, cam_w, cam_h, road, wide):
-  warp = Warp(buffer_length)
-  mock_buf = MockVisionBuf(cam_w, cam_h)
-  transform = np.eye(3, dtype=np.float32).flatten()
-  bufs = {road: mock_buf, wide: mock_buf}
-  transforms = {road: transform, wide: transform}
-
-  out = warp.process(bufs, transforms)
-  assert isinstance(out, dict)
-  assert road in out and wide in out
-  assert out[road].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
-  assert out[wide].shape == (1, 12, MODEL_H // 2, MODEL_W // 2)
-
-  key = (cam_w, cam_h)
-  assert key in warp.jit_cache
-
-  out2 = warp.process(bufs, transforms)
-  assert out2[road].shape == out[road].shape
-
-
-@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
-def test_warp_buffer_shift(road, wide):
-  warp = Warp(2)
-  cam_w, cam_h = CAMERA_CONFIGS[1]
-  transform = np.eye(3, dtype=np.float32).flatten()
-
-  buf1 = MockVisionBuf(cam_w, cam_h)
-  buf1.data[0] = 255
-  bufs1 = {road: buf1, wide: buf1}
-  transforms = {road: transform, wide: transform}
-  out1 = warp.process(bufs1, transforms)
-  road1 = out1[road].numpy().copy()
-
-  buf2 = MockVisionBuf(cam_w, cam_h)
-  buf2.data[0] = 128
-  bufs2 = {road: buf2, wide: buf2}
-  out2 = warp.process(bufs2, transforms)
-  assert not np.array_equal(road1, out2[road].numpy())
-
-
-@pytest.mark.parametrize("buffer_length", [2, 5])
-@pytest.mark.parametrize("road, wide", VISION_NAME_PAIRS)
-def test_warp_buffer_accumulation(buffer_length, road, wide):
-  warp = Warp(buffer_length)
-  cam_w, cam_h = CAMERA_CONFIGS[0]
-  transform = np.eye(3, dtype=np.float32).flatten()
-  transforms = {road: transform, wide: transform}
-  outputs = []
-
-  for i in range(buffer_length + 1):
-    buf = MockVisionBuf(cam_w, cam_h)
-    buf.data[:] = i * 10
-    out = warp.process({road: buf, wide: buf}, transforms)
-    outputs.append(out[road].numpy().copy())
-
-  assert warp.full_buffers['img'].shape == (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-  for i in range(1, len(outputs)):
-    assert not np.array_equal(outputs[i - 1], outputs[i])
-
-
-def test_warp_different_cameras_same_instance():
-  warp = Warp(2)
-  transform = np.eye(3, dtype=np.float32).flatten()
-
-  buf1 = MockVisionBuf(*CAMERA_CONFIGS[0])
-  warp.process({'img': buf1, 'big_img': buf1}, {'img': transform, 'big_img': transform})
-  assert len(warp.jit_cache) == 1
-
-  buf2 = MockVisionBuf(*CAMERA_CONFIGS[1])
-  warp.process({'img': buf2, 'big_img': buf2}, {'img': transform, 'big_img': transform})
-  assert len(warp.jit_cache) == 2

sunnypilot/modeld_v2/tests/tf_test/build.sh

@@ -1,2 +0,0 @@
-#!/usr/bin/env bash
-clang++ -I /home/batman/one/external/tensorflow/include/ -L /home/batman/one/external/tensorflow/lib -Wl,-rpath=/home/batman/one/external/tensorflow/lib main.cc -ltensorflow

sunnypilot/modeld_v2/tests/tf_test/main.cc

@@ -1,69 +0,0 @@
-#include <cassert>
-#include <cstdio>
-#include <cstdlib>
-#include "tensorflow/c/c_api.h"
-
-void* read_file(const char* path, size_t* out_len) {
-  FILE* f = fopen(path, "r");
-  if (!f) {
-    return NULL;
-  }
-  fseek(f, 0, SEEK_END);
-  long f_len = ftell(f);
-  rewind(f);
-
-  char* buf = (char*)calloc(f_len, 1);
-  assert(buf);
-
-  size_t num_read = fread(buf, f_len, 1, f);
-  fclose(f);
-
-  if (num_read != 1) {
-    free(buf);
-    return NULL;
-  }
-
-  if (out_len) {
-    *out_len = f_len;
-  }
-
-  return buf;
-}
-
-static void DeallocateBuffer(void* data, size_t) {
-  free(data);
-}
-
-int main(int argc, char* argv[]) {
-  TF_Buffer* buf;
-  TF_Graph* graph;
-  TF_Status* status;
-  char *path = argv[1];
-
-  // load model
-  {
-    size_t model_size;
-    char tmp[1024];
-    snprintf(tmp, sizeof(tmp), "%s.pb", path);
-    printf("loading model %s\n", tmp);
-    uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size);
-    buf = TF_NewBuffer();
-    buf->data = model_data;
-    buf->length = model_size;
-    buf->data_deallocator = DeallocateBuffer;
-    printf("loaded model of size %d\n", model_size);
-  }
-
-  // import graph
-  status = TF_NewStatus();
-  graph = TF_NewGraph();
-  TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions();
-  TF_GraphImportGraphDef(graph, buf, opts, status);
-  TF_DeleteImportGraphDefOptions(opts);
-  TF_DeleteBuffer(buf);
-  if (TF_GetCode(status) != TF_OK) {
-    printf("FAIL: %s\n", TF_Message(status));
-  } else {
-    printf("SUCCESS\n");
-  }
-}

sunnypilot/modeld_v2/tests/tf_test/pb_loader.py

@@ -1,8 +0,0 @@
-#!/usr/bin/env python3
-import sys
-import tensorflow as tf
-
-with open(sys.argv[1], "rb") as f:
-  graph_def = tf.compat.v1.GraphDef()
-  graph_def.ParseFromString(f.read())
-  #tf.io.write_graph(graph_def, '', sys.argv[1]+".try")

sunnypilot/modeld_v2/tests/timing/benchmark.py

@@ -1,38 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import time
-import numpy as np
-
-import cereal.messaging as messaging
-from openpilot.system.manager.process_config import managed_processes
-
-
-N = int(os.getenv("N", "5"))
-TIME = int(os.getenv("TIME", "30"))
-
-if __name__ == "__main__":
-  sock = messaging.sub_sock('modelV2', conflate=False, timeout=1000)
-
-  execution_times = []
-
-  for _ in range(N):
-    os.environ['LOGPRINT'] = 'debug'
-    managed_processes['modeld'].start()
-    time.sleep(5)
-
-    t = []
-    start = time.monotonic()
-    while time.monotonic() - start < TIME:
-      msgs = messaging.drain_sock(sock, wait_for_one=True)
-      for m in msgs:
-        t.append(m.modelV2.modelExecutionTime)
-
-    execution_times.append(np.array(t[10:]) * 1000)
-    managed_processes['modeld'].stop()
-
-  print("\n\n")
-  print(f"ran modeld {N} times for {TIME}s each")
-  for _, t in enumerate(execution_times):
-    print(f"\tavg: {sum(t)/len(t):0.2f}ms, min: {min(t):0.2f}ms, max: {max(t):0.2f}ms")
-  print("\n\n")

sunnypilot/modeld_v2/warp.py

@@ -1,171 +0,0 @@
-import pickle
-import time
-import numpy as np
-from pathlib import Path
-from tinygrad.tensor import Tensor
-from tinygrad.engine.jit import TinyJit
-from tinygrad.device import Device
-
-from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
-from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
-from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
-from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare as _make_frame_prepare
-
-CAMERA_CONFIGS = [
-  (_ar_ox_fisheye.width, _ar_ox_fisheye.height),
-  (_os_fisheye.width, _os_fisheye.height),
-]
-
-
-def make_frame_prepare(cam_w, cam_h, model_w, model_h):
-  nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-  return _make_frame_prepare(nv12, model_w, model_h)
-
-
-def warp_pkl_path(w, h):
-  from openpilot.selfdrive.modeld.helpers import MODELS_DIR
-  return MODELS_DIR / f'warp_{w}x{h}_tinygrad.pkl'
-
-
-def make_update_img_input(frame_prepare, model_w, model_h):
-  def update_img_input_tinygrad(tensor, frame, M_inv):
-    M_inv = M_inv.to(Device.DEFAULT)
-    new_img = frame_prepare(frame, M_inv)
-    tensor.assign(tensor[6:].cat(new_img, dim=0).contiguous())
-    return Tensor.cat(tensor[:6], tensor[-6:], dim=0).contiguous().reshape(1, 12, model_h//2, model_w//2)
-  return update_img_input_tinygrad
-
-
-def make_update_both_imgs(frame_prepare, model_w, model_h):
-  update_img = make_update_img_input(frame_prepare, model_w, model_h)
-  def update_both_imgs_tinygrad(calib_img_buffer, new_img, M_inv,
-                                calib_big_img_buffer, new_big_img, M_inv_big):
-    calib_img_pair = update_img(calib_img_buffer, new_img, M_inv)
-    calib_big_img_pair = update_img(calib_big_img_buffer, new_big_img, M_inv_big)
-    return calib_img_pair, calib_big_img_pair
-  return update_both_imgs_tinygrad
-
-MODELS_DIR = Path(__file__).parent / 'models'
-MODEL_W, MODEL_H = MEDMODEL_INPUT_SIZE
-UPSTREAM_BUFFER_LENGTH = 5
-
-
-def v2_warp_pkl_path(cam_w, cam_h, buffer_length):
-  return MODELS_DIR / f'warp_{cam_w}x{cam_h}_b{buffer_length}_tinygrad.pkl'
-
-
-def compile_v2_warp(cam_w, cam_h, buffer_length):
-  _, _, _, yuv_size = get_nv12_info(cam_w, cam_h)
-  img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-
-  print(f"Compiling v2 warp for {cam_w}x{cam_h} buffer_length={buffer_length}...")
-
-  frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
-  update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
-  update_img_jit = TinyJit(update_both_imgs, prune=True)
-
-  full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
-  big_full_buffer = Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize()
-  new_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  new_big_frame_np = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  for i in range(10):
-    img_inputs = [full_buffer,
-                  Tensor.from_blob(new_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-                  Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
-    big_img_inputs = [big_full_buffer,
-                      Tensor.from_blob(new_big_frame_np.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-                      Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY')]
-    inputs = img_inputs + big_img_inputs
-    Device.default.synchronize()
-
-    st = time.perf_counter()
-    _ = update_img_jit(*inputs)
-    mt = time.perf_counter()
-    Device.default.synchronize()
-    et = time.perf_counter()
-    print(f"  [{i+1}/10] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
-
-  pkl_path = v2_warp_pkl_path(cam_w, cam_h, buffer_length)
-  with open(pkl_path, "wb") as f:
-    pickle.dump(update_img_jit, f)
-  print(f"  Saved to {pkl_path}")
-
-  jit = pickle.load(open(pkl_path, "rb"))
-  verify_frame = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  verify_big_frame = np.random.randint(0, 256, yuv_size, dtype=np.uint8)
-  fresh_inputs = [
-    Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
-    Tensor.from_blob(verify_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-    Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
-    Tensor.zeros(img_buffer_shape, dtype='uint8').contiguous().realize(),
-    Tensor.from_blob(verify_big_frame.ctypes.data, (yuv_size,), dtype='uint8').realize(),
-    Tensor(Tensor.randn(3, 3).mul(8).realize().numpy(), device='NPY'),
-  ]
-  jit(*fresh_inputs)
-
-
-class Warp:
-  def __init__(self, buffer_length=2):
-    self.buffer_length = buffer_length
-    self.img_buffer_shape = (buffer_length * 6, MODEL_H // 2, MODEL_W // 2)
-
-    self.jit_cache = {}
-    self.full_buffers = {k: Tensor.zeros(self.img_buffer_shape, dtype='uint8').contiguous().realize() for k in ['img', 'big_img']}
-    self._blob_cache: dict[int, Tensor] = {}
-    self._nv12_cache: dict[tuple[int, int], int] = {}
-    self.transforms_np = {k: np.zeros((3, 3), dtype=np.float32) for k in ['img', 'big_img']}
-    self.transforms = {k: Tensor(v, device='NPY').realize() for k, v in self.transforms_np.items()}
-
-  def process(self, bufs, transforms):
-    if not bufs:
-      return {}
-    road = next(n for n in bufs if 'big' not in n)
-    wide = next(n for n in bufs if 'big' in n)
-    cam_w, cam_h = bufs[road].width, bufs[road].height
-    key = (cam_w, cam_h)
-
-    if key not in self.jit_cache:
-      v2_pkl = v2_warp_pkl_path(cam_w, cam_h, self.buffer_length)
-      if v2_pkl.exists():
-        with open(v2_pkl, 'rb') as f:
-          self.jit_cache[key] = pickle.load(f)
-      elif self.buffer_length == UPSTREAM_BUFFER_LENGTH:
-        upstream_pkl = warp_pkl_path(cam_w, cam_h)
-        if upstream_pkl.exists():
-          with open(upstream_pkl, 'rb') as f:
-            self.jit_cache[key] = pickle.load(f)
-      if key not in self.jit_cache:
-        frame_prepare = make_frame_prepare(cam_w, cam_h, MODEL_W, MODEL_H)
-        update_both_imgs = make_update_both_imgs(frame_prepare, MODEL_W, MODEL_H)
-        self.jit_cache[key] = TinyJit(update_both_imgs, prune=True)
-
-    if key not in self._nv12_cache:
-      self._nv12_cache[key] = get_nv12_info(cam_w, cam_h)[3]
-    yuv_size = self._nv12_cache[key]
-
-    road_ptr = bufs[road].data.ctypes.data
-    wide_ptr = bufs[wide].data.ctypes.data
-    if road_ptr not in self._blob_cache:
-      self._blob_cache[road_ptr] = Tensor.from_blob(road_ptr, (yuv_size,), dtype='uint8')
-    if wide_ptr not in self._blob_cache:
-      self._blob_cache[wide_ptr] = Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
-    road_blob = self._blob_cache[road_ptr]
-    wide_blob = self._blob_cache[wide_ptr] if wide_ptr != road_ptr else Tensor.from_blob(wide_ptr, (yuv_size,), dtype='uint8')
-    np.copyto(self.transforms_np['img'], transforms[road].reshape(3, 3))
-    np.copyto(self.transforms_np['big_img'], transforms[wide].reshape(3, 3))
-
-    Device.default.synchronize()
-    res = self.jit_cache[key](
-      self.full_buffers['img'], road_blob, self.transforms['img'],
-      self.full_buffers['big_img'], wide_blob, self.transforms['big_img'],
-    )
-    out_road = res[0].realize()
-    out_wide = res[1].realize()
-
-    return {road: out_road, wide: out_wide}
-
-
-if __name__ == "__main__":
-  for cam_w, cam_h in CAMERA_CONFIGS:
-    for bl in [2, 5]:
-      compile_v2_warp(cam_w, cam_h, bl)

sunnypilot/models/helpers.py

@@ -6,80 +6,138 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 import hashlib
+import os
 import pickle
+from pathlib import Path
 import numpy as np
 
-from openpilot.common.params import Params
 from cereal import custom
-from openpilot.sunnypilot.models.constants import Meta, MetaTombRaider, MetaSimPose
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRaider
 from openpilot.system.hardware.hw import Paths
-from pathlib import Path
-
-# see the README.md for more details on the model selector versioning
-CURRENT_SELECTOR_VERSION = 15
-REQUIRED_MIN_SELECTOR_VERSION = 14
 
+# SET ME TO THE EXACT JSON VERSION WE SET IN SUNNYPILOT_MODELS REPO
+REQUIRED_JSON_VERSION = 15
 
 CUSTOM_MODEL_PATH = Paths.model_root()
 METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
-
 ModelManager = custom.ModelManagerSP
+_LAST_VALIDATED_RAW = None
+
+
+def _compute_hash(file_path: str) -> str | None:
+  from openpilot.common.file_chunker import read_file_chunked
+  try:
+    return hashlib.sha256(read_file_chunked(file_path)).hexdigest().lower()
+  except FileNotFoundError:
+    return None
 
 
 async def verify_file(file_path: str, expected_hash: str) -> bool:
-  from openpilot.common.file_chunker import read_file_chunked
-  try:
-    data = read_file_chunked(file_path)
-  except FileNotFoundError:
-    return False
-  return hashlib.sha256(data).hexdigest().lower() == expected_hash.lower()
+  file_hash = _compute_hash(file_path)
+  return file_hash == expected_hash.lower() if file_hash else False
+
+
+def _verify_file(file_path: str, expected_hash: str) -> bool:
+  file_hash = _compute_hash(file_path)
+  return file_hash == expected_hash.lower() if file_hash else False
 
 
 def is_bundle_version_compatible(bundle: dict) -> bool:
   """
-  Checks whether the model bundle is compatible with the current selector version constraints.
-
-  The bundle specifies a `minimum_selector_version`, which defines the minimum selector version
+  The bundle parsed from the json specifies a `minimum_selector_version`, which defines the minimum selector version
   required to load the model. This function ensures that:
-
-    1. The model is not too old: the bundle must require at least `REQUIRED_MIN_SELECTOR_VERSION`.
-    2. The model is not too new: it must support the current selector version (`CURRENT_SELECTOR_VERSION`).
-
-  This allows the selector to enforce both a minimum and maximum range of supported models,
-  even if a model would otherwise be compatible.
-
-  :param bundle: Dictionary containing `minimum_selector_version`, as defined by the model bundle.
-  :type bundle: Dict
-  :return: True if the selector version is within the accepted range for the bundle; otherwise False.
-  :rtype: Bool
+    the bundle MUST match the `REQUIRED_JSON_VERSION` set here in helpers.
   """
-  return bool(REQUIRED_MIN_SELECTOR_VERSION <= bundle.get("minimumSelectorVersion", 0) <= CURRENT_SELECTOR_VERSION)
+  return bundle.get("minimumSelectorVersion", 0) == REQUIRED_JSON_VERSION
 
 
-def get_active_bundle(params: Params = None) -> custom.ModelManagerSP.ModelBundle:
-  """Gets the active model bundle from cache"""
-  if params is None:
-    params = Params()
+def _bundle_artifacts(bundle: custom.ModelManagerSP.ModelBundle) -> list[tuple[str, str]]:
+  artifacts = []
+  for model in getattr(bundle, 'models', []) or []:
+    for artifact in (getattr(model, 'artifact', None), getattr(model, 'metadata', None)):
+      if artifact and getattr(artifact, 'fileName', None) and getattr(artifact, 'downloadUri', None):
+        sha256 = getattr(artifact.downloadUri, 'sha256', None)
+        if sha256:
+          artifacts.append((artifact.fileName, sha256))
+  return artifacts
 
+
+def _bundle_is_valid_locally(bundle: custom.ModelManagerSP.ModelBundle) -> bool:
+  model_root = Paths.model_root()
+  return all(_verify_file(os.path.join(model_root, file_name), expected_hash)
+             for file_name, expected_hash in _bundle_artifacts(bundle))
+
+
+def _bundle_needs_reset(active_bundle: custom.ModelManagerSP.ModelBundle, available_bundles: list[custom.ModelManagerSP.ModelBundle] | None) -> bool:
+  if active_bundle is None:
+    return False
+
+  if available_bundles is not None:
+    matching_bundle = None
+    for bundle in available_bundles:
+      if getattr(active_bundle, 'ref', None) and getattr(bundle, 'ref', None):
+        if active_bundle.ref == bundle.ref:
+          matching_bundle = bundle
+          break
+      elif getattr(active_bundle, 'internalName', None) == getattr(bundle, 'internalName', None):
+        matching_bundle = bundle
+        break
+
+    if matching_bundle is None:
+      return True
+    if active_bundle.minimumSelectorVersion != matching_bundle.minimumSelectorVersion:
+      return True
+
+    active_runner = getattr(active_bundle, 'runner', None)
+    matching_runner = getattr(matching_bundle, 'runner', None)
+    if active_runner is not None and matching_runner is not None:
+      if getattr(active_runner, 'raw', active_runner) != getattr(matching_runner, 'raw', matching_runner):
+        return True
+    if set(_bundle_artifacts(active_bundle)) != set(_bundle_artifacts(matching_bundle)):
+      return True
+
+  return not _bundle_is_valid_locally(active_bundle)
+
+
+def validate_active_bundle(params: Params, available_bundles: list[custom.ModelManagerSP.ModelBundle] | None = None) -> None:
+  global _LAST_VALIDATED_RAW
+
+  raw_bundle = params.get("ModelManager_ActiveBundle")
+  if not raw_bundle:
+    return
+
+  if raw_bundle == _LAST_VALIDATED_RAW:
+    return
+
+  active_bundle = get_active_bundle(params, raw_bundle_dict=raw_bundle)
+  if active_bundle is None or _bundle_needs_reset(active_bundle, available_bundles):
+    cloudlog.warning("Active model bundle invalid; resetting to default")
+    params.remove("ModelManager_ActiveBundle")
+    params.put("ModelRunnerTypeCache", int(custom.ModelManagerSP.Runner.stock), block=True)
+    _LAST_VALIDATED_RAW = None
+  else:
+    _LAST_VALIDATED_RAW = raw_bundle
+
+
+def get_active_bundle(params: Params | None = None, raw_bundle_dict: dict | bytes | None = None) -> "custom.ModelManagerSP.ModelBundle | None":
+  params = params or Params()
   try:
-    if (active_bundle := params.get("ModelManager_ActiveBundle") or {}) and is_bundle_version_compatible(active_bundle):
-      return custom.ModelManagerSP.ModelBundle(**active_bundle)
+    active_bundle_dict = raw_bundle_dict if raw_bundle_dict is not None else (params.get("ModelManager_ActiveBundle") or {})
+    if active_bundle_dict and is_bundle_version_compatible(active_bundle_dict):
+      return custom.ModelManagerSP.ModelBundle(**active_bundle_dict)
   except Exception:
     pass
-
   return None
 
 
-def get_active_model_runner(params: Params = None, force_check=False) -> int:
-  if params is None:
-    params = Params()
-
+def get_active_model_runner(params: Params | None = None, force_check: bool = False) -> int:
+  params = params or Params()
   cached_runner_type = params.get("ModelRunnerTypeCache")
   if cached_runner_type is not None and not force_check:
     return cached_runner_type
-
   runner_type = custom.ModelManagerSP.Runner.stock
-
   if active_bundle := get_active_bundle(params):
     runner_type = active_bundle.runner.raw
 
@@ -88,66 +146,40 @@ def get_active_model_runner(params: Params = None, force_check=False) -> int:
 
   return runner_type
 
+
 def _get_model():
   if bundle := get_active_bundle():
     drive_model = next(model for model in bundle.models if model.type == ModelManager.Model.Type.supercombo)
     return drive_model
-
   return None
 
-def load_metadata():
-  metadata_path = METADATA_PATH
 
-  if model := _get_model():
-    metadata_path = f"{CUSTOM_MODEL_PATH}/{model.metadata.fileName}"
+def load_metadata():
+  model = _get_model()
+  metadata_path = f"{CUSTOM_MODEL_PATH}/{model.metadata.fileName}" if model else METADATA_PATH
 
   with open(metadata_path, 'rb') as f:
     return pickle.load(f)
 
 
-def prepare_inputs(model_metadata) -> dict[str, np.ndarray]:
-  # img buffers are managed in openCL transform code so we don't pass them as inputs
-  inputs = {
-    k: np.zeros(v, dtype=np.float32).flatten()
-    for k, v in model_metadata['input_shapes'].items()
-    if 'img' not in k
+def prepare_inputs(model_metadata: dict) -> dict[str, np.ndarray]:
+  return {
+    key: np.zeros(shape, dtype=np.float32).flatten()
+    for key, shape in model_metadata['input_shapes'].items()
+    if 'img' not in key
   }
 
-  return inputs
 
+def load_meta_constants(model_metadata: dict):
+  """ Loads the appropriate meta model class based on key shapes"""
+  if 'sim_pose' in model_metadata['input_shapes']:
+    return MetaSimPose
 
-def load_meta_constants(model_metadata):
-  """
-  Determines and loads the appropriate meta model class based on the metadata provided. The function checks
-  specific keys and conditions within the provided metadata dictionary to identify the corresponding meta
-  model class to return.
+  meta_slice = model_metadata['output_slices']['meta']
+  if (meta_slice.start, meta_slice.stop, meta_slice.step) == (5868, 5921, None):
+    return MetaTombRaider
 
-  :param model_metadata: Dictionary containing metadata about the model. It includes
-      details such as input shapes, output slices, and other configurations for identifying
-      metadata-dependent meta model classes.
-  :type model_metadata: dict
-  :return: The appropriate meta model class (Meta, MetaSimPose, or MetaTombRaider)
-      based on the conditions and metadata provided.
-  :rtype: type
-  """
-  meta = Meta  # Default Meta
-
-  if 'sim_pose' in model_metadata['input_shapes'].keys():
-    # Meta for models with sim_pose input
-    meta = MetaSimPose
-  else:
-    # Meta for Tomb Raider, it does not include sim_pose input but has the same meta slice as previous models
-    meta_slice = model_metadata['output_slices']['meta']
-    meta_tf_slice = slice(5868, 5921, None)
-
-    if (
-            meta_slice.start == meta_tf_slice.start and
-            meta_slice.stop == meta_tf_slice.stop and
-            meta_slice.step == meta_tf_slice.step
-    ):
-      meta = MetaTombRaider
-
-  return meta
+  return Meta
 
 
 # The following method(s) are modeld helper methods

sunnypilot/models/manager.py

@@ -17,7 +17,7 @@ from openpilot.system.hardware.hw import Paths
 
 from cereal import messaging, custom
 from openpilot.sunnypilot.models.fetcher import ModelFetcher
-from openpilot.sunnypilot.models.helpers import verify_file, get_active_bundle
+from openpilot.sunnypilot.models.helpers import get_active_bundle, validate_active_bundle, verify_file
 
 
 class ModelManagerSP:
@@ -239,6 +239,7 @@ class ModelManagerSP:
     while True:
       try:
         self.available_models = self.model_fetcher.get_available_bundles()
+        validate_active_bundle(self.params, self.available_models)
         self.active_bundle = get_active_bundle(self.params)
 
         if (index_to_download := self.params.get("ModelManager_DownloadIndex")) is not None:
@@ -252,8 +253,8 @@ class ModelManagerSP:
               self.selected_bundle = None
 
         if self.params.get("ModelManager_ClearCache"):
-            self.clear_model_cache()
-            self.params.remove("ModelManager_ClearCache")
+          self.clear_model_cache()
+          self.params.remove("ModelManager_ClearCache")
 
         self._report_status()
         rk.keep_time()

sunnypilot/selfdrive/car/interfaces.py

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

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

@@ -1,8 +0,0 @@
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
-  AccelController,
-  AccelControllerResult,
-  AccelControllerState,
-)
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import AccelProfile
-
-__all__ = ["AccelController", "AccelControllerResult", "AccelControllerState", "AccelProfile"]

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

@@ -1,683 +0,0 @@
-#!/usr/bin/env python3
-from collections import deque
-from dataclasses import dataclass, field
-from enum import IntEnum
-import math
-
-import numpy as np
-
-from cereal import log
-from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
-  LongitudinalMpc, LongitudinalPlanSource, 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_personality.constants import (
-  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, BRAKING_ACCEL_LIMIT_THRESHOLD, CAP_FILTER_FRAMES,
-  LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW,
-  LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_TAPER_GAIN, MATCHED_PACE_DECEL_RATE, MAX_LEAD_ACCEL_TAU,
-  MIN_LEAD_SPEED, PACE_RELIEF_DEADBAND, PACE_TARGET_ARM_MARGIN, PACE_TARGET_RESERVE,
-  PACE_RESTRICT_DEADBAND, PROFILE_CONFIGS, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
-  STOP_GAP_RESERVE_LEAD_SPEED,
-  STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED,
-  STOP_HOLD_MAX_LEAD_DISTANCE, STOPPED_LEAD_SPEED, VEGO_NOISE_TOLERANCE, AccelProfile,
-)
-
-
-class AccelControllerState(IntEnum):
-  inactive = 0
-  free = 1
-  restrict = 2
-  hold = 3
-  release = 4
-  stopHold = 5
-
-
-@dataclass(frozen=True)
-class EnergyEnvelope:
-  cap: 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_index: int = -1
-  departure_lead_speed: float = math.inf
-  departure_cap: float = math.inf
-  departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf)
-  usable_gap: float = math.inf
-  closing_speed: float = 0.0
-  required_decel: float = 0.0
-  has_nearly_stopped_lead: bool = False
-  lead_status: bool = False
-
-
-@dataclass(frozen=True)
-class AccelControllerResult:
-  target_speed: float
-  enabled: bool
-  active: bool
-  shadow_active: bool
-  launching: bool
-  departure_launching: bool
-  profile: AccelProfile
-  profile_accel_max: float
-  positive_accel_max: float
-  effective_accel_max: float
-  mpc_accel_max: tuple[float, ...] | None
-  state: AccelControllerState
-  shadow_state: AccelControllerState
-  base_speed: float
-  raw_energy_cap: float
-  live_filtered_cap: float
-  shadow_filtered_cap: float
-  selected_lead: int
-  selected_lead_speed: float
-  usable_gap: float
-  closing_speed: float
-  required_decel: float
-
-
-@dataclass
-class _ControllerPath:
-  cap_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
-  lead_speed_samples: deque[float] = field(default_factory=lambda: deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
-  lead_accel_samples: deque[float] = field(default_factory=lambda: deque([0.0] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES))
-  departure_samples: tuple[deque[float], deque[float]] = field(
-    default_factory=lambda: (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES)),
-  )
-  departure_references: list[float | None] = field(default_factory=lambda: [None, None])
-  pace: float | None = None
-  state: AccelControllerState = AccelControllerState.inactive
-  departure_frames: int = 0
-  active_frames: int = 0
-  lead_loss_frames: int = 0
-  lead_switch_guard_frames: int = 0
-  selected_lead: int = -1
-  selected_lead_track_id: int = -1
-  stale_frames: int = 0
-  launching: bool = False
-  departure_launch: bool = False
-  matched_lead: bool = False
-  braking_limited: bool = False
-  braking_handoff: bool = False
-  pace_reserve_armed: bool = False
-  matched_accel_limit: float | None = None
-
-  @property
-  def filtered_cap(self) -> float:
-    return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
-
-  @property
-  def filtered_lead_speed(self) -> float:
-    return sorted(self.lead_speed_samples)[CAP_FILTER_FRAMES // 2]
-
-  @property
-  def filtered_lead_accel(self) -> float:
-    return sorted(self.lead_accel_samples)[CAP_FILTER_FRAMES // 2]
-
-  def robust_departure_separation(self, lead_index: int) -> float:
-    samples = self.departure_samples[lead_index]
-    return float(np.median(samples)) if samples else -math.inf
-
-  def reset(self) -> None:
-    self.cap_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
-    self.lead_speed_samples = deque([math.inf] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
-    self.lead_accel_samples = deque([0.0] * CAP_FILTER_FRAMES, maxlen=CAP_FILTER_FRAMES)
-    self.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
-    self.departure_references = [None, None]
-    self.pace = None
-    self.state = AccelControllerState.inactive
-    self.departure_frames = 0
-    self.active_frames = 0
-    self.lead_loss_frames = 0
-    self.lead_switch_guard_frames = 0
-    self.selected_lead = -1
-    self.selected_lead_track_id = -1
-    self.stale_frames = 0
-    self.launching = False
-    self.departure_launch = False
-    self.matched_lead = False
-    self.braking_limited = False
-    self.braking_handoff = False
-    self.pace_reserve_armed = False
-    self.matched_accel_limit = None
-
-
-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.CP = CP
-    self.dt = dt
-    self.lead_loss_hold_frames = max(CAP_FILTER_FRAMES, math.ceil(LEAD_LOSS_HOLD_TIME / dt))
-    self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
-    self.live = _ControllerPath()
-    self.shadow = _ControllerPath()
-    self._held_envelope: EnergyEnvelope | None = None
-
-  @staticmethod
-  def _profile(profile: int | AccelProfile) -> AccelProfile:
-    try:
-      return AccelProfile(profile)
-    except (TypeError, ValueError):
-      return AccelProfile.normal
-
-  @classmethod
-  def get_profile_accel_max(cls, profile: int | AccelProfile, v_ego: float) -> float:
-    if not math.isfinite(v_ego):
-      return math.nan
-    selected_profile = cls._profile(profile)
-    return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[selected_profile]))
-
-  def _delay(self) -> float:
-    try:
-      return float(self.CP.longitudinalActuatorDelay) + DT_MDL
-    except (AttributeError, OverflowError, TypeError, ValueError):
-      return math.nan
-
-  @staticmethod
-  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)
-
-  @staticmethod
-  def _lead_values(lead) -> tuple[float, float, float, float] | None:
-    try:
-      if not lead.status:
-        return None
-      d_rel, v_lead = float(lead.dRel), float(lead.vLeadK)
-    except (AttributeError, OverflowError, TypeError, ValueError):
-      return None
-    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
-
-    try:
-      a_lead = float(lead.aLeadK)
-    except (AttributeError, OverflowError, TypeError, ValueError):
-      a_lead = 0.0
-    if not math.isfinite(a_lead):
-      a_lead = 0.0
-
-    try:
-      a_lead_tau = float(lead.aLeadTau)
-    except (AttributeError, OverflowError, TypeError, ValueError):
-      a_lead_tau = _LEAD_ACCEL_TAU
-    if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
-      a_lead_tau = _LEAD_ACCEL_TAU
-    return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
-
-  @staticmethod
-  def _lead_track_id(lead) -> int:
-    try:
-      return max(int(lead.radarTrackId), -1)
-    except (AttributeError, OverflowError, TypeError, ValueError):
-      return -1
-
-  def calculate_energy_envelope(self, radar_state, v_ego: float, a_ego: float, profile: int | AccelProfile,
-                                follow_personality=log.LongitudinalPersonality.standard) -> EnergyEnvelope:
-    delay = self._delay()
-    if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
-      return EnergyEnvelope()
-
-    try:
-      leads = (radar_state.leadOne, radar_state.leadTwo)
-      lead_status = any(bool(lead.status) for lead in leads)
-    except (AttributeError, TypeError, ValueError):
-      return EnergyEnvelope()
-
-    try:
-      t_follow = get_T_FOLLOW(follow_personality)
-    except (NotImplementedError, TypeError, ValueError):
-      t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard)
-    if not math.isfinite(t_follow) or t_follow < 0.0:
-      return EnergyEnvelope(lead_status=lead_status)
-
-    x_ego, v_ego_delay = self._project_ego(v_ego, a_ego, delay)
-    comfort_decel = PROFILE_CONFIGS[self._profile(profile)].comfort_decel
-    candidates: list[EnergyEnvelope] = []
-    departure_candidates: list[tuple[float, int]] = []
-    departure_speeds = [math.inf, math.inf]
-    departure_separations = [-math.inf, -math.inf]
-    departure_caps = [math.inf, math.inf]
-
-    for lead_index, lead in enumerate(leads):
-      values = self._lead_values(lead)
-      if values is None:
-        continue
-      try:
-        d_rel, 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)
-        reserve = float(np.interp(v_lead_delay, (0.0, STOP_GAP_RESERVE_LEAD_SPEED), (STOP_GAP_RESERVE, 0.0)))
-        reserve_scale = float(np.interp(required_decel, STOP_GAP_RESERVE_DECEL_BP, (1.0, 0.0)))
-        usable_gap = max(safety_gap - reserve * reserve_scale, 0.0)
-        cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
-        departure_cap = 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))
-      except (FloatingPointError, OverflowError, TypeError, ValueError):
-        continue
-
-      finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, cap, departure_cap, 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:
-        continue
-      if not math.isfinite(separation):
-        continue
-
-      candidates.append(EnergyEnvelope(
-        cap=cap, selected_lead=lead_index, selected_lead_track_id=self._lead_track_id(lead),
-        selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead,
-        usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, lead_status=lead_status,
-      ))
-      departure_candidates.append((departure_distance, lead_index))
-      departure_speeds[lead_index] = v_lead_delay
-      departure_separations[lead_index] = separation
-      departure_caps[lead_index] = departure_cap
-
-    if not candidates:
-      return EnergyEnvelope(lead_status=lead_status)
-
-    selected = min(candidates, key=lambda candidate: candidate.cap)
-    departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1]
-    departure_lead_speed = departure_speeds[departure_lead_index]
-    return EnergyEnvelope(
-      cap=selected.cap, selected_lead=selected.selected_lead, selected_lead_track_id=selected.selected_lead_track_id,
-      selected_lead_speed=selected.selected_lead_speed,
-      selected_lead_accel=selected.selected_lead_accel,
-      departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed,
-      departure_cap=departure_caps[departure_lead_index], departure_lead_separations=tuple(departure_separations),
-      usable_gap=selected.usable_gap, closing_speed=selected.closing_speed, required_decel=selected.required_decel,
-      has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED, lead_status=lead_status,
-    )
-
-  @staticmethod
-  def _move(value: float, target: float, rate: float, dt: float) -> float:
-    return float(np.clip(target, value - rate * dt, value + rate * dt))
-
-  @staticmethod
-  def _lead_source(source) -> bool:
-    return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
-
-  @staticmethod
-  def _update_samples(path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
-    had_filtered_lead = math.isfinite(path.filtered_cap)
-    has_lead = envelope.selected_lead >= 0
-    path.cap_samples.append(envelope.cap if has_lead else math.inf)
-    path.lead_speed_samples.append(envelope.selected_lead_speed if has_lead else math.inf)
-    path.lead_accel_samples.append(envelope.selected_lead_accel if has_lead else 0.0)
-    path.lead_loss_frames = 0 if has_lead else path.lead_loss_frames + 1
-    for lead_index, separation in enumerate(envelope.departure_lead_separations):
-      if math.isfinite(separation):
-        path.departure_samples[lead_index].append(separation)
-    return not had_filtered_lead and math.isfinite(path.filtered_cap)
-
-  @staticmethod
-  def _seed_departure_tracking(path: _ControllerPath, envelope: EnergyEnvelope) -> None:
-    path.departure_samples = (deque(maxlen=CAP_FILTER_FRAMES), deque(maxlen=CAP_FILTER_FRAMES))
-    path.departure_references = [None, None]
-    for lead_index, separation in enumerate(envelope.departure_lead_separations):
-      if math.isfinite(separation):
-        path.departure_samples[lead_index].append(separation)
-        path.departure_references[lead_index] = separation
-    path.departure_frames = 0
-
-  @staticmethod
-  def _creep_departure(path: _ControllerPath, envelope: EnergyEnvelope) -> bool:
-    lead_index = envelope.departure_lead_index
-    if lead_index < 0 or envelope.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
-      return False
-    reference = path.departure_references[lead_index]
-    separation = path.robust_departure_separation(lead_index)
-    return reference is not None and separation - reference >= STOP_HOLD_CREEP_DISTANCE
-
-  def _enter_stop_hold(self, path: _ControllerPath, envelope: EnergyEnvelope) -> None:
-    if path.state != AccelControllerState.stopHold:
-      self._seed_departure_tracking(path, envelope)
-    path.pace = 0.0
-    path.state = AccelControllerState.stopHold
-    path.departure_frames = 0
-    path.launching = False
-    path.departure_launch = False
-    path.matched_lead = False
-    path.pace_reserve_armed = False
-    path.matched_accel_limit = None
-
-  def _update_path(self, path: _ControllerPath, envelope: EnergyEnvelope, base_speed: float, v_ego: float,
-                   profile: AccelProfile, profile_accel_max: float, previous_should_stop: bool,
-                   previous_mpc_source, planner_speed: float, planner_accel: float) -> float:
-    confirmed_lead = self._update_samples(path, envelope)
-    path.active_frames += 1
-    has_lead = envelope.selected_lead >= 0
-    filtered_cap = path.filtered_cap
-    slot_changed = has_lead and path.selected_lead >= 0 and envelope.selected_lead != path.selected_lead
-    track_changed = (has_lead and path.selected_lead >= 0 and envelope.selected_lead == path.selected_lead
-                     and envelope.selected_lead_track_id != path.selected_lead_track_id
-                     and (path.selected_lead_track_id >= 0 or envelope.selected_lead_track_id >= 0))
-    false_relief = (has_lead and math.isfinite(filtered_cap)
-                    and envelope.cap >= filtered_cap + PACE_RELIEF_DEADBAND)
-    if (slot_changed or track_changed) and false_relief and path.lead_switch_guard_frames == 0 and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD:
-      path.lead_switch_guard_frames = self.lead_loss_hold_frames
-    elif path.lead_switch_guard_frames > 0:
-      path.lead_switch_guard_frames -= 1
-    if slot_changed or track_changed:
-      path.matched_lead = False
-      path.matched_accel_limit = None
-    if has_lead:
-      path.selected_lead = envelope.selected_lead
-      path.selected_lead_track_id = envelope.selected_lead_track_id
-    elif path.lead_loss_frames >= self.lead_loss_hold_frames:
-      path.lead_switch_guard_frames = 0
-      path.selected_lead = -1
-      path.selected_lead_track_id = -1
-    departure_separation = (envelope.departure_lead_separations[envelope.departure_lead_index]
-                            if envelope.departure_lead_index >= 0 else math.inf)
-    stopped_lead_hold = (has_lead and envelope.has_nearly_stopped_lead
-                         and (envelope.departure_cap < 0.50
-                              or (path.braking_limited and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE)))
-    invalid_lead = envelope.lead_status and not has_lead
-    prior_lead_context = self._lead_source(previous_mpc_source) or math.isfinite(filtered_cap) or path.braking_limited
-    previous_stop = (previous_should_stop and prior_lead_context
-                     and (not has_lead or envelope.departure_lead_speed < STOP_HOLD_EXIT_SPEED))
-    stop_evidence = (stopped_lead_hold or envelope.cap < 0.50 or filtered_cap < 0.50
-                     or (previous_stop and not path.launching) or invalid_lead)
-    confirmed_creep_departure = (path.launching and path.departure_launch and has_lead
-                                 and (envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
-                                      or self._creep_departure(path, envelope)))
-    if (path.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap)
-        and has_lead and planner_accel <= BRAKING_ACCEL_LIMIT_THRESHOLD):
-      path.braking_limited = True
-    elif not has_lead and path.lead_loss_frames >= self.lead_loss_hold_frames:
-      path.braking_limited = False
-
-    if path.pace is None:
-      e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
-      seed_from_ego = has_lead and planner_accel > BRAKING_ACCEL_LIMIT_THRESHOLD and not e2e_handoff
-      path.pace = min(base_speed, v_ego) if seed_from_ego else base_speed
-      path.braking_handoff = e2e_handoff and planner_accel < 0.0
-      path.state = AccelControllerState.free
-      if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence:
-        path.pace = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
-        path.state = AccelControllerState.release
-        path.launching = True
-        path.departure_launch = False
-    elif path.braking_handoff and planner_accel >= 0.0:
-      path.braking_handoff = False
-
-    path.pace = min(path.pace, base_speed)
-    if (v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not confirmed_creep_departure
-        and path.state != AccelControllerState.stopHold):
-      self._enter_stop_hold(path, envelope)
-      return path.pace
-
-    if path.state == AccelControllerState.stopHold:
-      for lead_index in range(len(path.departure_references)):
-        separation = path.robust_departure_separation(lead_index)
-        if math.isfinite(separation) and path.departure_references[lead_index] is None:
-          path.departure_references[lead_index] = separation
-      raw_departure = ((has_lead and envelope.departure_lead_speed > STOP_HOLD_CREEP_SPEED
-                        and envelope.departure_cap > STOP_HOLD_CREEP_SPEED)
-                       or (not envelope.lead_status and path.lead_loss_frames >= self.lead_loss_hold_frames))
-      departed = self._creep_departure(path, envelope) or raw_departure
-      path.departure_frames = path.departure_frames + 1 if departed else 0
-      path.pace = 0.0
-      if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
-        return path.pace
-      path.pace = base_speed
-      path.state = AccelControllerState.release
-      path.departure_frames = 0
-      path.launching = True
-      path.departure_launch = has_lead
-      return path.pace
-
-    if path.launching:
-      invalid_lead = envelope.lead_status and not has_lead
-      renewed_stop = (has_lead and not confirmed_creep_departure
-                      and (envelope.cap < STOP_HOLD_EXIT_SPEED
-                           or (envelope.has_nearly_stopped_lead and envelope.departure_cap < STOP_HOLD_EXIT_SPEED)))
-      guarded_departure_loss = path.departure_launch and not envelope.lead_status and path.lead_loss_frames < self.lead_loss_hold_frames
-      if invalid_lead:
-        path.launching = False
-        path.departure_launch = False
-        if v_ego < STOP_HOLD_EGO_SPEED:
-          self._enter_stop_hold(path, envelope)
-          return path.pace
-        path.state = AccelControllerState.hold
-        return path.pace
-      if guarded_departure_loss:
-        path.state = AccelControllerState.hold
-        return path.pace
-      if path.departure_launch and not has_lead:
-        path.departure_launch = False
-      if renewed_stop:
-        path.launching = False
-        path.departure_launch = False
-        if v_ego < STOP_HOLD_EGO_SPEED:
-          self._enter_stop_hold(path, envelope)
-          return path.pace
-      if path.departure_launch:
-        path.pace = base_speed
-      else:
-        launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
-        path.pace = min(base_speed, max(path.pace, launch_target) + LAUNCH_TARGET_SLEW * self.dt)
-      if v_ego >= LAUNCH_END_SPEED:
-        path.launching = False
-        path.departure_launch = False
-
-    comfort_decel = PROFILE_CONFIGS[profile].comfort_decel
-    if (has_lead and not path.launching and path.state == AccelControllerState.restrict
-        and envelope.closing_speed <= 0.0
-        and v_ego >= path.filtered_lead_speed - VEGO_NOISE_TOLERANCE):
-      path.matched_lead = True
-    elif not has_lead and path.lead_loss_frames >= self.lead_loss_hold_frames:
-      path.matched_lead = False
-
-    if path.matched_lead:
-      if not has_lead:
-        if self._lead_source(previous_mpc_source) and planner_speed < path.pace:
-          path.pace = max(planner_speed, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
-        path.state = AccelControllerState.hold
-        return path.pace
-      if math.isfinite(path.filtered_lead_speed):
-        recovery_speed = min(base_speed, path.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * envelope.usable_gap))
-        desired_accel_limit = min(profile_accel_max, LEAD_MATCH_TAPER_GAIN * max(recovery_speed - v_ego, 0.0))
-      else:
-        desired_accel_limit = 0.0
-      if path.filtered_lead_accel < BRAKING_ACCEL_LIMIT_THRESHOLD:
-        desired_accel_limit = profile_accel_max
-      if path.matched_accel_limit is None:
-        path.matched_accel_limit = profile_accel_max
-      if path.lead_switch_guard_frames > 0:
-        desired_accel_limit = min(desired_accel_limit, path.matched_accel_limit)
-      path.matched_accel_limit = min(profile_accel_max,
-                                     self._move(path.matched_accel_limit, desired_accel_limit, LEAD_MATCH_ACCEL_SLEW, self.dt))
-      matched_ceiling = min(base_speed, filtered_cap)
-      if matched_ceiling <= path.pace - PACE_RESTRICT_DEADBAND:
-        path.pace = max(matched_ceiling, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
-        path.state = AccelControllerState.restrict
-      elif path.lead_switch_guard_frames == 0 and matched_ceiling >= path.pace + PACE_RELIEF_DEADBAND:
-        path.pace = min(matched_ceiling, path.pace + profile_accel_max * self.dt)
-        path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
-      else:
-        path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
-      return path.pace
-    path.matched_accel_limit = None
-
-    ceiling = min(base_speed, filtered_cap)
-    if (confirmed_lead and path.active_frames == CAP_FILTER_FRAMES // 2 + 1 and not path.launching
-        and planner_speed < path.pace):
-      path.pace = max(planner_speed, path.pace - comfort_decel * self.dt)
-
-    if self._lead_source(previous_mpc_source) and not has_lead and planner_speed < path.pace:
-      path.pace = max(planner_speed, path.pace - MATCHED_PACE_DECEL_RATE * self.dt)
-      path.state = AccelControllerState.hold
-      return path.pace
-
-    if ceiling <= path.pace - PACE_RESTRICT_DEADBAND or (path.state == AccelControllerState.restrict and ceiling < path.pace):
-      path.pace = max(ceiling, path.pace - comfort_decel * self.dt)
-      path.state = AccelControllerState.restrict
-      return path.pace
-
-    filter_warmup = has_lead and not math.isfinite(filtered_cap)
-    guarded_lead_loss = not has_lead and path.lead_loss_frames < self.lead_loss_hold_frames
-    if (filter_warmup or guarded_lead_loss) and path.pace < base_speed - PACE_RESTRICT_DEADBAND:
-      path.state = AccelControllerState.hold
-      return path.pace
-
-    confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss
-    relief = not has_lead or envelope.closing_speed <= 0.0
-    if relief and (ceiling >= path.pace + PACE_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > path.pace)):
-      if path.lead_switch_guard_frames == 0:
-        path.pace = ceiling
-      path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.release
-    else:
-      path.state = AccelControllerState.free if path.pace >= base_speed - PACE_RESTRICT_DEADBAND else AccelControllerState.hold
-    return path.pace
-
-  @staticmethod
-  def _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 _update_freshness(self, path: _ControllerPath, radar_fresh: bool) -> bool:
-    if radar_fresh:
-      path.stale_frames = 0
-      return True
-    path.stale_frames += 1
-    if path.stale_frames >= self.radar_stale_frames:
-      path.reset()
-    return False
-
-  @staticmethod
-  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.maximum(limit, a0 - ACCEL_LIMIT_HORIZON_JERK * T_IDXS)
-    ceiling = np.clip(ceiling, 0.0, ACCEL_MAX)
-    ceiling[0] = max(ceiling[0], a0)
-    return tuple(float(value) for value in ceiling)
-
-  def reset(self) -> None:
-    self.live.reset()
-    self.shadow.reset()
-    self._held_envelope = None
-
-  def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, profile: int | AccelProfile, follow_personality,
-             enabled: bool, acc_selected: bool, engaged: bool, cruise_initialized: bool, stock_accel_max: float,
-             previous_should_stop: bool, radar_fresh: bool = True,
-             previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0) -> AccelControllerResult:
-    selected_profile = self._profile(profile)
-    sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
-    profile_accel_max = self.get_profile_accel_max(selected_profile, sanitized_v_ego)
-    try:
-      stock_accel_max = float(stock_accel_max)
-    except (OverflowError, TypeError, ValueError):
-      stock_accel_max = math.nan
-    positive_accel_max = (max(0.0, min(profile_accel_max, stock_accel_max, ACCEL_MAX))
-                          if math.isfinite(profile_accel_max) and math.isfinite(stock_accel_max) else math.nan)
-    planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
-    valid_context = self._valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self._delay(),
-                                        engaged, cruise_initialized)
-    if valid_context and radar_fresh:
-      envelope = self.calculate_energy_envelope(radar_state, sanitized_v_ego, a_ego, selected_profile, follow_personality)
-      self._held_envelope = envelope
-    elif valid_context and self._held_envelope is not None:
-      envelope = self._held_envelope
-    else:
-      envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state))
-      if not valid_context:
-        self._held_envelope = None
-
-    shadow_fresh = self._update_freshness(self.shadow, radar_fresh) if valid_context else False
-    if valid_context and radar_fresh:
-      self._update_path(self.shadow, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max, previous_should_stop,
-                        previous_mpc_source, planner_speed, planner_accel)
-      shadow_active = True
-    elif valid_context and not shadow_fresh and self.shadow.pace is not None:
-      shadow_active = True
-    else:
-      self.shadow.reset()
-      shadow_active = False
-
-    live_context = valid_context and bool(enabled) and bool(acc_selected)
-    live_fresh = self._update_freshness(self.live, radar_fresh) if live_context else False
-    if live_context and radar_fresh:
-      pace_target = self._update_path(self.live, envelope, base_speed, sanitized_v_ego, selected_profile, profile_accel_max,
-                                      previous_should_stop,
-                                      previous_mpc_source, planner_speed, planner_accel)
-      live_active = True
-    elif live_context and not live_fresh and self.live.pace is not None:
-      pace_target = self.live.pace
-      live_active = True
-    else:
-      self.live.reset()
-      pace_target = base_speed
-      live_active = False
-
-    if not radar_fresh and not shadow_active and not live_active:
-      self._held_envelope = None
-      envelope = EnergyEnvelope(lead_status=self._radar_has_lead(radar_state))
-
-    stop_hold_active = live_active and self.live.state == AccelControllerState.stopHold
-    matched_limit_active = (live_active and self.live.matched_lead and self.live.matched_accel_limit is not None
-                            and not self.live.braking_handoff)
-    lead_accel_request = (live_active and envelope.selected_lead >= 0
-                          and envelope.closing_speed <= 0.0 and planner_accel >= 0.0)
-    profile_limit_active = live_active and not stop_hold_active and (self.live.launching or not envelope.lead_status or lead_accel_request)
-    if matched_limit_active:
-      effective_accel_max = min(positive_accel_max, self.live.matched_accel_limit)
-    elif profile_limit_active:
-      effective_accel_max = positive_accel_max
-    else:
-      effective_accel_max = math.inf
-    if matched_limit_active or profile_limit_active:
-      mpc_accel_max = self._build_accel_ceiling(effective_accel_max, planner_accel)
-    else:
-      mpc_accel_max = None
-    guarded_lead_loss = (not envelope.lead_status and self.live.selected_lead >= 0
-                         and self.live.lead_loss_frames < self.lead_loss_hold_frames)
-    lead_context = envelope.lead_status or math.isfinite(self.live.filtered_cap) or guarded_lead_loss
-    reserve_eligible = (live_active and lead_context and not stop_hold_active and self.live.lead_switch_guard_frames == 0
-                        and not self.live.launching and not self.live.braking_handoff)
-    if not lead_context:
-      self.live.pace_reserve_armed = False
-    elif (reserve_eligible and not self.live.pace_reserve_armed and math.isfinite(self.live.filtered_cap)
-          and self.live.filtered_cap <= pace_target + PACE_TARGET_ARM_MARGIN):
-      self.live.pace_reserve_armed = True
-
-    target_speed = 0.0 if stop_hold_active else pace_target
-    if reserve_eligible and self.live.pace_reserve_armed:
-      target_speed = max(0.0, target_speed - PACE_TARGET_RESERVE)
-
-    return AccelControllerResult(
-      target_speed=target_speed,
-      enabled=bool(enabled), active=live_active, shadow_active=shadow_active, launching=live_active and self.live.launching,
-      departure_launching=live_active and self.live.launching and self.live.departure_launch,
-      profile=selected_profile, profile_accel_max=profile_accel_max if live_active else math.inf,
-      positive_accel_max=positive_accel_max if live_active else math.inf, effective_accel_max=effective_accel_max,
-      mpc_accel_max=mpc_accel_max, state=self.live.state,
-      shadow_state=self.shadow.state, base_speed=base_speed, raw_energy_cap=envelope.cap,
-      live_filtered_cap=self.live.filtered_cap if live_active else math.inf,
-      shadow_filtered_cap=self.shadow.filtered_cap if shadow_active else math.inf, selected_lead=envelope.selected_lead,
-      selected_lead_speed=envelope.selected_lead_speed, usable_gap=envelope.usable_gap,
-      closing_speed=envelope.closing_speed, required_decel=envelope.required_decel,
-    )
-
-  @staticmethod
-  def _radar_has_lead(radar_state) -> bool:
-    try:
-      return bool(radar_state.leadOne.status or radar_state.leadTwo.status)
-    except (AttributeError, TypeError, ValueError):
-      return True

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

@@ -1,63 +0,0 @@
-from dataclasses import dataclass
-from enum import IntEnum
-
-
-class AccelProfile(IntEnum):
-  eco = 0
-  normal = 1
-  sport = 2
-
-
-@dataclass(frozen=True)
-class ProfileConfig:
-  comfort_decel: float
-
-
-PROFILE_CONFIGS = {
-  AccelProfile.eco: ProfileConfig(comfort_decel=0.25),
-  AccelProfile.normal: ProfileConfig(comfort_decel=0.30),
-  AccelProfile.sport: ProfileConfig(comfort_decel=0.35),
-}
-
-ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
-ACCEL_PROFILE_MAX_V = {
-  AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16],
-  AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
-  AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
-}
-
-CAP_FILTER_FRAMES = 5
-LEAD_LOSS_HOLD_TIME = 0.50
-PACE_RESTRICT_DEADBAND = 0.15
-PACE_RELIEF_DEADBAND = 0.35
-PACE_TARGET_ARM_MARGIN = 1.0
-PACE_TARGET_RESERVE = 0.10
-LAUNCH_TARGET_HEADROOM = 3.0
-LAUNCH_TARGET_SLEW = 8.75
-LAUNCH_END_SPEED = 3.0
-ACCEL_LIMIT_HORIZON_JERK = 1.0
-LEAD_MATCH_GAP_GAIN = 0.04
-LEAD_MATCH_SPEED_HEADROOM = 1.25
-LEAD_MATCH_TAPER_GAIN = 1.00
-LEAD_MATCH_ACCEL_SLEW = 0.25
-MATCHED_PACE_DECEL_RATE = 0.50
-BRAKING_ACCEL_LIMIT_THRESHOLD = -0.11
-MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
-MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
-MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
-
-STOP_HOLD_EGO_SPEED = 0.30
-STOPPED_LEAD_SPEED = 0.30
-STOP_HOLD_EXIT_SPEED = 0.80
-STOP_HOLD_EXIT_FRAMES = 4
-STOP_HOLD_CREEP_SPEED = 0.15
-STOP_HOLD_CREEP_DISTANCE = 0.30
-STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
-STOP_GAP_RESERVE = 0.75
-STOP_GAP_RESERVE_LEAD_SPEED = 2.0
-STOP_GAP_RESERVE_DECEL_BP = (0.30, 0.80)
-
-RADAR_STALE_TIMEOUT = 0.50
-MAX_LEAD_ACCEL_TAU = 10.0
-MIN_LEAD_SPEED = -1.0
-VEGO_NOISE_TOLERANCE = 0.10

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -1,687 +0,0 @@
-import math
-from types import SimpleNamespace
-
-import numpy as np
-import pytest
-
-from 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_personality import AccelController, AccelControllerState, AccelProfile
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
-  ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, CAP_FILTER_FRAMES, LAUNCH_END_SPEED,
-  LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_PACE_DECEL_RATE, PROFILE_CONFIGS,
-  PACE_TARGET_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES,
-)
-
-
-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(status=status, dRel=d_rel, 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))
-
-
-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,
-    "previous_should_stop": False,
-  }
-  args.update(overrides)
-  return controller.update(radar_state or make_radar(), **args)
-
-
-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.1):
-  stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
-  return update(controller, stopped, base_speed=base_speed, v_ego=v_ego, previous_should_stop=True)
-
-
-class TestProfiles:
-  def test_lookup_table_is_explicit_and_tunable(self):
-    assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0]
-    assert ACCEL_PROFILE_MAX_V == {
-      AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16],
-      AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
-      AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
-    }
-
-  @pytest.mark.parametrize("profile", list(AccelProfile))
-  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 AccelController.get_profile_accel_max(profile, speed) == expected
-
-    limits = [AccelController.get_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 = [AccelController.get_profile_accel_max(profile, speed) for profile in AccelProfile]
-    assert eco < normal < sport
-
-  def test_invalid_profile_defaults_to_normal(self):
-    assert update(make_controller(), profile=999).profile == 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_loss_hold_frames)]
-    result = results[-1]
-    assert result.profile_accel_max == pytest.approx(1.15)
-    assert result.positive_accel_max == pytest.approx(0.30)
-    assert result.effective_accel_max == 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_loss_hold_frames)][-1]
-    eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20)
-
-    assert sport.effective_accel_max == pytest.approx(1.15)
-    assert eco.effective_accel_max == pytest.approx(0.72)
-
-  def test_matched_lead_waits_until_ego_catches_the_lead(self):
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    slow_controller, caught_controller = make_controller(), make_controller()
-    for controller in (slow_controller, caught_controller):
-      for _ in range(CAP_FILTER_FRAMES + 10):
-        update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-
-    update(slow_controller, radar, v_ego=3.0, planner_accel=-0.2)
-    update(caught_controller, radar, v_ego=8.0, planner_accel=-0.2)
-
-    assert not slow_controller.live.matched_lead
-    assert caught_controller.live.matched_lead
-
-  def test_stock_limit_reduction_applies_immediately(self):
-    controller = make_controller()
-    for _ in range(controller.lead_loss_hold_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 reduced.effective_accel_max == 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_loss_hold_frames + 10):
-      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)
-
-    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 limited.effective_accel_max == 0.0
-    assert recovered.effective_accel_max == pytest.approx(clean.effective_accel_max)
-
-  @pytest.mark.parametrize("radar_fresh", (True, False), ids=("dropout", "stale"))
-  def test_matched_lead_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(CAP_FILTER_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.live.matched_lead
-
-    limited = update(controller, stock_accel_max=0.0, radar_fresh=radar_fresh)
-    assert limited.effective_accel_max == 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 result.positive_accel_max == ACCEL_MAX
-    assert result.mpc_accel_max is None
-
-
-class TestMpcCeiling:
-  @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(AccelController._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(AccelController._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)
-
-  def test_inactive_controller_has_no_custom_ceiling(self):
-    result = update(make_controller(), enabled=False)
-    assert not result.active
-    assert result.mpc_accel_max is None
-    assert math.isinf(result.effective_accel_max)
-
-  def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self):
-    controller = make_controller()
-    radar = restrictive_radar()
-    warmup = [update(controller, radar, planner_accel=-0.2) for _ in range(controller.lead_loss_hold_frames)]
-
-    assert all(sample.mpc_accel_max is None for sample in warmup)
-    assert controller.live.braking_limited
-
-    bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False)
-    assert not bypassed.active and bypassed.mpc_accel_max is None
-    assert not controller.live.braking_limited
-
-  def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self):
-    controller = make_controller()
-    for _ in range(CAP_FILTER_FRAMES + 5):
-      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.effective_accel_max == pytest.approx(result.positive_accel_max)
-    assert result.mpc_accel_max is not None
-
-  def test_matched_lead_terminal_taper_changes_smoothly(self):
-    controller = make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    for _ in range(CAP_FILTER_FRAMES + 5):
-      update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-
-    braking = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    braking_limit = controller.live.matched_accel_limit
-    accelerating = update(controller, radar, v_ego=8.0, planner_accel=0.2)
-
-    assert controller.live.matched_lead
-    assert braking.mpc_accel_max is not None and accelerating.mpc_accel_max is not None
-    assert braking_limit is not None
-    assert abs(controller.live.matched_accel_limit - braking_limit) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9
-    assert braking.effective_accel_max <= braking.positive_accel_max
-    assert accelerating.effective_accel_max <= accelerating.positive_accel_max
-
-  def test_matched_lead_ignores_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(CAP_FILTER_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 noisy.effective_accel_max == pytest.approx(clean.effective_accel_max)
-      assert noisy.target_speed == pytest.approx(clean.target_speed)
-
-  def test_matched_lead_ignores_two_frame_acceleration_jump(self):
-    clean_controller, noisy_controller = make_controller(), make_controller()
-    steady = 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(CAP_FILTER_FRAMES + 10):
-        update(controller, steady, v_ego=10.0, planner_accel=-0.2)
-      for _ in range(20):
-        update(controller, steady, v_ego=8.0, planner_accel=-0.2)
-
-    braking_jump = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-1.0))
-    for _ in range(2):
-      clean = update(clean_controller, steady, v_ego=8.0)
-      noisy = update(noisy_controller, braking_jump, v_ego=8.0)
-      assert noisy.effective_accel_max == pytest.approx(clean.effective_accel_max)
-      assert noisy.target_speed == pytest.approx(clean.target_speed)
-
-
-class TestEnergyEnvelope:
-  def test_relative_pace_energy_formula(self):
-    controller = make_controller()
-    lead = make_lead(status=True, d_rel=50.0, v_lead_k=8.0)
-    envelope = controller.calculate_energy_envelope(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, _ = controller._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)
-    expected = v_lead + math.sqrt(2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * safety_gap)
-
-    assert envelope.cap == pytest.approx(expected)
-    assert envelope.cap != pytest.approx(math.sqrt(v_lead**2 + 2.0 * PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * safety_gap))
-
-  def test_profile_order_controls_approach_timing(self):
-    radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0))
-    caps = [make_controller().calculate_energy_envelope(radar, 10.0, 0.0, profile).cap for profile in AccelProfile]
-    assert caps[0] < caps[1] < caps[2]
-
-  def test_stopped_lead_reserve_only_reduces_comfort_gap(self):
-    envelope = make_controller().calculate_energy_envelope(
-      make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0)), 5.0, 0.0, AccelProfile.normal,
-    )
-    comfort_decel = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel
-    safety_gap = (envelope.departure_cap - envelope.departure_lead_speed) ** 2 / (2.0 * comfort_decel)
-    assert envelope.required_decel < 0.30
-    assert safety_gap - envelope.usable_gap == pytest.approx(STOP_GAP_RESERVE)
-    assert envelope.departure_cap > envelope.cap
-
-  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 make_controller().calculate_energy_envelope(radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1
-
-  @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)
-    envelope = make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert envelope.selected_lead == 0
-    assert math.isfinite(envelope.cap)
-
-  @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)
-    envelope = make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert envelope.selected_lead == -1
-    assert envelope.lead_status
-    assert math.isinf(envelope.cap)
-
-  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()
-    make_controller().calculate_energy_envelope(make_radar(lead), 10.0, 0.0, AccelProfile.normal)
-    assert vars(lead) == before
-
-
-class TestPaceAndLifecycle:
-  def test_five_frame_median_needs_three_restrictive_samples(self):
-    controller = make_controller()
-    results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES)]
-    assert math.isinf(results[1].live_filtered_cap)
-    assert math.isfinite(results[2].live_filtered_cap)
-
-  def test_restriction_uses_comfort_rate_with_one_bounded_reserve_step(self):
-    controller = make_controller()
-    results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES + 10)]
-    targets = np.asarray([result.target_speed for result in results])
-    max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
-
-    target_steps = -np.diff(targets)
-    assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
-    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
-    assert results[-1].state == AccelControllerState.restrict
-    assert results[-1].target_speed < results[0].target_speed
-
-  @pytest.mark.parametrize("clear_frames", (1, 2, CAP_FILTER_FRAMES + 1))
-  def test_lead_acquired_after_clear_road_cannot_step_pace_to_planner(self, clear_frames):
-    controller = make_controller()
-    for _ in range(clear_frames):
-      update(controller, base_speed=25.0, v_ego=20.0, planner_speed=25.0)
-
-    results = [update(controller, restrictive_radar(), base_speed=25.0, v_ego=20.0, planner_speed=20.0)
-               for _ in range(CAP_FILTER_FRAMES)]
-    targets = np.asarray([25.0, *(result.target_speed for result in results)])
-    max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
-
-    target_steps = -np.diff(targets)
-    assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
-    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
-
-  def test_lead_slot_is_forgotten_before_reacquisition(self):
-    controller = make_controller()
-    lead_one = restrictive_radar()
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      update(controller, lead_one, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2)
-
-    for _ in range(controller.lead_loss_hold_frames):
-      before = update(controller, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2)
-    assert controller.live.selected_lead == -1
-
-    lead_two = make_radar(lead_two=make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
-    results = [update(controller, lead_two, base_speed=25.0, v_ego=20.0, planner_speed=5.0, planner_accel=-0.2)
-               for _ in range(CAP_FILTER_FRAMES)]
-    targets = np.asarray([before.target_speed, *(result.target_speed for result in results)])
-    max_step = PROFILE_CONFIGS[AccelProfile.normal].comfort_decel * DT_MDL
-
-    target_steps = -np.diff(targets)
-    assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
-    assert np.max(target_steps) <= PACE_TARGET_RESERVE + max_step + 1e-9
-
-  @pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
-  def test_false_relief_track_replacement_freezes_bounded_pace_release(self, replacement_track_id):
-    controller = make_controller()
-    original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100))
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
-    for _ in range(20):
-      before = update(controller, original, base_speed=25.0, v_ego=8.0, planner_speed=8.0, planner_accel=-0.2)
-    assert controller.live.matched_lead
-
-    replacement = make_radar(make_lead(status=True, d_rel=40.0, v_lead_k=12.0, radar_track_id=replacement_track_id))
-    switched = update(controller, replacement, base_speed=25.0, v_ego=8.0, planner_speed=5.0, planner_accel=-0.2)
-
-    target_drop = before.target_speed - switched.target_speed
-    assert -PACE_TARGET_RESERVE - 1e-9 <= target_drop <= MATCHED_PACE_DECEL_RATE * DT_MDL + 1e-9
-    assert switched.effective_accel_max <= switched.positive_accel_max + 1e-9
-    assert switched.target_speed < switched.base_speed
-    assert controller.live.lead_switch_guard_frames == controller.lead_loss_hold_frames
-
-  def test_track_id_churn_without_false_relief_does_not_arm_guard(self):
-    controller = make_controller()
-    original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100))
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
-
-    replacement = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=200))
-    update(controller, replacement, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
-
-    assert controller.live.lead_switch_guard_frames == 0
-
-  def test_short_dropout_holds_then_releases_without_a_second_accel_cap(self):
-    controller = make_controller()
-    for _ in range(CAP_FILTER_FRAMES + 20):
-      restricted = update(controller, restrictive_radar())
-
-    held = [update(controller) for _ in range(controller.lead_loss_hold_frames - 1)]
-    assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held)
-
-    released = update(controller)
-    assert released.target_speed == released.base_speed
-
-  def test_previous_lead_source_synchronizes_down_to_planner(self):
-    controller = make_controller()
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      restricted = update(controller, restrictive_radar())
-    planner_speed = restricted.target_speed - 2.0
-    synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
-    assert restricted.target_speed - synchronized.target_speed == pytest.approx(MATCHED_PACE_DECEL_RATE * DT_MDL)
-    assert synchronized.state == AccelControllerState.hold
-
-  def test_matched_lead_dropout_synchronizes_down_to_planner(self):
-    controller = make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-    for _ in range(20):
-      matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    assert controller.live.matched_lead
-
-    planner_speed = matched.target_speed - 2.0
-    synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
-    assert matched.target_speed - synchronized.target_speed == pytest.approx(MATCHED_PACE_DECEL_RATE * DT_MDL)
-    assert synchronized.state == AccelControllerState.hold
-
-  def test_reused_radar_holds_matched_lead_until_a_fresh_dropout(self):
-    controller = make_controller()
-    radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      update(controller, radar, v_ego=10.0, planner_accel=-0.2)
-    for _ in range(20):
-      matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
-    assert controller.live.matched_lead
-
-    planner_speed = matched.target_speed - 2.0
-    held = update(controller, radar, previous_mpc_source=LongitudinalPlanSource.lead0,
-                  planner_speed=planner_speed, radar_fresh=False)
-    synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
-
-    assert held.target_speed == pytest.approx(matched.target_speed)
-    assert held.state == matched.state
-    assert held.target_speed - synchronized.target_speed == pytest.approx(MATCHED_PACE_DECEL_RATE * DT_MDL)
-    assert synchronized.state == AccelControllerState.hold
-
-  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 + LAUNCH_TARGET_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) <= LAUNCH_TARGET_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_far_stopped_lead_does_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(result.state != AccelControllerState.stopHold for result in results)
-
-  def test_far_stopped_lead_does_not_use_sticky_braking_history_as_stop_evidence(self):
-    controller = make_controller()
-    far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0))
-    for _ in range(controller.lead_loss_hold_frames):
-      update(controller, far_stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2)
-    assert controller.live.braking_limited
-
-    result = update(controller, far_stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2)
-    assert result.state != AccelControllerState.stopHold
-    assert result.target_speed > 0.0
-
-  def test_near_stopped_lead_uses_braking_history_to_hold_completed_stop(self):
-    controller = make_controller()
-    stopped = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=0.0))
-    for _ in range(controller.lead_loss_hold_frames):
-      update(controller, stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2)
-    assert controller.live.braking_limited
-
-    result = update(controller, stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2)
-    assert result.state == AccelControllerState.stopHold
-    assert controller.live.pace == 0.0
-    assert result.target_speed == 0.0
-    assert math.isinf(result.effective_accel_max)
-    assert result.mpc_accel_max is None
-
-    stock_limited = update(controller, stopped, base_speed=12.0, v_ego=0.2, stock_accel_max=0.0)
-    assert math.isinf(stock_limited.effective_accel_max)
-    assert stock_limited.mpc_accel_max is None
-
-  def test_stop_hold_needs_four_confirmed_departure_frames(self):
-    controller = make_controller()
-    held = enter_stop_hold(controller)
-    assert controller.live.pace == 0.0
-    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
-    results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
-    launch_index = next(index for index, result in enumerate(results) if result.launching)
-
-    assert held.state == AccelControllerState.stopHold
-    assert held.target_speed == 0.0 and math.isinf(held.effective_accel_max)
-    assert held.mpc_accel_max is None
-    assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results[:launch_index])
-    assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
-    assert results[launch_index].target_speed >= 0.1 + LAUNCH_TARGET_HEADROOM
-    assert results[launch_index].departure_launching
-    assert results[launch_index].effective_accel_max == pytest.approx(results[launch_index].positive_accel_max)
-
-  def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
-
-    for frame in range(STOP_HOLD_EXIT_FRAMES):
-      fresh = update(controller, departing, base_speed=8.0, v_ego=0.1)
-      held = update(controller, departing, base_speed=8.0, v_ego=0.1, radar_fresh=False,
-                    previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=0.01)
-      assert held.target_speed == pytest.approx(fresh.target_speed)
-      assert held.state == fresh.state
-      assert held.selected_lead == fresh.selected_lead == 0
-      assert held.effective_accel_max == pytest.approx(fresh.effective_accel_max)
-      if frame < STOP_HOLD_EXIT_FRAMES - 1:
-        assert fresh.state == AccelControllerState.stopHold
-        assert math.isinf(fresh.effective_accel_max)
-        assert fresh.mpc_accel_max is None
-
-    assert fresh.launching and held.launching
-    assert fresh.departure_launching and held.departure_launching
-    assert fresh.target_speed == held.target_speed == 8.0
-    assert fresh.effective_accel_max == pytest.approx(fresh.positive_accel_max)
-
-  def test_single_frame_departure_stays_at_zero_target_without_an_accel_ceiling(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
-    stopped = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=0.0))
-
-    warm = update(controller, departing, base_speed=8.0, v_ego=0.0)
-    held = update(controller, stopped, base_speed=8.0, v_ego=0.0)
-
-    assert warm.state == held.state == AccelControllerState.stopHold
-    assert not warm.launching and not held.launching
-    assert math.isinf(warm.effective_accel_max) and warm.mpc_accel_max is None
-    assert math.isinf(held.effective_accel_max) and held.mpc_accel_max is None
-    assert held.target_speed == 0.0
-
-  def test_previous_stop_without_a_lead_does_not_latch_stop_hold(self):
-    result = update(
-      make_controller(), base_speed=8.0, v_ego=0.0, previous_should_stop=True,
-      previous_mpc_source=LongitudinalPlanSource.cruise,
-    )
-
-    assert result.state != AccelControllerState.stopHold
-    assert result.target_speed >= LAUNCH_TARGET_HEADROOM
-
-  def test_previous_lead_stop_survives_a_fresh_full_field_dropout(self):
-    result = update(
-      make_controller(), base_speed=8.0, v_ego=0.0, previous_should_stop=True,
-      previous_mpc_source=LongitudinalPlanSource.lead0,
-    )
-
-    assert result.state == AccelControllerState.stopHold
-    assert result.target_speed == 0.0
-    assert math.isinf(result.effective_accel_max)
-    assert result.mpc_accel_max is None
-
-  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.state == AccelControllerState.stopHold
-    assert missing.target_speed == 0.0
-    assert math.isinf(missing.effective_accel_max)
-    assert missing.mpc_accel_max is None
-
-  def test_confirmed_creep_departure_does_not_reenter_stop_hold(self):
-    controller = make_controller()
-    enter_stop_hold(controller, v_ego=0.0)
-    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 all(result.state != AccelControllerState.stopHold for result in results[launch_index:])
-    assert all(result.target_speed > 0.0 for result in results[launch_index:])
-
-  def test_departure_dropout_holds_without_resurrecting_stop_hold(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
-    results = [update(controller, departing, base_speed=8.0, v_ego=0.1) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
-    launched = next(result for result in results if result.launching)
-    before_dropout = results[-1]
-    dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_loss_hold_frames + 1)]
-
-    assert launched.launching
-    assert all(result.state != AccelControllerState.stopHold for result in dropout)
-    assert all(result.target_speed <= before_dropout.target_speed + 1e-9 for result in dropout[:controller.lead_loss_hold_frames - 1])
-    assert dropout[controller.lead_loss_hold_frames - 1].target_speed > before_dropout.target_speed
-    assert dropout[-1].launching
-
-  def test_invalid_departure_geometry_returns_to_stop_hold(self):
-    controller = make_controller()
-    enter_stop_hold(controller)
-    departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
-    for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES):
-      launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
-    invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0))
-    guarded = update(controller, invalid, base_speed=8.0, v_ego=0.1)
-    assert launched.launching
-    assert guarded.state == AccelControllerState.stopHold
-    assert guarded.target_speed == 0.0
-
-  def test_stale_timeout_fully_resets_live_and_shadow(self):
-    controller = make_controller()
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      restricted = update(controller, restrictive_radar())
-    stale_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL)
-    held = [update(controller, radar_fresh=False) for _ in range(stale_frames - 1)]
-    timed_out = update(controller, radar_fresh=False)
-
-    assert all(result.active and result.target_speed == pytest.approx(restricted.target_speed) for result in held)
-    assert not timed_out.active and not timed_out.shadow_active
-    assert timed_out.target_speed == timed_out.base_speed
-    assert timed_out.mpc_accel_max is None
-    assert timed_out.selected_lead == -1 and math.isinf(timed_out.raw_energy_cap)
-    assert controller.live.pace is None and controller.shadow.pace is None
-
-  @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(CAP_FILTER_FRAMES + 10):
-      update(controller, restrictive_radar())
-    result = update(controller, restrictive_radar(), **override)
-
-    assert not result.active
-    assert result.target_speed == result.base_speed
-    assert result.mpc_accel_max is None
-    assert controller.live.pace is None
-
-  def test_shadow_history_never_steps_into_live_actuation(self):
-    controller = make_controller()
-    for _ in range(CAP_FILTER_FRAMES + 20):
-      shadow = update(controller, restrictive_radar(), acc_selected=False)
-    live = update(controller)
-
-    assert shadow.shadow_active and not shadow.active
-    assert shadow.shadow_filtered_cap < math.inf
-    assert live.active and live.target_speed == live.base_speed
-    assert math.isinf(live.live_filtered_cap)
-
-  def test_explicit_reset_clears_every_path_field(self):
-    controller = make_controller()
-    for _ in range(CAP_FILTER_FRAMES + 10):
-      update(controller, restrictive_radar())
-    controller.reset()
-
-    assert controller._held_envelope is None
-    for path in (controller.live, controller.shadow):
-      assert path.pace is None and path.matched_accel_limit is None
-      assert path.state == AccelControllerState.inactive
-      assert path.departure_frames == path.active_frames == path.lead_loss_frames == path.stale_frames == 0
-      assert not path.launching and not path.departure_launch and not path.matched_lead
-      assert not path.braking_limited and not path.braking_handoff and not path.pace_reserve_armed
-      assert math.isinf(path.filtered_cap) and math.isinf(path.filtered_lead_speed) and path.filtered_lead_accel == 0.0

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller_interfaces.py

@@ -1,406 +0,0 @@
-import inspect
-import math
-from types import SimpleNamespace
-
-import numpy as np
-import pytest
-
-from cereal import custom, log, messaging
-from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
-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_personality import AccelControllerState, AccelProfile
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
-  MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
-)
-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),
-      selfdriveState=SimpleNamespace(personality=0),
-    )
-    self.valid = {"radarState": True}
-    self.alive = {"radarState": True}
-    self.logMonoTime = {"radarState": radar_log_mono_time}
-
-
-def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_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._accel_jerk_smoothing_blocked = False
-  planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
-  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(
-    planner, "accel_controller_result",
-    SimpleNamespace(
-      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,
-    ),
-  )
-  return planner, is_e2e_calls
-
-
-def run_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False):
-  calls = []
-  planner._run_mpc = lambda *args, **kwargs: calls.append((args, kwargs))
-  is_e2e = planner.update_accel_controller_mpc(
-    {}, 20.0, mpc_v_cruise, True, reset_state=False, cruise_initialized=True,
-    available_accel_max=ACCEL_MAX, previous_should_stop=False, force_decel=force_decel,
-  )
-  return is_e2e, calls
-
-
-def test_profile_enum_keeps_toyota_importable():
-  expected = {"eco": 0, "normal": 1, "sport": 2}
-  assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
-  assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
-  from opendbc.car.toyota.carstate import AccelPersonality, CarState
-
-  assert AccelPersonality.schema.enumerants == expected
-  assert CarState.__module__ == "opendbc.car.toyota.carstate"
-
-
-def test_mpc_accepts_optional_acceleration_ceiling_without_changing_stock_bounds():
-  assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality", "accel_max")
-  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)
-
-  requested_ceiling = np.full(N + 1, 0.4)
-  mpc.update(radar, 30.0, accel_max=requested_ceiling)
-  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.update(radar, 30.0, accel_max=malformed_ceiling)
-    np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
-    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)
-  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_weights(True, personality=log.LongitudinalPersonality.standard, jerk_cost_multiplier=1.0)
-  explicit_costs, explicit_constraints = captured[-1]
-  mpc.set_weights(True, personality=log.LongitudinalPersonality.standard, jerk_cost_multiplier=1.2)
-  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, jerk_cost_multiplier=1.2)
-  assert captured[-1][0][-2] == 0.0
-  assert captured[-1][0][-1] == pytest.approx(default_costs[-1] * 1.2)
-
-
-def test_inherited_planner_uses_real_state_raw_radar_and_one_mpc_solve():
-  radar = radar_state()
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner.a_desired = -0.2
-  planner.v_desired_filter = SimpleNamespace(x=12.0)
-  calls = []
-
-  def update_mpc(radar_arg, target, *, personality, accel_max):
-    calls.append(("update", radar_arg, target, personality, accel_max))
-
-  planner.mpc = SimpleNamespace(
-    set_weights=lambda constraint, personality, jerk_cost_multiplier: calls.append(
-      ("weights", constraint, personality, jerk_cost_multiplier),
-    ),
-    set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)),
-    update=update_mpc,
-  )
-  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
-  sm = {"radarState": radar, "selfdriveState": SimpleNamespace(personality=2)}
-  planner._run_mpc(sm, 17.5, True, ceiling, jerk_cost_multiplier=1.2)
-
-  assert calls == [
-    ("weights", True, 2, 1.2),
-    ("state", 12.0, -0.2),
-    ("update", radar, 17.5, 2, ceiling),
-  ]
-  assert calls[-1][1] is radar
-
-
-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, calls = run_controller_mpc(planner)
-
-  assert not is_e2e
-  assert len(mode_calls) == 1
-  assert calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})]
-
-
-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,
-  )
-  _, calls = run_controller_mpc(planner)
-
-  assert calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})]
-
-
-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,
-  )
-  _, calls = run_controller_mpc(planner)
-
-  assert calls == [(({}, 0.0, True, None), {"jerk_cost_multiplier": 1.0})]
-
-
-@pytest.mark.parametrize(
-  ("active", "departure_launching", "is_e2e", "expected"),
-  [
-    (True, True, False, False),
-    (True, False, False, True),
-    (False, True, False, True),
-    (True, True, True, True),
-  ],
-)
-def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, is_e2e, expected):
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner.accel_controller_result = SimpleNamespace(
-    active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold,
-  )
-  assert planner.accel_controller_should_stop(True, is_e2e) is expected
-  expected_hold = active and not departure_launching and not is_e2e
-  assert planner.accel_controller_should_stop(False, is_e2e) is expected_hold
-
-
-@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, calls = run_controller_mpc(planner)
-
-  assert returned_e2e is is_e2e
-  assert len(mode_calls) == 1
-  assert calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})]
-
-
-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)
-  _, calls = run_controller_mpc(planner, mpc_v_cruise=0.0, force_decel=True)
-
-  assert len(mode_calls) == 1
-  assert calls == [(({}, 0.0, True, None), {"jerk_cost_multiplier": 1.0})]
-
-
-def test_previous_mpc_failure_gets_one_stock_recovery_cycle():
-  ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
-  planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
-  resets = []
-  planner.accel_controller = SimpleNamespace(reset=lambda: resets.append(True))
-  planner.mpc = SimpleNamespace(last_solution_status=4)
-
-  _, failed_recovery_calls = run_controller_mpc(planner)
-  assert resets == [True]
-  assert len(mode_calls) == 1
-  assert failed_recovery_calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})]
-
-  planner.mpc.last_solution_status = 0
-  _, recovered_calls = run_controller_mpc(planner)
-  assert resets == [True]
-  assert len(mode_calls) == 2
-  assert recovered_calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})]
-
-
-def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier():
-  planner, _ = planner_for_mpc_test(
-    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
-    mpc_source=MpcLongitudinalPlanSource.cruise,
-  )
-  _, calls = run_controller_mpc(planner)
-
-  assert calls == [(({}, 15.0, True, None), {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER})]
-
-
-def test_lead_source_blocks_smoothing_only_until_the_restriction_episode_ends():
-  planner, _ = planner_for_mpc_test(
-    state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
-    mpc_source=MpcLongitudinalPlanSource.cruise,
-  )
-  _, initial_calls = run_controller_mpc(planner)
-  routine_result = planner.accel_controller_result
-  assert initial_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
-
-  planner.mpc.source = MpcLongitudinalPlanSource.lead0
-  _, lead_calls = run_controller_mpc(planner)
-  assert lead_calls[0][1] == {"jerk_cost_multiplier": 1.0}
-
-  planner.mpc.source = MpcLongitudinalPlanSource.cruise
-  _, flicker_calls = run_controller_mpc(planner)
-  assert flicker_calls[0][1] == {"jerk_cost_multiplier": 1.0}
-
-  free_result = SimpleNamespace(**(vars(routine_result) | {"state": AccelControllerState.free, "target_speed": 20.0}))
-  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", free_result)
-  run_controller_mpc(planner)
-  planner.update_accel_controller = lambda *_args, **_kwargs: setattr(planner, "accel_controller_result", routine_result)
-  _, rearmed_calls = run_controller_mpc(planner)
-  assert rearmed_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
-
-
-@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),
-    (AccelControllerState.restrict, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.lead0),
-    (AccelControllerState.restrict, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.lead1),
-  ],
-)
-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,
-  )
-  _, calls = run_controller_mpc(planner)
-
-  assert calls[0][1] == {"jerk_cost_multiplier": 1.0}
-
-
-def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
-  radar = radar_state()
-  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
-  planner.accel_personality = int(AccelProfile.normal)
-  planner.accel_personality_enabled = True
-  planner._radar_fresh_this_cycle = True
-  planner.a_desired = -0.4
-  planner.v_desired_filter = SimpleNamespace(x=9.5)
-  planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.lead0)
-  received = {}
-  planner.accel_controller = SimpleNamespace(
-    update=lambda *_args, **kwargs: received.update(kwargs) or SimpleNamespace(target_speed=12.0),
-  )
-  sm = {
-    "radarState": radar,
-    "carState": SimpleNamespace(vEgo=10.0, aEgo=-0.2),
-    "selfdriveState": SimpleNamespace(personality=0),
-  }
-  planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
-
-  assert received["previous_mpc_source"] == log.LongitudinalPlan.LongitudinalPlanSource.lead0
-  assert received["planner_speed"] == 9.5
-  assert received["planner_accel"] == -0.4
-  assert received["radar_fresh"] is True
-
-
-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._read_accel_controller_params = lambda: None
-  planner.events_sp = SimpleNamespace(clear=lambda: None)
-  dec_freshness = []
-  planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh: dec_freshness.append(radar_fresh))
-  planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
-  planner.accel_personality = int(AccelProfile.normal)
-  planner.accel_personality_enabled = True
-  planner.a_desired = 0.0
-  planner.v_desired_filter = SimpleNamespace(x=10.0)
-  planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise)
-  controller_freshness = []
-  planner.accel_controller = SimpleNamespace(
-    update=lambda *_args, **kwargs: controller_freshness.append(kwargs["radar_fresh"]) or SimpleNamespace(target_speed=20.0),
-  )
-
-  sm = PlannerSM(100)
-  for expected in (True, False):
-    planner.update(sm)
-    planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
-    assert dec_freshness[-1] is expected and controller_freshness[-1] is expected
-
-  sm.logMonoTime["radarState"] = 101
-  planner.update(sm)
-  planner.update_accel_controller(sm, 20.0, True, True, True, ACCEL_MAX, False)
-  assert dec_freshness[-1] is True and controller_freshness[-1] is True
-
-
-def test_shadow_telemetry_publishes_controller_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_result = SimpleNamespace(
-    enabled=True, active=False, shadow_active=True, profile=AccelProfile.normal,
-    state=AccelControllerState.inactive, shadow_state=AccelControllerState.restrict,
-    base_speed=20.0, raw_energy_cap=15.0, live_filtered_cap=math.inf, shadow_filtered_cap=12.5,
-    selected_lead=1, usable_gap=30.0, closing_speed=5.0, required_decel=0.4,
-    profile_accel_max=math.inf, effective_accel_max=math.inf,
-  )
-  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 and telemetry.shadowOnly
-  assert telemetry.profile == int(AccelProfile.normal)
-  assert telemetry.state == int(AccelControllerState.restrict)
-  assert telemetry.vTargetBase == pytest.approx(20.0)
-  assert telemetry.vTargetRaw == pytest.approx(15.0)
-  assert telemetry.vTargetShadow == pytest.approx(12.5)
-  assert telemetry.leadIndex == 1
-  assert telemetry.usableGap == pytest.approx(30.0)
-  assert telemetry.closingSpeed == pytest.approx(5.0)
-  assert telemetry.requiredDecel == pytest.approx(0.4)
-  assert telemetry.aMaxProfile == math.inf
-  assert telemetry.aMaxEffective == math.inf

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

@@ -1,42 +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
-  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

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

@@ -6,116 +6,129 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
+from cereal import messaging
+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
 
-from cereal import messaging
-from numpy import interp
-from opendbc.car import structs
-from openpilot.common.params import Params
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+# 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):
@@ -129,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
@@ -163,14 +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
 
   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:
@@ -183,151 +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.status))
-      self._has_any_lead = bool(lead_one.status or lead_two.status)
-      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.status))
+    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
 
-    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 _radar_acc_lead_score(self, lead_one) -> float:
-    radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
-    return float(lead_one.status 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 _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:
-      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 not self._CP.radarUnavailable and self._has_radar_acc_lead:
-      return 'acc', True
+  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) -> None:
-    self._read_params()
-    self.set_mpc_fcw_crash_cnt()
-    self._update_calculations(sm, radar_fresh)
-
-    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

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)  # Ensure urgency triggers slowdown
+  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"

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

@@ -1,479 +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.status = 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
-  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_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_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"

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -8,15 +8,7 @@ See the LICENSE.md file in the root directory for more details.
 from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
-from openpilot.sunnypilot import get_sanitize_int_param
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelController, AccelControllerState, AccelProfile
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
-  MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION,
-)
 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
@@ -30,8 +22,7 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
-  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
-    self.params = Params()
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
@@ -41,29 +32,10 @@ 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.accel_controller = AccelController(CP, dt=dt)
-    self.accel_controller_result = None
-    self._accel_jerk_smoothing_blocked = False
-    self._radar_log_mono_time = None
-    self._radar_fresh_this_cycle = True
-
-    self._param_read_frames = max(1, int(round(0.25 / dt)))
-    self._param_frame = 0
-    self.accel_personality_enabled = False
-    self.accel_personality = int(AccelProfile.normal)
 
     self.output_v_target = 0.
     self.output_a_target = 0.
 
-  def _read_accel_controller_params(self) -> None:
-    if self._param_frame % self._param_read_frames == 0:
-      self.accel_personality_enabled = self.params.get_bool("AccelPersonalityEnabled")
-      self.accel_personality = get_sanitize_int_param(
-        "AccelPersonality", int(AccelProfile.eco), int(AccelProfile.sport), self.params,
-      )
-
-    self._param_frame += 1
-
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
     if not self.dec.active():
@@ -101,92 +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:
-    try:
-      radar_log_mono_time = int(sm.logMonoTime['radarState'])
-      radar_healthy = bool(sm.valid['radarState'] and sm.alive['radarState'])
-    except (AttributeError, KeyError, TypeError, ValueError):
-      return True
-
-    previous_log_mono_time = getattr(self, '_radar_log_mono_time', None)
-    radar_advanced = previous_log_mono_time is None or radar_log_mono_time > previous_log_mono_time
-    if radar_advanced:
-      self._radar_log_mono_time = radar_log_mono_time
-    return radar_healthy and radar_advanced
-
-  def update_accel_controller(self, sm: messaging.SubMaster, base_speed: float, engaged: bool, cruise_initialized: bool,
-                              acc_selected: bool, stock_accel_max: float, previous_should_stop: bool) -> float:
-    self.accel_controller_result = self.accel_controller.update(
-      sm['radarState'], base_speed=base_speed, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
-      profile=self.accel_personality, follow_personality=sm['selfdriveState'].personality,
-      enabled=self.accel_personality_enabled, acc_selected=acc_selected, engaged=engaged, cruise_initialized=cruise_initialized,
-      stock_accel_max=stock_accel_max, previous_should_stop=previous_should_stop,
-      radar_fresh=getattr(self, '_radar_fresh_this_cycle', True),
-      previous_mpc_source=getattr(getattr(self, 'mpc', None), 'source', None),
-      planner_speed=getattr(getattr(self, 'v_desired_filter', None), 'x', sm['carState'].vEgo),
-      planner_accel=getattr(self, 'a_desired', sm['carState'].aEgo),
-    )
-    return self.accel_controller_result.target_speed
-
-  def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None,
-               *, jerk_cost_multiplier: float = 1.0) -> None:
-    self.mpc.set_weights(
-      prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_cost_multiplier=jerk_cost_multiplier,
-    )
-    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality, accel_max=accel_max)
-
-  def update_accel_controller_mpc(self, sm: messaging.SubMaster, base_v_cruise: float, mpc_v_cruise: float,
-                                  prev_accel_constraint: bool, *, reset_state: bool, cruise_initialized: bool,
-                                  available_accel_max: float, previous_should_stop: bool, force_decel: bool):
-    is_e2e = self.is_e2e(sm)
-    previous_mpc_failed = getattr(getattr(self, 'mpc', None), 'last_solution_status', 0) != 0
-    if previous_mpc_failed and hasattr(self, 'accel_controller'):
-      self.accel_controller.reset()
-
-    self.update_accel_controller(
-      sm, base_v_cruise, engaged=not reset_state and not force_decel, cruise_initialized=cruise_initialized,
-      acc_selected=not is_e2e and not previous_mpc_failed, stock_accel_max=available_accel_max, previous_should_stop=previous_should_stop,
-    )
-    result = self.accel_controller_result
-    actuating = result.active and not is_e2e and not force_decel and not previous_mpc_failed
-    valid_lead_stop_hold = (actuating and result.state == AccelControllerState.stopHold
-                            and result.selected_lead >= 0)
-    controller_v_cruise = mpc_v_cruise if valid_lead_stop_hold else min(mpc_v_cruise, result.target_speed) if actuating else mpc_v_cruise
-    accel_max = result.mpc_accel_max if actuating else None
-    target_reduction = mpc_v_cruise - controller_v_cruise
-    lead_restriction = (
-      actuating and prev_accel_constraint and result.state == AccelControllerState.restrict and result.selected_lead >= 0
-      and not result.launching and target_reduction > 1e-6
-    )
-    smoothing_eligible = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
-                          and 0.0 < result.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
-    smoothing_blocked = getattr(self, '_accel_jerk_smoothing_blocked', False)
-    if previous_mpc_failed:
-      smoothing_blocked = True
-    elif not lead_restriction:
-      smoothing_blocked = False
-    elif not smoothing_blocked and (getattr(self.mpc, 'source', None) != MpcLongitudinalPlanSource.cruise or not smoothing_eligible):
-      smoothing_blocked = True
-    self._accel_jerk_smoothing_blocked = smoothing_blocked
-    jerk_cost_multiplier = MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not smoothing_blocked else 1.0
-    self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, jerk_cost_multiplier=jerk_cost_multiplier)
-
-    return is_e2e
-
-  def accel_controller_should_stop(self, should_stop: bool, is_e2e: bool) -> bool:
-    result = self.accel_controller_result
-    if result is None or not result.active or is_e2e:
-      return should_stop
-    if result.departure_launching:
-      return False
-    return should_stop or result.state == AccelControllerState.stopHold
-
   def update(self, sm: messaging.SubMaster) -> None:
-    self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
-    self._read_accel_controller_params()
     self.events_sp.clear()
-    self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle)
+    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:
@@ -206,25 +95,6 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
-    if self.accel_controller_result is not None:
-      result = self.accel_controller_result
-      accel_controller = longitudinalPlanSP.accelController
-      accel_controller.enabled = result.enabled
-      accel_controller.active = result.active
-      accel_controller.shadowOnly = result.shadow_active and not result.active
-      accel_controller.profile = int(result.profile)
-      accel_controller.state = int(result.state if result.active else result.shadow_state)
-      accel_controller.vTargetBase = float(result.base_speed)
-      accel_controller.vTargetRaw = float(result.raw_energy_cap)
-      accel_controller.vTargetFiltered = float(result.live_filtered_cap)
-      accel_controller.vTargetShadow = float(result.shadow_filtered_cap)
-      accel_controller.leadIndex = result.selected_lead
-      accel_controller.usableGap = float(result.usable_gap)
-      accel_controller.closingSpeed = float(result.closing_speed)
-      accel_controller.requiredDecel = float(result.required_decel)
-      accel_controller.aMaxProfile = float(result.profile_accel_max)
-      accel_controller.aMaxEffective = float(result.effective_accel_max)
-
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

sunnypilot/selfdrive/controls/lib/relc.py

@@ -1,84 +0,0 @@
-"""
-Copyright (c) 2021-, rav4kumar, 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 openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.common.params import Params
-
-NEARSIDE_PROB = 0.2
-EDGE_PROB = 0.35
-EDGE_REACTION_TIME = 1.0
-EDGE_CLEAR_TIME = 0.3
-MIN_SPEED = 20 * CV.MPH_TO_MS
-
-
-class RoadEdgeLaneChangeController:
-  def __init__(self, desire_helper):
-    self.DH = desire_helper
-    self.params = Params()
-    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
-    self.param_read_counter = 0
-    self.left_edge_detected = False
-    self.right_edge_detected = False
-    self.left_edge_timer = 0.0
-    self.right_edge_timer = 0.0
-    self.left_clear_timer = 0.0
-    self.right_clear_timer = 0.0
-
-  def read_params(self) -> None:
-    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
-
-  def update_params(self) -> None:
-    if self.param_read_counter % 50 == 0:
-      self.read_params()
-    self.param_read_counter += 1
-
-  def reset(self) -> None:
-    self.left_edge_detected = False
-    self.right_edge_detected = False
-    self.left_edge_timer = 0.0
-    self.right_edge_timer = 0.0
-    self.left_clear_timer = 0.0
-    self.right_clear_timer = 0.0
-
-  def update(self, road_edge_stds, lane_line_probs, v_ego: float) -> None:
-    self.update_params()
-
-    if not self.enabled or v_ego < MIN_SPEED:
-      self.reset()
-      return
-
-    left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
-    right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
-    left_lane_prob = lane_line_probs[0]
-    right_lane_prob = lane_line_probs[3]
-
-    left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and right_lane_prob >= left_lane_prob
-    right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and left_lane_prob >= right_lane_prob
-
-    if left_cond:
-      self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
-      self.left_clear_timer = 0.0
-      if self.left_edge_timer > EDGE_REACTION_TIME:
-        self.left_edge_detected = True
-    else:
-      self.left_clear_timer += DT_MDL
-      if self.left_clear_timer > EDGE_CLEAR_TIME:
-        self.left_edge_timer = 0.0
-        self.left_edge_detected = False
-
-    if right_cond:
-      self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
-      self.right_clear_timer = 0.0
-      if self.right_edge_timer > EDGE_REACTION_TIME:
-        self.right_edge_detected = True
-    else:
-      self.right_clear_timer += DT_MDL
-      if self.right_clear_timer > EDGE_CLEAR_TIME:
-        self.right_edge_timer = 0.0
-        self.right_edge_detected = False

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

@@ -4,8 +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
-
 import numpy as np
 import pytest
 
@@ -15,12 +13,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
 
@@ -124,21 +118,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
@@ -164,253 +143,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",
     [

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.selfdrive.test.longitudinal_maneuvers.plant import Plant
-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
-
-
-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_personality_enabled = False
-  planner._read_accel_controller_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.

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

sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py

@@ -1,998 +0,0 @@
-from collections.abc import Callable
-from dataclasses import dataclass
-import gc
-
-import numpy as np
-import pytest
-
-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, get_T_FOLLOW
-from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
-from openpilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, Plant
-from openpilot.sunnypilot.selfdrive.controls.lib import longitudinal_planner as longitudinal_planner_sp
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality import AccelControllerState, AccelProfile
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import (
-  MATCHED_PACE_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, PACE_TARGET_RESERVE,
-)
-
-ACTUATOR_DYNAMICS = (
-  (0.10, 0.20),
-  (0.15, 0.25),
-  (0.20, 0.20),
-  (0.25, 0.30),
-  (0.30, 0.35),
-)
-ACTUATOR_IDS = ("toyota", "honda", "gm", "hyundai", "ford")
-ROUTINE_GAP_TOLERANCE = 0.10
-ROUTINE_DECEL_TOLERANCE = 0.10
-DROPOUT_GAP_TOLERANCE = 0.15
-MOVING_LEAD_GAP_TOLERANCE = 0.12
-
-
-@dataclass
-class ClosedLoopTrace:
-  time: np.ndarray
-  speed: np.ndarray
-  distance: np.ndarray
-  distance_lead: np.ndarray
-  a_target: np.ndarray
-  acceleration: np.ndarray
-  should_stop: np.ndarray
-  fcw: np.ndarray
-  source: list
-  dec_mode: list[str]
-  active: np.ndarray
-  shadow_active: np.ndarray
-  launching: np.ndarray
-  target_speed: np.ndarray
-  raw_cap: np.ndarray
-  filtered_cap: np.ndarray
-  selected_lead: np.ndarray
-  profile_accel_max: np.ndarray
-  effective_accel_max: np.ndarray
-  state: np.ndarray
-  required_decel: np.ndarray
-  planner_seed_accel: np.ndarray
-  mpc_seed_accel: np.ndarray
-  mpc_upper_first: np.ndarray
-  mpc_upper_min: np.ndarray
-  mpc_upper_max: np.ndarray
-  stock_bounds_valid: np.ndarray
-  raw_radar_passthrough: np.ndarray
-  actuator_command: np.ndarray
-  solver_status: np.ndarray
-  mpc_calls: np.ndarray
-  solver_failures: int
-  solver_failure_times: list[float]
-
-
-def _configure_plant(plant: Plant, *, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None:
-  plant.planner.accel_personality_enabled = enabled
-  plant.planner.accel_personality = profile
-  plant.planner._read_accel_controller_params = lambda: None
-  plant.planner.dec._enabled = dec_enabled
-  plant.planner.dec._read_params = lambda: None
-
-
-def _run(
-  *,
-  duration: float,
-  controller_enabled: bool,
-  profile: int = 1,
-  v_lead: float | Callable[[float], float] = 0.0,
-  v_cruise: float = 30.0,
-  dec_enabled: bool = False,
-  radar_fresh_fn: Callable[[int], bool] | None = None,
-  **plant_kwargs,
-) -> ClosedLoopTrace:
-  gc.collect()
-  plant = Plant(**plant_kwargs)
-  _configure_plant(plant, enabled=controller_enabled, profile=profile, dec_enabled=dec_enabled)
-  plant.v_lead_prev = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(0.0))
-  if radar_fresh_fn is not None:
-    radar_frame = 0
-
-    def patterned_radar_freshness(_sm):
-      nonlocal radar_frame
-      fresh = radar_fresh_fn(radar_frame)
-      radar_frame += 1
-      return fresh
-
-    plant.planner._update_radar_freshness = patterned_radar_freshness
-
-  solver_failures = 0
-  solver_failure_times = []
-  mpc_call_count = 0
-  controller_radar = None
-  radar_passthrough = []
-  seed_calls = []
-  original_controller_update = plant.planner.accel_controller.update
-  original_mpc_reset = plant.planner.mpc.reset
-  original_mpc_set_cur_state = plant.planner.mpc.set_cur_state
-  original_mpc_update = plant.planner.mpc.update
-
-  def record_controller_radar(radar_state, *args, **kwargs):
-    nonlocal controller_radar
-    controller_radar = radar_state
-    return original_controller_update(radar_state, *args, **kwargs)
-
-  def count_failed_solve(*args, **kwargs) -> None:
-    nonlocal solver_failures
-    if plant.planner.mpc.solution_status != 0:
-      solver_failures += 1
-      solver_failure_times.append(plant.current_time)
-    original_mpc_reset(*args, **kwargs)
-
-  def record_seed(v_ego, a_ego):
-    seed_calls.append((float(plant.planner.a_desired), float(a_ego)))
-    return original_mpc_set_cur_state(v_ego, a_ego)
-
-  def count_mpc_call(radar_state, *args, **kwargs):
-    nonlocal mpc_call_count
-    mpc_call_count += 1
-    radar_passthrough.append(radar_state is controller_radar)
-    return original_mpc_update(radar_state, *args, **kwargs)
-
-  plant.planner.accel_controller.update = record_controller_radar
-  plant.planner.mpc.reset = count_failed_solve
-  plant.planner.mpc.set_cur_state = record_seed
-  plant.planner.mpc.update = count_mpc_call
-  rows = []
-  sources = []
-  dec_modes = []
-  try:
-    while plant.current_time < duration:
-      lead_speed = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(plant.current_time))
-      calls_before = mpc_call_count
-      radar_checks_before = len(radar_passthrough)
-      seed_calls_before = len(seed_calls)
-      result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
-      controller = plant.planner.accel_controller_result
-      calls_this_frame = mpc_call_count - calls_before
-      passthrough_this_frame = (len(radar_passthrough) > radar_checks_before
-                                and all(radar_passthrough[radar_checks_before:]))
-      if len(seed_calls) > seed_calls_before:
-        planner_seed_accel, mpc_seed_accel = seed_calls[-1]
-      else:
-        planner_seed_accel = mpc_seed_accel = np.nan
-      lower = plant.planner.mpc.params[:, 0]
-      upper = plant.planner.mpc.params[:, 1]
-      bounds_valid = (np.allclose(lower, ACCEL_MIN) and np.all(np.isfinite(upper))
-                      and np.all(upper >= lower) and np.all(upper <= ACCEL_MAX + 1e-9))
-      rows.append((
-        plant.current_time, result["speed"], result["distance"], result["distance_lead"], result["a_target"],
-        result["realized_acceleration"], result["should_stop"], result["fcw"], controller.active,
-        controller.shadow_active, controller.launching, controller.target_speed, controller.raw_energy_cap,
-        controller.live_filtered_cap, controller.selected_lead, controller.profile_accel_max,
-        controller.effective_accel_max, controller.state, controller.required_decel, planner_seed_accel,
-        mpc_seed_accel, upper[0], np.min(upper), np.max(upper), bounds_valid, passthrough_this_frame,
-        result["actuator_command"], plant.planner.mpc.last_solution_status, calls_this_frame,
-      ))
-      sources.append(result["mpc_source"])
-      dec_modes.append(result["dec_mode"])
-  finally:
-    plant.planner.accel_controller.update = original_controller_update
-    plant.planner.mpc.reset = original_mpc_reset
-    plant.planner.mpc.set_cur_state = original_mpc_set_cur_state
-    plant.planner.mpc.update = original_mpc_update
-
-  data = np.asarray(rows, dtype=float)
-  trace = ClosedLoopTrace(
-    time=data[:, 0], speed=data[:, 1], distance=data[:, 2], distance_lead=data[:, 3], a_target=data[:, 4], acceleration=data[:, 5],
-    should_stop=data[:, 6].astype(bool), fcw=data[:, 7].astype(bool), source=sources, dec_mode=dec_modes,
-    active=data[:, 8].astype(bool), shadow_active=data[:, 9].astype(bool), launching=data[:, 10].astype(bool), target_speed=data[:, 11],
-    raw_cap=data[:, 12], filtered_cap=data[:, 13], selected_lead=data[:, 14].astype(int), profile_accel_max=data[:, 15],
-    effective_accel_max=data[:, 16], state=data[:, 17].astype(int), required_decel=data[:, 18], planner_seed_accel=data[:, 19],
-    mpc_seed_accel=data[:, 20], mpc_upper_first=data[:, 21], mpc_upper_min=data[:, 22], mpc_upper_max=data[:, 23],
-    stock_bounds_valid=data[:, 24].astype(bool), raw_radar_passthrough=data[:, 25].astype(bool), actuator_command=data[:, 26],
-    solver_status=data[:, 27].astype(int), mpc_calls=data[:, 28].astype(int), solver_failures=solver_failures,
-    solver_failure_times=solver_failure_times,
-  )
-  gc.collect()
-  return trace
-
-
-def _first_time_below(trace: ClosedLoopTrace, threshold: float) -> float:
-  indices = np.flatnonzero(trace.a_target <= threshold)
-  assert len(indices), f"never reached {threshold} m/s²"
-  return float(trace.time[indices[0]])
-
-
-def _sustained_time_below(trace: ClosedLoopTrace, threshold: float, *, after: float = 0.5, duration: float = 0.5) -> float:
-  required_frames = round(duration / DT_MDL)
-  below = (trace.time >= after) & (trace.a_target <= threshold)
-  sustained = np.convolve(below.astype(int), np.ones(required_frames, dtype=int), mode="valid") == required_frames
-  indices = np.flatnonzero(sustained)
-  assert len(indices), f"never sustained {threshold} m/s² for {duration} s"
-  return float(trace.time[indices[0]])
-
-
-def _command_jerk(trace: ClosedLoopTrace, after: float = 0.0) -> np.ndarray:
-  indices = np.flatnonzero(trace.time >= after)
-  assert len(indices) >= 2
-  return np.diff(trace.a_target[indices]) / DT_MDL
-
-
-def _filtered_realized_jerk(trace: ClosedLoopTrace, after: float = 1.0) -> np.ndarray:
-  filtered_acceleration = np.convolve(trace.acceleration, np.ones(3) / 3.0, mode="valid")
-  samples = trace.time[2:-1] >= after
-  return (np.diff(filtered_acceleration) / DT_MDL)[samples]
-
-
-def _has_brake_coast_brake(values: np.ndarray, brake: float = -0.8, coast: float = -0.35, frames: int = 2) -> bool:
-  phase = 0
-  for index in range(len(values) - frames + 1):
-    window = values[index:index + frames]
-    if np.all(window <= brake):
-      if phase == 2:
-        return True
-      phase = 1
-    elif phase == 1 and np.all(window >= coast):
-      phase = 2
-  return False
-
-
-def _has_propulsion_after_braking(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool:
-  braking = False
-  for index in range(len(values) - frames + 1):
-    window = values[index:index + frames]
-    if np.all(window <= brake):
-      braking = True
-    elif braking and np.all(window >= propulsion):
-      return True
-  return False
-
-
-def _has_propulsion_brake_cycle(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool:
-  phases = []
-  for index in range(len(values) - frames + 1):
-    window = values[index:index + frames]
-    phase = 1 if np.all(window >= propulsion) else -1 if np.all(window <= brake) else 0
-    if phase and (not phases or phase != phases[-1]):
-      phases.append(phase)
-      if len(phases) >= 3 and phases[-1] == phases[-3]:
-        return True
-  return False
-
-
-def _assert_non_actuating_matches_stock(trace: ClosedLoopTrace, baseline: ClosedLoopTrace) -> None:
-  np.testing.assert_allclose(trace.a_target, baseline.a_target, atol=1e-6, rtol=0.0)
-  np.testing.assert_array_equal(trace.should_stop, baseline.should_stop)
-  np.testing.assert_array_equal(trace.fcw, baseline.fcw)
-  np.testing.assert_array_equal(trace.solver_status, baseline.solver_status)
-  assert trace.source == baseline.source
-  assert trace.solver_failures == baseline.solver_failures
-  assert trace.solver_failure_times == baseline.solver_failure_times
-
-
-def _assert_no_new_solver_failures(trace: ClosedLoopTrace, baseline: ClosedLoopTrace) -> None:
-  assert trace.solver_failures <= baseline.solver_failures
-
-
-@pytest.mark.parametrize(
-  ("plant_kwargs", "expect_shadow"),
-  [
-    ({"enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0}, False),
-    ({"e2e": True, "lead_relevancy": False, "speed": 20.0}, True),
-  ],
-  ids=("disengaged", "e2e-shadow"),
-)
-def test_non_actuating_modes_match_stock(plant_kwargs, expect_shadow):
-  common = dict(duration=2.0, v_lead=14.0, **plant_kwargs)
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-
-  _assert_non_actuating_matches_stock(trace, baseline)
-  assert not trace.active.any()
-  np.testing.assert_array_equal(trace.shadow_active, np.full_like(trace.active, expect_shadow))
-  np.testing.assert_allclose(trace.mpc_upper_min, baseline.mpc_upper_min)
-  assert trace.raw_radar_passthrough.all()
-  assert np.all(trace.mpc_calls == 1)
-
-
-def test_disabled_profiles_are_identical():
-  common = dict(duration=2.0, controller_enabled=False, lead_relevancy=True, speed=20.0, distance_lead=70.0, v_lead=14.0)
-  traces = [_run(profile=profile, **common) for profile in range(3)]
-  for trace in traces[1:]:
-    _assert_non_actuating_matches_stock(trace, traces[0])
-
-
-@pytest.mark.parametrize("lead_relevancy", (False, True), ids=("clear-road", "lead"))
-def test_force_decel_matches_stock(lead_relevancy):
-  common = dict(duration=2.0, force_decel=True, lead_relevancy=lead_relevancy, speed=20.0,
-                distance_lead=70.0, v_lead=14.0, profile=0)
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-  _assert_non_actuating_matches_stock(trace, baseline)
-  assert not trace.active.any()
-  np.testing.assert_allclose(trace.mpc_upper_min, ACCEL_MAX)
-
-
-@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
-def test_active_controller_uses_one_raw_mpc_solve_and_feasible_stock_bounds(profile):
-  trace = _run(
-    duration=4.0, controller_enabled=True, profile=profile, lead_relevancy=False, speed=0.0,
-    v_cruise=22.352, actuator_delay=0.15, actuator_lag=0.20,
-  )
-
-  assert trace.active.all()
-  assert np.all(trace.mpc_calls == 1)
-  assert trace.raw_radar_passthrough.all()
-  assert trace.stock_bounds_valid.all()
-  np.testing.assert_allclose(trace.mpc_seed_accel, trace.planner_seed_accel, atol=1e-12, rtol=0.0)
-  assert np.all(trace.mpc_upper_first + 1e-9 >= trace.mpc_seed_accel)
-  assert np.all(trace.mpc_upper_min >= 0.0)
-  assert np.any(trace.mpc_upper_min < ACCEL_MAX - 0.05)
-  assert trace.solver_failures == 0
-
-
-def test_e2e_to_radar_acc_handoff_keeps_braking_continuous():
-  def run_handoff(controller_enabled: bool):
-    plant = Plant(
-      lead_relevancy=True, speed=10.0, distance_lead=30.0, actuator_delay=0.15, actuator_lag=0.20,
-      model_action_fn=lambda current_time, _v_ego, _a_ego: (-1.0 if current_time < 2.0 else 0.0, False),
-    )
-    _configure_plant(plant, enabled=controller_enabled)
-    rows = []
-    while plant.current_time < 2.4:
-      plant.e2e = plant.current_time < 2.0
-      result = plant.step(v_lead=8.0, v_cruise=20.0)
-      rows.append((plant.current_time, result["a_target"], plant.planner.mpc.last_solution_status,
-                   plant.planner.accel_controller_result.active))
-    return np.asarray(rows, dtype=float).T
-
-  baseline_time, baseline_accel, baseline_status, _ = run_handoff(False)
-  time_values, acceleration, solver_status, active = run_handoff(True)
-  np.testing.assert_allclose(time_values, baseline_time, atol=0.0, rtol=0.0)
-  transition = np.flatnonzero(time_values > 2.0)[0]
-  baseline_jump = abs(baseline_accel[transition] - baseline_accel[transition - 1])
-  controlled_jump = abs(acceleration[transition] - acceleration[transition - 1])
-  baseline_jerk = np.max(np.abs(np.diff(baseline_accel[transition:]) / DT_MDL))
-  controlled_jerk = np.max(np.abs(np.diff(acceleration[transition:]) / DT_MDL))
-
-  assert controlled_jump <= baseline_jump + 1e-6
-  assert controlled_jerk <= baseline_jerk + 0.10
-  assert np.count_nonzero(solver_status[transition:]) <= np.count_nonzero(baseline_status[transition:])
-  assert active[transition]
-
-
-def test_dec_retains_acc_through_route_like_radar_marker_dropout():
-  dropout_start = 1.0
-  reacquisition_time = 1.8
-
-  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation:
-    frame = round(current_time / DT_MDL)
-    if current_time < dropout_start:
-      marked_slot = "leadOne" if frame % 2 == 0 else "leadTwo"
-      return truth | {"radar": lead_name == marked_slot, "radarTrackId": 985 + frame if lead_name == marked_slot else -1}
-    if current_time < reacquisition_time:
-      return truth | {"radar": False, "radarTrackId": -1}
-    return truth | {"radar": lead_name == "leadOne", "radarTrackId": 1263 if lead_name == "leadOne" else -1}
-
-  trace = _run(
-    duration=2.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=True, speed=20.0,
-    distance_lead=35.0, v_lead=18.0, v_cruise=30.0, lead_observation_fn=observe,
-    model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False), actuator_delay=0.15, actuator_lag=0.20,
-  )
-  response = (trace.time >= dropout_start - DT_MDL) & (trace.time <= reacquisition_time + 0.5)
-
-  assert all(mode == "acc" for mode in trace.dec_mode)
-  assert all(str(source) != "e2e" for source in trace.source)
-  assert not trace.fcw.any()
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  assert np.max(np.abs(np.diff(trace.a_target[response]) / DT_MDL)) < 3.0
-  assert trace.raw_radar_passthrough.all()
-  assert np.all(trace.mpc_calls == 1)
-  assert trace.solver_failures == 0
-
-
-def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
-  traces = [
-    _run(
-      duration=12.0, controller_enabled=True, profile=profile, lead_relevancy=False, speed=0.0,
-      v_cruise=22.352, actuator_delay=0.15, actuator_lag=0.20,
-    )
-    for profile in range(3)
-  ]
-
-  for trace in traces:
-    positive = np.flatnonzero(trace.a_target > 0.05)
-    moving = np.flatnonzero(trace.speed > 0.01)
-    assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL
-    assert len(moving) and trace.time[moving[0]] <= 1.0
-    assert np.interp(1.0, trace.time, trace.speed) >= 0.33
-    assert not np.any(trace.a_target < -0.05)
-    assert trace.solver_failures == 0
-
-  launch_window = traces[0].time <= 0.5
-  np.testing.assert_allclose(traces[1].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0)
-  np.testing.assert_allclose(traces[2].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0)
-  speed_at_eight = [float(np.interp(8.0, trace.time, trace.speed)) for trace in traces]
-  assert speed_at_eight[0] + 0.75 < speed_at_eight[1]
-  assert speed_at_eight[1] + 0.30 < speed_at_eight[2]
-  final_speed = [float(trace.speed[-1]) for trace in traces]
-  assert final_speed[0] + 1.25 < final_speed[1]
-  assert final_speed[1] + 0.75 < final_speed[2]
-  ceiling_at_ten = [float(np.interp(10.0, trace.speed, trace.mpc_upper_min)) for trace in traces]
-  assert ceiling_at_ten[0] < ceiling_at_ten[1] < ceiling_at_ten[2]
-
-
-@pytest.mark.parametrize(
-  ("speed", "v_cruise"),
-  ((0.0, 22.352), (25.0, 30.0), (35.0, 35.0)),
-)
-def test_decel_smoothing_does_not_change_clear_road_acceleration_at_representative_speeds(monkeypatch, speed, v_cruise):
-  common = dict(
-    duration=3.0, controller_enabled=True, profile=AccelProfile.sport, lead_relevancy=False,
-    speed=speed, v_cruise=v_cruise, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0)
-  stock_weight = _run(**common)
-  monkeypatch.setattr(longitudinal_planner_sp, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER)
-  smoothed = _run(**common)
-
-  np.testing.assert_allclose(smoothed.a_target, stock_weight.a_target, atol=1e-9, rtol=0.0)
-  np.testing.assert_allclose(smoothed.speed, stock_weight.speed, atol=1e-9, rtol=0.0)
-  np.testing.assert_allclose(smoothed.effective_accel_max, stock_weight.effective_accel_max, atol=1e-9, rtol=0.0)
-  assert smoothed.solver_failures == stock_weight.solver_failures == 0
-
-
-def test_prius_route_model_launches_without_a_dead_pedal():
-  trace = _run(
-    duration=3.0, controller_enabled=True, profile=1, lead_relevancy=False, speed=0.0,
-    v_cruise=22.352, actuator_model=PRIUS_TSS2_ROUTE_MODEL,
-  )
-  positive = np.flatnonzero(trace.a_target > 0.05)
-  moving = np.flatnonzero(trace.speed > 0.05)
-  assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL
-  assert len(moving) and trace.time[moving[0]] <= 1.0
-  assert trace.solver_failures == 0
-
-
-def test_stop_hold_survives_short_full_field_dropout():
-  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    return None if 1.0 <= current_time < 1.1 else truth
-
-  common = dict(
-    duration=2.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
-    v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  baseline = _run(**(common | {"controller_enabled": False}))
-  trace = _run(**common)
-  assert np.max(trace.speed) < 1e-3
-  assert np.all(trace.target_speed == 0.0)
-  assert np.all(trace.state == int(AccelControllerState.stopHold))
-  assert not trace.fcw.any()
-  _assert_no_new_solver_failures(trace, baseline)
-
-
-@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
-def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag):
-  departure_time = 1.0
-
-  def lead_speed(current_time: float) -> float:
-    return 0.0 if current_time < departure_time else 2.0
-
-  trace = _run(
-    duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
-    v_lead=lead_speed, v_cruise=8.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
-  )
-  first_three = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9)
-  release = np.flatnonzero((trace.time >= departure_time) & trace.launching)
-  moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05))
-
-  assert not trace.launching[first_three].any()
-  assert trace.should_stop[first_three].all()
-  assert len(release) and trace.time[release[0]] >= departure_time + 3 * DT_MDL
-  assert not trace.should_stop[release[0]]
-  assert len(moving) and trace.time[moving[0]] <= departure_time + 3 * DT_MDL + 1.0
-  assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time])
-  assert trace.solver_failures == 0
-
-
-def test_reused_radar_frames_do_not_pulse_stop_state_during_departure():
-  departure_time = 1.0
-  trace = _run(
-    duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
-    v_lead=lambda current_time: 0.0 if current_time < departure_time else 2.0,
-    v_cruise=8.0, actuator_delay=0.15, actuator_lag=0.25, radar_fresh_fn=lambda frame: frame % 2 == 0,
-  )
-  after_departure = trace.time >= departure_time
-  should_stop = trace.should_stop[after_departure]
-  release = np.flatnonzero(after_departure & trace.launching)
-  moving = np.flatnonzero(after_departure & (trace.speed > 0.05))
-
-  assert np.count_nonzero(np.diff(should_stop.astype(int))) <= 1
-  assert len(release) and len(moving)
-  assert trace.time[moving[0]] <= trace.time[release[0]] + 1.0
-  assert not _has_propulsion_brake_cycle(trace.a_target[after_departure])
-  assert trace.solver_failures == 0
-
-
-@pytest.mark.parametrize("departure_frames", [1, 2, 3])
-def test_short_false_departure_does_not_launch_the_vehicle(departure_frames):
-  trace = _run(
-    duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
-    v_lead=lambda current_time: 2.0 if 1.0 <= current_time < 1.0 + departure_frames * DT_MDL else 0.0,
-    v_cruise=8.0, actuator_delay=0.10, actuator_lag=0.20,
-  )
-
-  assert np.max(trace.speed) < 0.01
-  assert not trace.launching.any()
-  assert trace.should_stop.all()
-  assert trace.state[-1] == int(AccelControllerState.stopHold)
-  assert trace.solver_failures == 0
-
-
-@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
-def test_matched_lead_recovery_preserves_profile_ordering(actuator_delay, actuator_lag):
-  traces = [
-    _run(
-      duration=32.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0,
-      distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
-    )
-    for profile in range(3)
-  ]
-  response = (traces[0].time >= 15.0) & (traces[0].time <= 28.5)
-  mean_accel = [float(np.mean(trace.a_target[response])) for trace in traces]
-  final_speed = [float(trace.speed[np.flatnonzero(response)[-1]]) for trace in traces]
-
-  assert mean_accel[0] + 0.06 < mean_accel[1]
-  assert mean_accel[1] + 0.025 < mean_accel[2]
-  assert final_speed[0] < final_speed[1] < final_speed[2]
-  assert max(final_speed) < 13.5
-  assert all(not _has_propulsion_brake_cycle(trace.a_target[response]) for trace in traces)
-  assert all(trace.solver_failures == 0 for trace in traces)
-
-
-def test_creeping_lead_departure_is_prompt_and_safe():
-  departure_time = 1.0
-
-  def lead_speed(current_time: float) -> float:
-    if current_time < departure_time:
-      return 0.0
-    if current_time < departure_time + 0.5:
-      return 1.6 * (current_time - departure_time)
-    return min(2.5, 0.8 + 0.7 * (current_time - departure_time - 0.5))
-
-  def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True}
-
-  common = dict(
-    duration=6.0, profile=0, lead_relevancy=True, speed=0.0, distance_lead=3.6, v_lead=lead_speed,
-    v_cruise=22.352, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-  after_departure = trace.time >= departure_time
-  lead_speeds = np.array([lead_speed(max(0.0, current_time - DT_MDL)) for current_time in trace.time])
-  baseline_moving = np.flatnonzero((baseline.time >= departure_time) & (baseline.speed > 0.05))
-  moving = np.flatnonzero(after_departure & (trace.speed > 0.05))
-
-  assert len(baseline_moving) and len(moving)
-  assert trace.time[moving[0]] <= baseline.time[baseline_moving[0]]
-  assert np.all(trace.speed[after_departure] <= lead_speeds[after_departure] + 0.20)
-  assert not _has_brake_coast_brake(trace.a_target[after_departure])
-  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3
-  assert trace.solver_failures == 0
-
-
-def test_constant_creep_departure_does_not_pulse_between_launch_and_stop_hold():
-  departure_time = 1.0
-
-  def lead_speed(current_time: float) -> float:
-    return 0.0 if current_time < departure_time else 0.2
-
-  def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True}
-
-  trace = _run(
-    duration=8.0, controller_enabled=True, profile=0, lead_relevancy=True, speed=0.0, distance_lead=3.6,
-    v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  launched = np.flatnonzero((trace.time >= departure_time) & trace.launching)
-  assert len(launched)
-  after_launch = slice(launched[0], None)
-
-  assert not np.any(trace.state[after_launch] == int(AccelControllerState.stopHold))
-  assert not _has_propulsion_brake_cycle(trace.a_target[after_launch])
-  assert np.max(trace.speed[after_launch]) <= 0.4
-  assert not trace.fcw.any()
-  assert trace.solver_failures == 0
-
-
-def test_invalid_departure_geometry_aborts_launch_until_reconfirmed():
-  def lead_speed(current_time: float) -> float:
-    return 0.0 if current_time < 1.0 else 2.0
-
-  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation:
-    return truth | {"vLeadK": -2.0} if 1.45 <= current_time < 1.70 else truth
-
-  trace = _run(
-    duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0,
-    v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  invalid = (trace.time >= 1.50) & (trace.time < 1.75)
-  assert invalid.any()
-  assert not trace.launching[invalid].any()
-  assert np.max(trace.speed[invalid]) < 0.10
-  assert np.max(trace.target_speed[invalid]) == 0.0
-  assert np.all(np.isfinite(trace.a_target))
-  assert trace.solver_failures == 0
-
-
-def test_moving_full_field_dropout_never_releases_pace_or_adds_solver_failures():
-  dropout_start = 2.0
-  dropout_end = 2.15
-
-  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    return None if dropout_start <= current_time < dropout_end else truth
-
-  common = dict(
-    duration=4.0, lead_relevancy=True, speed=22.0, distance_lead=85.0, v_lead=14.0, v_cruise=30.0,
-    lead_observation_fn=observe, actuator_delay=0.20, actuator_lag=0.25,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-  before = trace.target_speed[np.flatnonzero(trace.time < dropout_start)[-1]]
-  response = (trace.time >= dropout_start) & (trace.time <= dropout_end + 0.5)
-
-  assert np.max(trace.target_speed[response]) <= before + 1e-6
-  assert not _has_propulsion_after_braking(trace.a_target[response])
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - DROPOUT_GAP_TOLERANCE
-  _assert_no_new_solver_failures(trace, baseline)
-
-
-def test_false_range_relief_matches_clean_controller_response():
-  glitch_start = 3.0
-  glitch_end = 3.15
-
-  def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation:
-    return truth | {"dRel": truth["dRel"] + 5.0} if glitch_start <= current_time < glitch_end else truth
-
-  common = dict(
-    duration=4.0, lead_relevancy=True, speed=22.0, distance_lead=85.0, v_lead=14.0,
-    v_cruise=30.0, actuator_delay=0.20, actuator_lag=0.25,
-  )
-  baseline = _run(controller_enabled=False, lead_observation_fn=observe, **common)
-  clean = _run(controller_enabled=True, **common)
-  trace = _run(controller_enabled=True, lead_observation_fn=observe, **common)
-  response = (trace.time >= glitch_start) & (trace.time <= glitch_end + 0.5)
-  jerk_response = (trace.time[1:] >= glitch_start) & (trace.time[1:] <= glitch_end + 0.5)
-
-  assert np.max(np.abs(trace.a_target[response] - clean.a_target[response])) < 0.07
-  assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0
-  assert not _has_propulsion_after_braking(trace.a_target[response])
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  _assert_no_new_solver_failures(trace, baseline)
-
-
-@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
-@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
-def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, actuator_delay, actuator_lag):
-  glitch_start = 67.0
-  glitch_end = 67.5
-
-  def lead_speed(current_time: float) -> float:
-    braking_time = np.clip(current_time - 60.0, 0.0, 7.0)
-    return 10.0 - 0.42 * braking_time
-
-  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    if glitch_start <= current_time < glitch_end:
-      if lead_name == "leadOne":
-        return None
-      return truth | {
-        "dRel": truth["dRel"] + 20.0,
-        "vLead": truth["vLead"] + 4.0,
-        "vLeadK": truth["vLeadK"] + 4.0,
-        "vRel": truth["vRel"] + 4.0,
-        "aLeadK": 0.0,
-        "radar": True,
-        "radarTrackId": 200,
-      }
-    if lead_name == "leadTwo":
-      return None
-    return truth | {"aLeadK": -0.42 if 60.0 <= current_time < glitch_start else 0.0, "radar": True, "radarTrackId": 100}
-
-  common = dict(
-    duration=73.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0,
-    distance_lead=100.0, v_lead=lead_speed, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
-  )
-  clean = _run(**common)
-  trace = _run(lead_observation_fn=observe, **common)
-  response = (trace.time >= 66.0) & (trace.time <= 72.0)
-  jerk_response = (trace.time[1:] >= 66.0) & (trace.time[1:] <= 72.0)
-  clean_gap = clean.distance_lead - clean.distance
-  gap = trace.distance_lead - trace.distance
-
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  assert not _has_brake_coast_brake(trace.a_target[response])
-  assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0
-  assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= max(PACE_TARGET_RESERVE, MATCHED_PACE_DECEL_RATE * DT_MDL) + 1e-9
-  assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE
-  assert not trace.fcw.any()
-  assert trace.solver_failures == 0
-  assert trace.raw_radar_passthrough.all()
-  assert np.all(trace.mpc_calls == 1)
-
-
-@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
-def test_profile_ceiling_and_pace_stay_smooth_through_slot_switch_noise(profile):
-  glitch_start = 24.0
-  glitch_end = 28.0
-
-  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    if not glitch_start <= current_time < glitch_end:
-      return truth if lead_name == "leadOne" else None
-
-    selected_slot = "leadOne" if round(current_time / DT_MDL) % 2 == 0 else "leadTwo"
-    if lead_name != selected_slot:
-      return None
-    sign = 1.0 if lead_name == "leadOne" else -1.0
-    speed_offset = 0.25 * sign
-    return truth | {
-      "dRel": max(0.0, truth["dRel"] + 1.5 * sign),
-      "vLead": max(0.0, truth["vLead"] + speed_offset),
-      "vLeadK": max(0.0, truth["vLeadK"] + speed_offset),
-      "vRel": truth["vRel"] + speed_offset,
-      "aLeadK": 0.0,
-      "radar": True,
-      "radarTrackId": 100 if lead_name == "leadOne" else 200,
-    }
-
-  common = dict(
-    duration=32.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0,
-    distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25,
-  )
-  baseline = _run(**common)
-  trace = _run(lead_observation_fn=observe, **common)
-  glitch = (trace.time >= glitch_start) & (trace.time < glitch_end)
-  response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0)
-  effective_accel_max = trace.effective_accel_max[response]
-  selected_leads = trace.selected_lead[glitch]
-  finite_limits = np.isfinite(effective_accel_max)
-  stock_accel_max = np.asarray([get_max_accel(speed) for speed in trace.speed[response]])
-
-  assert set(selected_leads) == {0, 1}
-  assert np.count_nonzero(np.diff(selected_leads)) > 20
-  assert np.all(effective_accel_max[finite_limits] <= trace.profile_accel_max[response][finite_limits] + 1e-9)
-  assert np.all(effective_accel_max[finite_limits] <= stock_accel_max[finite_limits] + 1e-9)
-  assert np.max(trace.a_target[response]) > 0.2
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  assert not _has_brake_coast_brake(trace.a_target[response])
-  assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
-  assert trace.raw_radar_passthrough.all()
-  assert not trace.fcw.any()
-  _assert_no_new_solver_failures(trace, baseline)
-
-
-@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport"))
-def test_matched_lead_dropout_keeps_the_profile_acceleration_ceiling(profile):
-  dropout_start = 25.0
-  dropout_end = 25.15
-
-  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    if lead_name == "leadTwo" or dropout_start <= current_time < dropout_end:
-      return None
-    return truth
-
-  common = dict(
-    duration=28.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0,
-    distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25,
-  )
-  clean = _run(**common)
-  trace = _run(lead_observation_fn=observe, **common)
-  dropout = (trace.time >= dropout_start) & (trace.time <= dropout_end)
-  response = (trace.time >= dropout_start - 0.5) & (trace.time <= dropout_end + 0.75)
-
-  assert np.all(np.isfinite(trace.effective_accel_max[dropout]))
-  assert np.max(np.abs(trace.a_target[response] - clean.a_target[response])) < 0.08
-  assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  assert trace.raw_radar_passthrough.all()
-  assert np.all(trace.mpc_calls == 1)
-  assert not trace.fcw.any()
-  _assert_no_new_solver_failures(trace, clean)
-
-
-def test_low_speed_lead_stop_has_no_release_then_rebrake():
-  def lead_speed(current_time: float) -> float:
-    return max(0.0, 1.9 - 1.16 * current_time)
-
-  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None:
-    if lead_name == "leadTwo":
-      return None
-    moving = lead_speed(current_time) > 0.0
-    return truth | {"vLeadK": truth["vLeadK"] if moving else -0.01, "aLeadK": -1.16 if moving else 0.0,
-                    "radarTrackId": 7, "radar": True}
-
-  common = dict(
-    duration=6.0, profile=0, lead_relevancy=True, speed=4.5, distance_lead=18.0, v_lead=lead_speed,
-    v_cruise=23.056, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-  stop_hold = trace.state == int(AccelControllerState.stopHold)
-  stopped_response = trace.time >= trace.time[np.flatnonzero(trace.speed < 1e-3)[0]]
-
-  assert stop_hold.any()
-  assert np.max(trace.speed[stopped_response]) <= np.max(baseline.speed[stopped_response]) + 0.01
-  assert np.max(trace.a_target[stopped_response]) <= np.max(baseline.a_target[stopped_response]) + 0.05
-  assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
-  assert np.min(trace.a_target) >= np.min(baseline.a_target) - ROUTINE_GAP_TOLERANCE
-  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE
-  assert not trace.fcw.any()
-  assert trace.solver_failures == 0
-
-
-@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
-def test_high_speed_stopped_lead_approach_holds_the_completed_stop(actuator_delay, actuator_lag):
-  trace = _run(
-    duration=14.0, controller_enabled=True, profile=1, lead_relevancy=True, speed=20.0,
-    distance_lead=130.0, v_lead=0.0, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag,
-  )
-  stopped = np.flatnonzero(trace.speed < 0.05)
-  assert len(stopped)
-  after_stop = slice(stopped[0], None)
-  gap = trace.distance_lead - trace.distance
-
-  assert STOP_DISTANCE <= np.min(gap[after_stop]) <= 25.0
-  assert np.max(trace.speed[after_stop]) < 0.10
-  assert not _has_propulsion_brake_cycle(trace.a_target[after_stop])
-  assert trace.state[-1] == int(AccelControllerState.stopHold)
-  assert trace.raw_radar_passthrough.all()
-  assert np.all(trace.mpc_calls == 1)
-  assert not trace.fcw.any()
-  assert trace.solver_failures == 0
-
-
-def test_decelerating_moving_lead_is_stock_safe_without_propulsion_reversal():
-  def lead_speed(current_time: float) -> float:
-    if current_time < 2.0:
-      return 15.0
-    progress = min((current_time - 2.0) / 6.0, 1.0)
-    return 15.0 - 5.0 * (3.0 * progress**2 - 2.0 * progress**3)
-
-  common = dict(
-    duration=14.0, profile=1, lead_relevancy=True, speed=20.0, distance_lead=110.0,
-    v_lead=lead_speed, v_cruise=30.0, actuator_delay=0.20, actuator_lag=0.25,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-  response = trace.time >= 1.0
-  baseline_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95))
-  trace_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95))
-
-  assert not _has_brake_coast_brake(trace.a_target[response])
-  assert not _has_propulsion_after_braking(trace.a_target[response])
-  assert trace_p95 <= baseline_p95 + 0.02
-  assert np.min(trace.acceleration) >= np.min(baseline.acceleration) - ROUTINE_DECEL_TOLERANCE
-  assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - MOVING_LEAD_GAP_TOLERANCE
-  assert not trace.fcw.any()
-  _assert_no_new_solver_failures(trace, baseline)
-
-
-def test_severe_closing_never_delays_stock_braking_or_reduces_clearance():
-  common = dict(
-    duration=12.0, lead_relevancy=True, speed=20.0, distance_lead=160.0, v_lead=3.5,
-    actuator_delay=0.20, actuator_lag=0.20,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-  for threshold in (-1.0, -2.0):
-    assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9
-
-  baseline_gap = baseline.distance_lead - baseline.distance
-  controlled_gap = trace.distance_lead - trace.distance
-  baseline_closing = baseline.speed - 3.5
-  controlled_closing = trace.speed - 3.5
-  baseline_ttc = np.min(baseline_gap[baseline_closing > 0.1] / baseline_closing[baseline_closing > 0.1])
-  controlled_ttc = np.min(controlled_gap[controlled_closing > 0.1] / controlled_closing[controlled_closing > 0.1])
-  onset = (trace.time[1:] > 0.5) & (trace.time[1:] < 3.0)
-
-  assert np.min(controlled_gap) >= np.min(baseline_gap) - 0.02
-  assert controlled_ttc >= baseline_ttc - 0.02
-  assert np.min(controlled_gap) > 0.0
-  assert np.max(np.abs(np.diff(trace.a_target)[onset] / DT_MDL)) < 4.0
-  assert trace.solver_failures == 0
-
-
-@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS)
-def test_far_lead_profiles_start_early_in_order_without_solver_failures(actuator_delay, actuator_lag):
-  common = dict(
-    duration=11.0, lead_relevancy=True, speed=25.0, distance_lead=200.0, v_lead=15.0,
-    actuator_delay=actuator_delay, actuator_lag=actuator_lag,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  traces = [_run(controller_enabled=True, profile=profile, **common) for profile in range(3)]
-  baseline_onset = _sustained_time_below(baseline, -0.10)
-  baseline_jerk_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95))
-  required_improvement = max(0.002, 0.02 * baseline_jerk_p95)
-  onsets = [_sustained_time_below(trace, -0.10) for trace in traces]
-
-  assert onsets[0] <= baseline_onset - 0.5 + 1e-9
-  assert onsets[1] <= baseline_onset + 1e-9
-  assert onsets[2] <= baseline_onset + 1e-9
-  assert onsets[0] <= onsets[1] + DT_MDL + 1e-9
-  assert onsets[1] <= onsets[2] + DT_MDL + 1e-9
-  first_finite_caps = [trace.raw_cap[np.flatnonzero(np.isfinite(trace.raw_cap))[0]] for trace in traces]
-  assert first_finite_caps[0] < first_finite_caps[1] < first_finite_caps[2]
-  for trace in traces:
-    assert trace.acceleration.min() >= baseline.acceleration.min() - 0.1
-    assert float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) <= baseline_jerk_p95 - required_improvement
-    assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 1.0
-    assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0])
-    assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 1.0])
-    assert not trace.fcw.any()
-    assert trace.solver_failures == 0
-
-
-def test_steady_slow_lead_has_no_gas_brake_cycle():
-  duration = 60.0
-  lead_speed = 10.0
-  common = dict(
-    duration=duration, lead_relevancy=True, speed=20.0, distance_lead=100.0, v_lead=lead_speed,
-    v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25,
-  )
-  baseline = _run(controller_enabled=False, **common)
-  trace = _run(controller_enabled=True, **common)
-  response = trace.time >= 1.0
-  settled = trace.time >= duration - 5.0
-  desired_gap = STOP_DISTANCE + get_T_FOLLOW() * lead_speed
-  baseline_gap = baseline.distance_lead - baseline.distance
-  gap = trace.distance_lead - trace.distance
-  max_settled_gap = max(np.mean(baseline_gap[settled]) + 10.0, desired_gap + 30.0)
-
-  assert np.mean(trace.speed[settled]) >= lead_speed - 2.0
-  assert np.mean(gap[settled]) <= max_settled_gap
-  assert not _has_brake_coast_brake(trace.a_target[response])
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  assert np.min(gap) >= desired_gap - 1.6
-  assert np.min(trace.a_target) >= np.min(baseline.a_target) - ROUTINE_GAP_TOLERANCE
-  _assert_no_new_solver_failures(trace, baseline)
-
-
-def test_matched_lead_slowdown_stays_smooth_without_a_second_braking_stage():
-  slowdown_time = 70.0
-  settled_lead_speed = 7.0
-
-  def lead_speed(current_time: float) -> float:
-    return 10.0 if current_time < slowdown_time else max(settled_lead_speed, 10.0 - 0.5 * (current_time - slowdown_time))
-
-  trace = _run(
-    duration=100.0, controller_enabled=True, profile=1, lead_relevancy=True, speed=20.0,
-    distance_lead=100.0, v_lead=lead_speed, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25,
-  )
-  matched = (trace.time >= slowdown_time - 5.0) & (trace.time < slowdown_time)
-  response = trace.time >= slowdown_time
-  settled = trace.time >= 95.0
-  gap = trace.distance_lead - trace.distance
-  desired_gap = STOP_DISTANCE + get_T_FOLLOW() * settled_lead_speed
-
-  assert abs(np.mean(trace.speed[matched]) - 10.0) < 0.5
-  np.testing.assert_allclose(trace.effective_accel_max[matched], trace.profile_accel_max[matched], atol=1e-9)
-  assert not np.any(trace.state[response] == int(AccelControllerState.stopHold))
-  assert not trace.launching[response].any()
-  assert not _has_brake_coast_brake(trace.a_target[response])
-  assert not _has_propulsion_brake_cycle(trace.a_target[response])
-  assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
-  assert float(np.percentile(np.abs(_filtered_realized_jerk(trace, after=slowdown_time)), 95)) < 0.30
-  assert np.min(gap) > STOP_DISTANCE
-  assert np.mean(trace.speed[settled]) >= settled_lead_speed - 1.5
-  assert np.mean(gap[settled]) <= desired_gap + 20.0
-  assert not trace.fcw.any()
-  assert trace.solver_failures == 0
-
-
-def test_acceleration_output_remains_inside_stock_limits():
-  trace = _run(
-    duration=12.0, controller_enabled=True, profile=AccelProfile.sport, lead_relevancy=False,
-    speed=0.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.20,
-  )
-  stock_max = np.asarray([get_max_accel(speed) for speed in trace.speed])
-  assert np.all(trace.a_target >= ACCEL_MIN - 1e-9)
-  assert np.all(trace.a_target <= stock_max + 0.06)
-  assert trace.stock_bounds_valid.all()
-  assert trace.solver_failures == 0

sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -5,8 +5,6 @@ from openpilot.common.params import Params
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
-from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
-
 
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
@@ -109,11 +107,7 @@ def set_lane_turn_params():
 ])
 def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
   dh = DesireHelper()
-  relc = RoadEdgeLaneChangeController(dh)
-  relc.enabled = True
   dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
   for _ in range(10):
-    dh.update(carstate, lateral_active, lane_change_prob,
-              left_edge_detected=relc.left_edge_detected, right_edge_detected=relc.right_edge_detected)
+    dh.update(carstate, lateral_active, lane_change_prob)
   assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
-

sunnypilot/selfdrive/controls/lib/tests/test_relc.py

@@ -1,99 +0,0 @@
-"""
-Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
-
-This file is part of sunnypilot and is licensed under the MIT License.
-See the LICENSE.md file in the root directory for more details.
-"""
-import pytest
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
-  RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
-)
-
-V_HIGH = MIN_SPEED + 2.0
-V_LOW = MIN_SPEED - 1.0
-
-
-@pytest.fixture
-def relc(mocker):
-  mock_params = mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
-  mock_params.return_value.get_bool.return_value = True
-  controller = RoadEdgeLaneChangeController(DesireHelper())
-  controller.enabled = True
-  return controller
-
-
-def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH):
-  for _ in range(int(seconds / DT_MDL) + 1):
-    controller.update(road_edge_stds, lane_line_probs, v_ego)
-
-
-@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
-  ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
-  ([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
-])
-def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
-  drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
-  assert getattr(relc, attr)
-
-
-def test_edge_detection_requires_time(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
-  assert not relc.left_edge_detected
-
-
-def test_both_edges_detected(relc):
-  drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-  assert relc.right_edge_detected
-
-
-def test_noise_doesnt_clear(relc):
-  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
-  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
-
-  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  relc.update(*clear, V_HIGH)
-  relc.update(*edge, V_HIGH)
-  assert relc.left_edge_detected
-
-
-def test_clears_after_window(relc):
-  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
-  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
-
-  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
-  assert not relc.left_edge_detected
-  assert relc.left_edge_timer == 0.0
-
-
-def test_low_speed_skips(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
-  assert not relc.left_edge_detected
-  assert relc.left_edge_timer == 0.0
-
-
-def test_speed_drop_resets(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW)
-  assert not relc.left_edge_detected
-
-
-def test_param_off_resets(relc):
-  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
-  assert relc.left_edge_detected
-
-  relc.params.get_bool.return_value = False
-  relc.read_params()
-  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH)
-  assert not relc.left_edge_detected
-  assert not relc.right_edge_detected

sunnypilot/selfdrive/selfdrived/events.py

@@ -243,12 +243,4 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
-
-  EventNameSP.laneChangeRoadEdge: {
-    ET.WARNING: Alert(
-      "Lane Change Unavailable: Road Edge",
-      "",
-      AlertStatus.userPrompt, AlertSize.small,
-      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
-  },
 }

sunnypilot/sunnylink/params_metadata.json

@@ -1,26 +1,4 @@
 {
-  "AccelPersonality": {
-    "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"
-      }
-    ]
-  },
-  "AccelPersonalityEnabled": {
-    "title": "Enable Accel Controller",
-    "description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
-  },
   "AccessToken": {
     "title": "AccessTokenIsNice",
     "description": ""
@@ -1140,10 +1118,6 @@
     "title": "Record Front Lock",
     "description": ""
   },
-  "RoadEdgeLaneChangeEnabled": {
-    "title": "Block Lane Change: Road Edge Detection",
-    "description": ""
-  },
   "RoadName": {
     "title": "Road Name",
     "description": ""
@@ -1349,22 +1323,6 @@
     "max": 5.0,
     "step": 0.1,
     "unit": "m/s\u00b2"
-  },
-    "ToyotaAutoHold": {
-    "title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
-    "description": ""
-  },
-    "ToyotaEnhancedBsm": {
-    "title": "Toyota: Prius TSS2 BSM and some tssp",
-    "description": ""
-  },
-    "ToyotaTSS2Long": {
-    "title": "Toyota: custom longitudinal for TSS2",
-    "description": ""
-  },
-    "ToyotaDriveMode": {
-    "title": "Enable drive mode btn link",
-    "description": ""
   },
   "ToyotaEnforceStockLongitudinal": {
     "title": "Toyota: Enforce Factory Longitudinal Control",

sunnypilot/sunnylink/settings_ui.json

@@ -620,58 +620,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",
@@ -2053,22 +2001,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
-                }
-              ]
             }
           ]
         },
@@ -2236,50 +2168,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",

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)

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

sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml

@@ -60,30 +60,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

sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -272,22 +272,6 @@ class TestKnownPanels:
     nnlc_enable_keys = {r.get("key") for r in nnlc.get("enablement", []) if r.get("type") == "param"}
     assert "EnforceTorqueControl" in nnlc_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):

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
@@ -99,7 +98,7 @@ class GuiScrollPanel2:
       # simple exponential return if out of bounds
       # out of bounds is handled by snapping, so skip if set
       out_of_bounds = self.get_offset() > max_offset or self.get_offset() < min_offset
-      if out_of_bounds and snap_target is None and self._handle_out_of_bounds:
+      if out_of_bounds and snap_target is None:
         target = max_offset if self.get_offset() > max_offset else min_offset
 
         dt = rl.get_frame_time() or 1e-6

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,12 +191,8 @@ 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
-      if self._horizontal:
-        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)
-      else:
-        center_pos = self._rect.y + self._rect.height / 2
-        closest_delta_pos = min((((item.rect.y + item.rect.height / 2) - 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)