fix hem

selfdrive/controls/lib/longitudinal_planner.py

@@ -2312,6 +2312,7 @@ class LongitudinalPlanner:
         model_v2=sm['modelV2'],
         a_chill=output_a_target_mpc,
         a_exp=output_a_target_e2e,
+        t_follow=effective_t_follow,
       )
       output_should_stop = output_should_stop_mpc or output_should_stop_e2e
     elif tinygrad_model and self.mode != 'acc' and self.generation != 'v9':

selfdrive/controls/tests/test_starpilot_planner.py

@@ -175,7 +175,7 @@ def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monke
     planner.shutdown()
 
 
-def test_hybrid_mode_runs_continuous_controller(monkeypatch):
+def test_hybrid_mode_keeps_cem_detector_warm_with_modes_off(monkeypatch):
   planner = make_planner(monkeypatch)
   monkeypatch.setattr(planner.starpilot_cem, "update", lambda *args, **kwargs: None)
 
@@ -184,8 +184,6 @@ def test_hybrid_mode_runs_continuous_controller(monkeypatch):
 
     planner.update(0.0, False, make_sm(planner, frame=1, v_ego=20.0, left_blinker=False), toggles)
 
-    assert planner.hybrid_controller is not None
-    assert planner.hybrid_acceleration != 0.0
     assert planner.starpilot_ccm.experimental_mode is False
     assert planner.starpilot_cem.experimental_mode is False
   finally:

starpilot/common/assets/device_settings_layout.json

@@ -2031,8 +2031,8 @@
       },
       {
         "key": "HybridExperimental",
-        "label": "Continuous Hybrid Control",
-        "description": "Fuse the crisp throttle response of classical ACC with the model's early, natural vision braking using a single continuous controller. Classical ACC always provides the hard safety distance, while the model's vision trajectory is blended in for early stops. Unconfirmed vision braking is capped to a gentle coast, and vision is never allowed to under-brake.",
+        "label": "Hybrid Experimental Mode",
+        "description": "Fuse the throttle response of chill ACC with the model's early, natural vision braking using a single continuous controller. Chill ACC always provides the hard safety distance, while the E2E trajectory is blended in for early stops. Unconfirmed vision braking is capped to a gentle coast, and vision is never allowed to under-brake.",
         "picker_description": "Continuously blends Chill ACC with the model's vision braking.",
         "data_type": "bool",
         "ui_type": "toggle",

starpilot/controls/lib/hybrid_experimental_mode.py

@@ -25,17 +25,18 @@ def smooth_max(a: float, b: float, k: float = 6.0) -> float:
 class HybridExperimentalMode:
   """
   Fuses Chill Mode (radar/lead tracking) and Experimental Mode (vision/stop signs/lights):
-  1. Detects vision stopping intent from trajectory v_min and distance horizon.
+  1. Detects vision stopping intent from trajectory drop and horizon endpoints.
   2. Calculates kinematically required stopping deceleration (-v^2 / 2d).
-  3. Clamps Chill positive throttle during stop events to prevent brake fighting.
-  4. Holds 0 m/s at standstills to prevent creep.
+  3. Clamps Chill positive throttle during stop events to prevent brake fighting/dilution.
+  4. Holds brake at standstills to prevent creep, but releases immediately on green lights or gas tap.
   5. Falls back safely to Chill if lead vehicle distance is compromised.
-  6. Slew-rate limits acceleration to respect vehicle jerk limits.
+  6. Slew-rate limits acceleration to respect vehicle jerk limits, with emergency bypass.
   """
 
-  # Base physical actuator jerk limits (m/s^3)
+  # Physical actuator jerk limits (m/s^3)
   BASE_MAX_JERK_BRAKE = 3.5
   BASE_MAX_JERK_ACCEL = 5.5
+  EMERGENCY_JERK_BRAKE = 14.0
 
   # Base safety floor parameters
   BASE_T_FOLLOW = 1.45
@@ -55,8 +56,9 @@ class HybridExperimentalMode:
     self.jerk_factor = 1.0
     self._update_profile_limits(self.t_follow, self.jerk_factor)
 
-  def reset(self, a: float = 0.0):
-    self.prev_a_target = float(a)
+  def reset(self, a_ego: float = 0.0):
+    """Seed target with actual vehicle acceleration on engagement to prevent torque bumps."""
+    self.prev_a_target = float(a_ego) if np.isfinite(a_ego) else 0.0
     self.exp_authority = 0.5
 
   def set_tuning(self, exp_bias: float, vision_brake_sensitivity: float, t_follow=None, jerk_factor=None):
@@ -84,7 +86,10 @@ class HybridExperimentalMode:
     traj_v = getattr(velocity, "x", None) if velocity is not None else None
     if traj_v is None or len(traj_v) == 0:
       return np.array([v_ego], dtype=float)
-    return np.asarray(traj_v, dtype=float)
+    traj_v = np.asarray(traj_v, dtype=float)
+    if not np.all(np.isfinite(traj_v)):
+      return np.array([v_ego], dtype=float)
+    return traj_v
 
   @staticmethod
   def _get_model_trajectory_x(model_v2) -> np.ndarray:
@@ -92,7 +97,10 @@ class HybridExperimentalMode:
     traj_x = getattr(position, "x", None) if position is not None else None
     if traj_x is None or len(traj_x) == 0:
       return np.array([], dtype=float)
-    return np.asarray(traj_x, dtype=float)
+    traj_x = np.asarray(traj_x, dtype=float)
+    if not np.all(np.isfinite(traj_x)):
+      return np.array([], dtype=float)
+    return traj_x
 
   def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp,
              t_follow=None, jerk_factor=None):
@@ -101,6 +109,11 @@ class HybridExperimentalMode:
        (jerk_factor is not None and abs(jerk_factor - self.jerk_factor) > 1e-4):
       self._update_profile_limits(t_follow, jerk_factor)
 
+    if not np.isfinite(a_chill):
+      a_chill = float(self.prev_a_target)
+    if not np.isfinite(a_exp):
+      a_exp = a_chill
+
     lead_status = bool(getattr(lead_one, "status", False))
     lead_d_rel = float(getattr(lead_one, "dRel", 150.0))
 
@@ -108,53 +121,80 @@ class HybridExperimentalMode:
     traj_v = self._get_model_trajectory_v(model_v2, v_ego)
     traj_x = self._get_model_trajectory_x(model_v2)
 
+    v_min = float(np.min(traj_v))
     min_idx = int(np.argmin(traj_v))
-    v_min = float(traj_v[min_idx])
-    d_min = float(traj_x[min_idx]) if len(traj_x) > min_idx else 100.0
+    v_horizon = float(traj_v[-1]) if len(traj_v) > 0 else v_ego
     v_ref = max(v_ego, 2.0)
 
-    # Detect drop anywhere along trajectory (stop sign / red light profile)
+    # Detect deceleration profile or low-speed stop line target
     speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref)
-    is_stopping_profile = sigmoid(1.5 - v_min, k=4.0, x0=0.0) * sigmoid(v_ego, k=3.0, x0=1.0)
-    model_decel_strength = max(0.0, -a_exp / 2.5)
+    stop_target_active = sigmoid(1.2 - v_horizon, k=4.0, x0=0.0)
+    model_decel_strength = max(0.0, -a_exp / 2.0)
 
-    raw_vision_metric = max(speed_drop_ratio, is_stopping_profile, model_decel_strength)
+    raw_vision_metric = max(speed_drop_ratio, stop_target_active, model_decel_strength)
     w_vision = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0))
 
-    # Calculate actual kinematic braking required to stop at the detected stop line
-    if v_min < 1.2 and v_ego > 1.0 and d_min > 0.5:
-      # Kinematic decel: -v^2 / (2 * d) with 2.0m stop line cushion
-      d_stop_effective = max(d_min - 2.0, 1.5)
-      a_kinematic_stop = - (v_ego ** 2) / (2.0 * d_stop_effective)
+    # Kinematic stopping calculation when approaching a stop line
+    if v_min < 1.2 and v_horizon < 2.0 and v_ego > 0.1 and len(traj_x) > min_idx and traj_x[min_idx] > 0.2:
+      d_min = float(traj_x[min_idx])
+      d_stop_effective = max(d_min - 1.5, 2.0)
+      a_kinematic_stop = float(np.clip(- (v_ego ** 2) / (2.0 * d_stop_effective), -3.5, 0.0))
+      if d_stop_effective < 6.0 and v_ego < 3.0:
+        a_kinematic_stop = min(a_kinematic_stop, -0.6)
       a_exp_effective = min(a_exp, a_kinematic_stop)
+    elif v_horizon < 1.0 and v_ego > 0.05:
+      # Final roll-in: enforce negative acceleration to complete stop
+      a_exp_effective = min(a_exp, -0.5)
     else:
       a_exp_effective = a_exp
 
-    # Dynamic Exp Authority: scales directly to 100% when vision intent is high
     base_auth = float(np.clip(0.5 + (0.35 * self.HYBRID_EXP_BIAS), 0.0, 1.0))
     alpha_exp = lerp(base_auth, 1.0, w_vision)
     self.exp_authority = alpha_exp
 
     # 2. ACCELERATION FUSION (Throttle vs Braking Regimes)
-    # Throttle Regime: Snappy pickup on open roads
-    a_throttle_optimal = smooth_max(a_chill, a_exp, k=4.0)
+    # Throttle Regime: Snappy pickup on open roads with clean cruise setpoint clamp
+    a_throttle_raw = smooth_max(a_chill, a_exp, k=4.0)
+    if v_ego >= v_cruise:
+      a_throttle_optimal = min(a_throttle_raw, a_chill)
+    else:
+      overshoot_risk = sigmoid(v_ego - v_cruise, k=4.0, x0=-1.0)
+      a_throttle_capped = min(a_throttle_raw, max(0.0, a_chill))
+      a_throttle_optimal = lerp(a_throttle_raw, a_throttle_capped, overshoot_risk)
+
     a_throttle_conservative = smooth_min(a_chill, a_exp, k=4.0)
     a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision)
 
-    # Braking Regime: CLAMP Chill to <= 0 so cruise throttle cannot fight the stop!
-    a_chill_brake = min(a_chill, 0.0)
-    a_brake_fused = lerp(a_chill_brake, a_exp_effective, alpha_exp)
+    # Braking Regime: Never dilute Exp stop braking with Chill's 0.0 m/s^2
+    if a_exp_effective < 0.0:
+      a_chill_brake = min(a_chill, 0.0)
+      a_brake_fused = min(a_exp_effective, a_chill_brake) if a_chill_brake < a_exp_effective \
+        else lerp(a_exp_effective, a_chill_brake, 1.0 - alpha_exp)
+    else:
+      a_brake_fused = min(a_chill, a_exp_effective)
+
+    # Regime Selection: If vision sees a stop or braking is requested, lock out positive throttle
+    is_braking_phase = (w_vision > 0.3) or (a_exp_effective < -0.2) or (a_chill < -0.2)
+    if is_braking_phase:
+      w_accel = 0.0
+    else:
+      phase_metric = smooth_min(a_chill, a_exp, k=4.0)
+      w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1) * (1.0 - w_vision)
 
-    # Regime Selection: If vision sees a stop (w_vision -> 1), FORCE braking regime (w_accel -> 0)
-    phase_metric = smooth_min(a_chill, a_exp, k=4.0)
-    w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1) * (1.0 - w_vision)
     a_fused = lerp(a_brake_fused, a_throttle_fused, w_accel)
 
-    # 3. STANDSTILL ANCHOR (Prevent creeping at 0 mph)
+    # 3. STANDSTILL ANCHOR (Hold at 0 mph, release cleanly on green departure or gas tap)
     is_stopped = sigmoid(0.4 - v_ego, k=8.0, x0=0.0)
-    is_min_stopped = sigmoid(0.8 - v_min, k=4.0, x0=0.0)
-    standstill_weight = is_stopped * is_min_stopped
-    a_anchored = lerp(a_fused, smooth_min(a_fused, 0.0, k=8.0), standstill_weight)
+    is_staying_stopped = sigmoid(0.5 - v_horizon, k=6.0, x0=0.0)
+    lead_departing = lead_status and (getattr(lead_one, "vLead", 0.0) > 0.5)
+    vision_departing = (v_horizon > 0.5) and (a_exp > 0.1)
+    driver_departing = (a_chill > 0.4) and (not lead_status or lead_d_rel > 10.0)
+    # A real trajectory still predicting a stop line (red light / stop sign) keeps
+    # the anchor engaged: cruise creep must not release the brake at a stop it can't see.
+    model_stop_predicted = len(traj_v) > 1 and v_horizon < 0.5
+    departing = (lead_departing or vision_departing or driver_departing) and not model_stop_predicted
+    standstill_weight = (0.0 if departing else 1.0) * is_stopped * is_staying_stopped
+    a_anchored = lerp(a_fused, smooth_min(a_fused, -0.5, k=6.0), standstill_weight)
 
     # 4. SAFETY BARRIER (Lead Vehicle Proximity Check)
     d_static_effective = self.D_STATIC_SAFE + max(0.0, 1.5 * (1.0 - (v_ego / 4.0)))
@@ -163,12 +203,25 @@ class HybridExperimentalMode:
     distance_ratio = (lead_d_rel - d_static_effective) / max(d_safe - d_static_effective, 1.0)
     lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0, x0=0.0) * float(lead_status)
 
-    a_emergency_brake = smooth_min(a_anchored, a_chill, k=6.0)
-    a_safe = lerp(a_anchored, a_emergency_brake, lead_safety_risk)
+    # Enforce hard ceiling when lead is within safety envelope or Chill is braking for lead
+    lead_safety_active = lead_status and (lead_d_rel < d_safe or a_chill < 0.0)
+    if lead_safety_active and a_chill < a_anchored:
+      a_safe = min(a_anchored, a_chill)
+    else:
+      a_safe = a_anchored
+
+    # 5. ASYMMETRIC DIRECTIONAL SLEW FILTER (Limit Jerk)
+    da = a_safe - self.prev_a_target
+    if da >= 0.0:
+      jerk_limit = self.MAX_JERK_ACCEL
+    else:
+      # Bypass comfort brake rate for lead collision or committed vision-stop emergencies.
+      is_lead_emergency = (lead_safety_risk > 0.5) and (a_chill < -1.5)
+      is_vision_emergency = (w_vision > 0.7) and (a_exp < -2.0)
+      jerk_limit = self.EMERGENCY_JERK_BRAKE if (is_lead_emergency or is_vision_emergency) else self.MAX_JERK_BRAKE
 
-    # 5. ASYMMETRIC SLEW FILTER (Limit Jerk)
-    jerk_limit = self.MAX_JERK_ACCEL if a_safe >= self.prev_a_target else self.MAX_JERK_BRAKE
     max_delta = jerk_limit * self.DT
-
-    self.prev_a_target = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta))
+    a_out = float(np.clip(a_safe, self.prev_a_target - max_delta, self.prev_a_target + max_delta))
+    if np.isfinite(a_out):
+      self.prev_a_target = a_out
     return self.prev_a_target

starpilot/controls/starpilot_card.py

@@ -123,6 +123,9 @@ class StarPilotCard:
     if getattr(starpilot_toggles, "safe_mode", False):
       return
 
+    if getattr(starpilot_toggles, "hybrid_experimental_mode", False):
+      return
+
     if starpilot_toggles.conditional_experimental_mode:
       current_status = self.params_memory.get_int("CEStatus", default=CEStatus["OFF"])
       override_value = next_manual_ce_status(current_status, sm["selfdriveState"].experimentalMode)

starpilot/controls/starpilot_planner.py

@@ -24,7 +24,6 @@ from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width,
 from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD
 from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
 from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
-from openpilot.starpilot.controls.lib.hybrid_experimental_mode import HybridExperimentalMode
 from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration
 from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents
 from openpilot.starpilot.controls.lib.starpilot_following import StarPilotFollowing
@@ -69,7 +68,6 @@ class StarPilotPlanner:
     self.starpilot_acceleration = StarPilotAcceleration(self)
     self.starpilot_cem = ConditionalExperimentalMode(self)
     self.starpilot_ccm = ConditionalChillMode(self, self.starpilot_cem)
-    self.hybrid_controller = HybridExperimentalMode()
     self.starpilot_events = StarPilotEvents(self, error_log, ThemeManager)
     self.starpilot_following = StarPilotFollowing(self)
     self.starpilot_vcruise = StarPilotVCruise(self)
@@ -102,7 +100,6 @@ class StarPilotPlanner:
     self.road_curvature = 0
     self.time_to_curve = 0
     self.v_cruise = 0
-    self.hybrid_acceleration = 0.0
 
     self.gps_position = None
 
@@ -224,27 +221,9 @@ class StarPilotPlanner:
 
     conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled
     if conditional_tracking_active and bool(getattr(starpilot_toggles, "hybrid_experimental_mode", False)):
-      # Continuous Hybrid Experimental Control: instead of a binary CEM/CCM mode
-      # switch, fuse the classical Chill ACC target with the model's E2E
-      # trajectory through the HybridExperimentalMode controller. CEM's detector
-      # stays warm so red-light/stop-sign scene state (redLight, forcing_stop)
-      # remains accurate. Experimental Mode stays off so the MPC keeps producing
-      # the crisp classical a_chill that the hybrid is built on.
       self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise)
       self.starpilot_ccm.experimental_mode = False
       self.starpilot_cem.experimental_mode = False
-      self.hybrid_controller.set_tuning(
-        getattr(starpilot_toggles, "hybrid_exp_bias", 0.0),
-        getattr(starpilot_toggles, "hybrid_vision_brake_sensitivity", 1.0),
-      )
-      self.hybrid_acceleration = self.hybrid_controller.update(
-        v_ego=v_ego,
-        v_cruise=v_cruise,
-        lead_one=self.lead_one,
-        model_v2=sm["modelV2"],
-        a_chill=self._get_chill_accel(v_ego, v_cruise),
-        a_exp=self._get_vision_exp_accel(sm),
-      )
     elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)):
       # Keep CEM's filters warm in AOL so engagement can inherit the current scene.
       self.starpilot_cem.update(v_ego, sm, starpilot_toggles, v_cruise)
@@ -321,40 +300,6 @@ class StarPilotPlanner:
     self.tracking_lead_filter.update(following_lead)
     return self.tracking_lead_filter.x >= THRESHOLD
 
-  def _get_vision_exp_accel(self, sm):
-    """Model's raw predicted acceleration for the E2E (Experimental) channel."""
-    try:
-      accel_x = sm["modelV2"].acceleration.x
-      if len(accel_x) > 0:
-        return float(accel_x[0])
-    except (AttributeError, IndexError, TypeError):
-      pass
-    try:
-      return float(sm["modelV2"].action.desiredAcceleration)
-    except (AttributeError, TypeError, ValueError):
-      return 0.0
-
-  def _get_chill_accel(self, v_ego, v_cruise):
-    """Cruise/lead tracking acceleration for the Chill (ACC) channel."""
-    max_accel = float(getattr(self.starpilot_acceleration, "max_accel", 0.0) or 0.0)
-    min_accel = float(getattr(self.starpilot_acceleration, "min_accel", 0.0) or 0.0)
-    if max_accel <= 0.0 and min_accel == 0.0:
-      max_accel, min_accel = 1.5, -2.0
-
-    a_chill = float(np.clip((v_cruise - v_ego) / 2.0, min_accel, max_accel))
-    lead = getattr(self, "lead_one", None)
-    if lead is not None and bool(getattr(lead, "status", False)):
-      d_rel = float(getattr(lead, "dRel", float("inf")))
-      v_lead = float(getattr(lead, "vLead", v_ego))
-      t_follow = float(getattr(self.starpilot_following, "t_follow", 1.45) or 1.45)
-      desired_gap = max(v_ego * t_follow, 4.0)
-      if d_rel < desired_gap:
-        gap_deficit = max(desired_gap - d_rel, 0.0)
-        closing = max(0.0, v_ego - v_lead)
-        lead_decel = -min(closing / 2.0 + gap_deficit / 4.0, 3.0)
-        a_chill = min(a_chill, lead_decel)
-    return float(np.clip(a_chill, min_accel, max_accel))
-
   def publish(self, theme_updated, sm, pm, starpilot_toggles, serialized_toggles=""):
     starpilot_plan_send = messaging.new_message("starpilotPlan")
     starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"])

starpilot/controls/tests/test_hybrid_experimental_mode.py

@@ -1,24 +1,27 @@
+#!/usr/bin/env python3
 import numpy as np
 import pytest
 
 from openpilot.starpilot.controls.lib.hybrid_experimental_mode import (
   HybridExperimentalMode,
+  lerp,
   sigmoid,
-  soft_max,
-  soft_min,
+  smooth_max,
+  smooth_min,
 )
 
 
 class FakeLead:
   def __init__(self, status=False, d_rel=150.0, v_lead=0.0):
     self.status = status
-    self.dRel = d_rel
-    self.vLead = v_lead
+    self.dRel = float(d_rel)
+    self.vLead = float(v_lead)
 
 
 class FakeModel:
-  def __init__(self, velocity=None):
-    self.velocity = type("Velocity", (), {"x": velocity})()
+  def __init__(self, velocity=None, position=None):
+    self.velocity = type("Velocity", (), {"x": list(velocity)})() if velocity is not None else None
+    self.position = type("Position", (), {"x": list(position)})() if position is not None else None
 
 
 def make_controller(prev=0.0):
@@ -29,81 +32,147 @@ def make_controller(prev=0.0):
 
 def run(controller, *, v_ego=20.0, v_cruise=30.0, lead=None, model=None, a_chill=0.5, a_exp=0.8, frames=80):
   lead = lead if lead is not None else FakeLead()
-  model = model if model is not None else FakeModel(velocity=[v_ego] * 20)
+  if model is None:
+    model = FakeModel(velocity=[v_ego] * 33, position=list(np.linspace(0.0, 100.0, 33)))
   result = 0.0
   for _ in range(frames):
     result = controller.update(v_ego, v_cruise, lead, model, a_chill, a_exp)
   return result
 
-
 def test_soft_operators_are_continuous_and_bounded():
   for value in (-5.0, -0.1, 0.0, 0.1, 5.0):
     assert 0.0 < sigmoid(value) < 1.0
-  assert soft_max(1.0, 2.0) == pytest.approx(2.0, abs=1e-2)
-  assert soft_min(1.0, 2.0) == pytest.approx(1.0, abs=1e-2)
-
+  assert smooth_max(1.0, 2.0) == pytest.approx(2.0, abs=1e-2)
+  assert smooth_min(1.0, 2.0) == pytest.approx(1.0, abs=1e-2)
+  assert lerp(10.0, 20.0, 0.5) == pytest.approx(15.0, abs=1e-3)
 
 def test_throttle_fusion_leans_toward_snappier_target():
   controller = make_controller()
-  # No lead, no vision stop: the continuous fusion routes toward the more
-  # confident throttle (exp 0.8) without a mode switch.
   a = run(controller, a_chill=0.5, a_exp=0.8)
-  assert a > 0.75
-  assert a < 0.85
+  assert 0.70 < a < 0.85
 
 
-def test_vision_stop_horizon_grants_full_braking():
+def test_throttle_fusion_conservative_when_chill_is_more_eager():
   controller = make_controller()
-  model = FakeModel(velocity=np.linspace(20.0, 0.1, 20))
-  # The model trajectory decays to a terminal stop, so w_vision -> 1 and the
-  # early vision braking curve takes over.
-  a = run(controller, lead=FakeLead(status=False), model=model, a_chill=-0.5, a_exp=-2.0)
-  assert a <= -1.5
+  a = run(controller, a_chill=1.2, a_exp=0.6)
+  assert a > 1.0
 
 
-def test_phantom_brake_shield_caps_unconfirmed_vision_braking():
+def test_throttle_fusion_smooth_cruise_speed_capping():
   controller = make_controller()
-  # No lead and a flat model horizon: unconfirmed vision braking is clamped to
-  # the gentle coast limit instead of passing the raw E2E target through.
-  a = run(controller, lead=FakeLead(status=False), a_chill=0.0, a_exp=-2.0)
-  assert -0.6 <= a <= -0.5
+  a = run(controller, v_ego=30.0, v_cruise=30.0, a_chill=-0.2, a_exp=1.0)
+  assert a <= 0.0, f"Must not accelerate past set cruise speed, got {a}"
+
+def test_green_light_departure_from_standstill():
+  controller = make_controller(prev=-0.5)
+  traj_v = np.linspace(0.0, 12.0, 33)
+  traj_x = np.linspace(0.0, 45.0, 33)
+  model = FakeModel(velocity=traj_v, position=traj_x)
+
+  a = run(controller, v_ego=0.0, v_cruise=20.0, model=model, a_chill=1.0, a_exp=1.5, frames=40)
+  assert a > 1.0, f"Vehicle should depart promptly on green light, got {a}"
 
 
-def test_cbf_safety_floor_prevents_under_braking_near_close_lead():
+def test_standstill_hold_at_red_light_without_lead():
+  controller = make_controller(prev=0.0)
+  traj_v = np.zeros(33)
+  traj_x = np.zeros(33)
+  model = FakeModel(velocity=traj_v, position=traj_x)
+
+  a = run(controller, v_ego=0.0, v_cruise=25.0, lead=FakeLead(status=False),
+          model=model, a_chill=0.2, a_exp=-1.0, frames=40)
+  assert a <= -0.4, f"Standstill brake must hold even when Chill cruise wants to go, got {a}"
+
+def test_red_light_high_speed_approach_braking():
+  controller = make_controller(prev=0.0)
+  traj_v = np.linspace(25.0, 0.0, 33)
+  traj_x = np.linspace(0.0, 60.0, 33)
+  model = FakeModel(velocity=traj_v, position=traj_x)
+
+  a = run(controller, v_ego=25.0, v_cruise=25.0, model=model, a_chill=0.5, a_exp=-2.0, frames=60)
+  assert a <= -2.0, f"Stopping deceleration should be fully honored without dilution, got {a}"
+
+
+def test_red_light_low_speed_roll_prevent_dilution():
+  controller = make_controller(prev=-0.5)
+  traj_v = np.linspace(0.8, 0.0, 33)
+  traj_x = np.linspace(0.0, 3.0, 33)
+  model = FakeModel(velocity=traj_v, position=traj_x)
+
+  a = run(controller, v_ego=0.8, v_cruise=20.0, model=model, a_chill=0.8, a_exp=-0.6, frames=30)
+  assert a <= -0.55, f"Kinematic stop clamp must enforce stopping bite at low speeds, got {a}"
+
+
+def test_kinematic_stopping_does_not_blow_up_on_close_dmin():
+  controller = make_controller(prev=0.0)
+  traj_v = np.zeros(33)
+  traj_x = np.ones(33) * 1.0
+  model = FakeModel(velocity=traj_v, position=traj_x)
+
+  a = controller.update(15.0, 20.0, FakeLead(), model, a_chill=0.0, a_exp=-1.0)
+  assert a >= -(controller.MAX_JERK_BRAKE * controller.DT + 1e-4)
+
+
+def test_kinematic_stopping_graceful_on_missing_position():
+  controller = make_controller(prev=0.0)
+  traj_v = np.linspace(10.0, 0.0, 33)
+  model = FakeModel(velocity=traj_v, position=[])
+
+  a = run(controller, v_ego=10.0, v_cruise=20.0, model=model, a_chill=0.5, a_exp=-2.0, frames=40)
+  assert a <= -1.8
+
+def test_radar_lead_safety_barrier_overrides_vision_throttle():
   controller = make_controller()
-  # Vision wants throttle but a close lead means the Control Barrier Function
-  # smoothly forces the target toward the Chill safety floor.
   lead = FakeLead(status=True, d_rel=5.0, v_lead=0.0)
-  a = run(controller, lead=lead, a_chill=-1.0, a_exp=0.5)
-  assert a == pytest.approx(-1.0, abs=1e-2)
+  model = FakeModel(velocity=[15.0] * 33, position=list(np.linspace(0.0, 100.0, 33)))
+
+  a = run(controller, v_ego=15.0, v_cruise=20.0, lead=lead, model=model, a_chill=-2.5, a_exp=1.0, frames=40)
+  assert a == pytest.approx(-2.5, abs=0.1), f"Safety barrier must enforce radar braking floor, got {a}"
 
 
-def test_cbf_is_inactive_when_lead_is_far():
+def test_radar_lead_safety_barrier_inactive_when_gap_is_safe():
   controller = make_controller()
   lead = FakeLead(status=True, d_rel=80.0, v_lead=25.0)
-  a = run(controller, lead=lead, a_chill=0.5, a_exp=0.8)
-  assert a > 0.7
+  model = FakeModel(velocity=[25.0] * 33, position=list(np.linspace(0.0, 150.0, 33)))
+
+  a = run(controller, v_ego=25.0, v_cruise=30.0, lead=lead, model=model, a_chill=0.6, a_exp=0.9, frames=40)
+  assert a > 0.75, f"Lead barrier should remain transparent when headway is safe, got {a}"
 
 
-def test_jerk_slew_limits_single_frame_step():
+def test_cut_in_emergency_braking_ramp_rate():
   controller = make_controller(prev=0.0)
-  lead = FakeLead(status=False)
-  model = FakeModel(velocity=[20.0] * 20)
-  a = controller.update(20.0, 30.0, lead, model, 2.0, 2.0)
-  assert a == pytest.approx(controller.MAX_JERK * controller.DT, abs=1e-3)
+  cut_in_lead = FakeLead(status=True, d_rel=6.0, v_lead=10.0)
+  model = FakeModel(velocity=[20.0] * 33)
+
+  for _ in range(8):
+    a = controller.update(20.0, 25.0, cut_in_lead, model, a_chill=-3.2, a_exp=0.0)
+  assert a <= -2.5, f"Emergency cut-in should ramp braking power quickly, got {a}"
+
+def test_asymmetric_jerk_slew_rates():
+  controller_accel = make_controller(prev=0.0)
+  a_up = controller_accel.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 4.0, 4.0)
+  assert a_up == pytest.approx(controller_accel.MAX_JERK_ACCEL * controller_accel.DT, abs=1e-3)
+
+  controller_brake = make_controller(prev=0.0)
+  a_down = controller_brake.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), -1.0, -1.0)
+  assert a_down == pytest.approx(-controller_brake.MAX_JERK_BRAKE * controller_brake.DT, abs=1e-3)
 
 
-def test_reset_clears_integrator_state():
-  controller = make_controller(prev=0.0)
-  lead = FakeLead(status=True, d_rel=20.0, v_lead=15.0)
-  model = FakeModel(velocity=[20.0] * 20)
-  controller.update(20.0, 30.0, lead, model, -1.0, -2.0)
-  assert controller.prev_a_target != 0.0
-  controller.reset(0.0)
-  assert controller.prev_a_target == 0.0
+def test_directional_brake_release_rate():
+  controller = make_controller(prev=-3.0)
+  a_rel = controller.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 0.0, 0.0)
+  expected = -3.0 + controller.MAX_JERK_ACCEL * controller.DT
+  assert a_rel == pytest.approx(expected, abs=1e-3)
 
 
-def test_set_tuning_clamps_to_documented_ranges():
+def test_profile_limits_dynamic_update():
+  controller = make_controller()
+  controller.update(20.0, 30.0, FakeLead(), FakeModel(velocity=[20.0] * 33), 0.5, 0.5, t_follow=1.1, jerk_factor=1.3)
+  assert controller.t_follow == pytest.approx(1.1, abs=1e-4)
+  assert controller.jerk_factor == pytest.approx(1.3, abs=1e-4)
+  assert controller.MAX_JERK_ACCEL == pytest.approx(5.5 * 1.3, abs=1e-3)
+
+def test_tuning_clamping():
   controller = make_controller()
   controller.set_tuning(5.0, 9.0)
   assert controller.HYBRID_EXP_BIAS == 1.0
@@ -111,15 +180,83 @@ def test_set_tuning_clamps_to_documented_ranges():
   controller.set_tuning(-5.0, -1.0)
   assert controller.HYBRID_EXP_BIAS == -1.0
   assert controller.VISION_BRAKE_SENSITIVITY == 0.0
-  controller.set_tuning(0.5, 1.5)
-  assert controller.HYBRID_EXP_BIAS == 0.5
-  assert controller.VISION_BRAKE_SENSITIVITY == 1.5
 
 
-def test_missing_model_velocity_falls_back_to_ego_speed():
+def test_reset_seeds_active_acceleration():
+  controller = make_controller(prev=0.0)
+  controller.reset(a_ego=-1.8)
+  assert controller.prev_a_target == -1.8
+
+
+def test_missing_or_corrupt_model_v2_fallbacks():
   controller = make_controller()
-  model = FakeModel(velocity=None)
-  lead = FakeLead(status=False)
-  # Falls back to a constant-velocity horizon, so no vision braking is injected.
-  a = run(controller, lead=lead, model=model, a_chill=0.5, a_exp=0.2)
-  assert a > 0.4
+  model_none = FakeModel(velocity=None, position=None)
+  a = run(controller, lead=FakeLead(status=False), model=model_none, a_chill=0.5, a_exp=0.2)
+  assert a > 0.35, "Controller should gracefully fallback when trajectory is None"
+
+  model_empty = FakeModel(velocity=[], position=[])
+  a = run(controller, lead=FakeLead(status=False), model=model_empty, a_chill=0.6, a_exp=0.7)
+  assert a > 0.55, "Controller should gracefully handle empty trajectory lists"
+
+
+def test_nan_a_exp_falls_back_to_chill_channel():
+  controller = make_controller(prev=0.0)
+  model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33)))
+  a = run(controller, model=model, a_chill=0.5, a_exp=float("nan"), frames=10)
+  assert np.isfinite(a), f"NaN a_exp must never propagate, got {a}"
+  assert 0.4 <= a <= 0.6, f"NaN a_exp should degrade to the Chill channel, got {a}"
+
+
+def test_inf_a_exp_does_not_propagate():
+  controller = make_controller(prev=0.0)
+  model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33)))
+  for a_exp in (float("inf"), float("-inf")):
+    controller.reset(0.0)
+    a = run(controller, model=model, a_chill=0.5, a_exp=a_exp, frames=10)
+    assert np.isfinite(a), f"Non-finite a_exp={a_exp} must not propagate, got {a}"
+
+
+def test_nan_a_chill_falls_back_to_last_target():
+  controller = make_controller(prev=0.0)
+  model = FakeModel(velocity=[20.0] * 33, position=list(np.linspace(0.0, 100.0, 33)))
+  a = run(controller, model=model, a_chill=float("nan"), a_exp=0.8, frames=10)
+  assert np.isfinite(a), f"NaN a_chill must never propagate, got {a}"
+
+
+def test_nan_in_trajectory_does_not_propagate():
+  controller = make_controller(prev=0.0)
+  v = [20.0] * 20 + [float("nan")] + [20.0] * 12
+  model = FakeModel(velocity=v, position=list(np.linspace(0.0, 100.0, 33)))
+  a = run(controller, model=model, a_chill=0.5, a_exp=0.8, frames=10)
+  assert np.isfinite(a), f"NaN trajectory must not propagate, got {a}"
+
+
+def test_reset_rejects_non_finite_seed():
+  controller = make_controller(prev=-1.5)
+  controller.reset(float("nan"))
+  assert controller.prev_a_target == 0.0
+
+
+def test_standstill_holds_without_model_brake_at_predicted_stop():
+  controller = make_controller(prev=0.0)
+  model = FakeModel(velocity=np.zeros(33), position=np.zeros(33))
+  a = run(controller, v_ego=0.0, v_cruise=25.0, lead=FakeLead(status=False),
+          model=model, a_chill=0.8, a_exp=0.0, frames=40)
+  assert a <= -0.4, f"Anchor must hold at a predicted stop even with cruise creep, got {a}"
+
+
+def test_green_light_departure_releases_anchor():
+  controller = make_controller(prev=-0.5)
+  traj_v = np.linspace(0.0, 12.0, 33)
+  traj_x = np.linspace(0.0, 45.0, 33)
+  model = FakeModel(velocity=traj_v, position=traj_x)
+  a = run(controller, v_ego=0.0, v_cruise=20.0, lead=FakeLead(status=False),
+          model=model, a_chill=1.0, a_exp=1.5, frames=40)
+  assert a > 1.0, f"Green light departure must still release the anchor, got {a}"
+
+
+def test_vision_stop_uses_emergency_brake_ramp():
+  controller = make_controller(prev=0.0)
+  model = FakeModel(velocity=list(np.linspace(20.0, 0.2, 33)), position=list(np.linspace(0.0, 40.0, 33)))
+  out = [controller.update(20.0, 25.0, FakeLead(status=False), model, 0.0, -3.5) for _ in range(5)]
+  assert out[-1] <= -2.5, f"Vision stop should ramp braking fast, got {out}"

tools/replay/mode_sim.py

@@ -532,7 +532,7 @@ def run_simulation(grid, bufs, toggles):
           a_exp = 0.0
 
       # Hybrid Experimental Mode continuous fusion
-      a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp)
+      a_hem = hybrid.update(v_ego, v_cruise, lead, model_v2, a_chill, a_exp, t_follow=t_follow)
       authority = hybrid.exp_authority
 
       model_v0 = v_ego