gpu

Ratched only down below 40m to prevent model jitter from overshooting.

Original PR author: whoisdomi <dcantu83@gmail.com>.

cereal/custom.capnp

@@ -221,11 +221,6 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
   trackingLead @36 :Bool;
   stopSignConfirmed @37 :Bool;
   pulseGlideCoasting @38 :Bool;  # developer-only P&G phase for on-road status UI
-  # Curve Speed Controller diagnostics, for tuning and rollout validation
-  cscOverridden @39 :Bool;          # driver cancelled this curve with RES+
-  cscLearnedLatAccel @40 :Float32;  # learned comfort at the current curvature, before margin
-  cscBindingDistance @41 :Float32;  # distance to the horizon point setting the target, m
-  approachStopLength @42 :Float32;  # pre-commit distance to a detected stop, m; 0 when off
 }
 
 struct StarPilotRadarState @0xb86e6369214c01c8 {

selfdrive/controls/controlsd.py

@@ -169,26 +169,27 @@ CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01  # 1/m
 CURVATURE_HOLD_CONFIRM_MIN = 0.003  # 1/m (~7 deg) of wound curvature before capture
 CURVATURE_HOLD_CONFIRM_SWEPT = 0.6  # rad of heading swept this blinker cycle; past this the push is exit-shaping, not initiation
 
-# Pull-away twitch guard. modeld divides the action head's lateral-ACCELERATION output by
-# max(1, v)^2, so its residual at pull-away (~0.02 m/s^2, the head's noise floor) reads as
-# curvature 0.015 — 38 deg of wheel — where the same value at highway speed is 0.2 deg.
-# Route 78511c37 twitched on 10 of 10 straight takeoffs. The model's own planned path is the
-# tell: it read straight there while the action demanded 6-108x more.
-TWITCH_GUARD_MAX_SPEED = 4.0      # m/s; above this the 1/v^2 amplification is gone
-TWITCH_GUARD_FADE_SPEED = 3.0     # m/s; full strength below, faded out by MAX_SPEED
-TWITCH_GUARD_PLAN_RATIO = 4.0     # allowed |action| / |plan curvature|
-TWITCH_GUARD_FLOOR = 0.002        # 1/m (~5 deg); a near-zero probe must not clamp to nothing
-TWITCH_GUARD_STRAIGHT_LO = 0.005  # 1/m; a plain ratio is too permissive near straight (3x of
-TWITCH_GUARD_STRAIGHT_HI = 0.014  # 0.003 still licenses 22 deg), so fade the allowance out too
-TWITCH_GUARD_MIN_REACH = 12.0     # m; shorter plans read straight while the action legitimately
-                                  # unwinds a turn (ce2b186c51 seg 28 t=14.6). Twitches: p5 24 m
+# Suppress low-speed action spikes while the model's spatial path remains straight.
+TWITCH_GUARD_MAX_SPEED = 4.0
+TWITCH_GUARD_FADE_SPEED = 3.0
+TWITCH_GUARD_DURATION = 1.5
+TWITCH_GUARD_PLAN_RATIO = 4.0
+TWITCH_GUARD_FLOOR = 0.002
+TWITCH_GUARD_STRAIGHT_LO = 0.005
+TWITCH_GUARD_STRAIGHT_HI = 0.014
+TWITCH_GUARD_MIN_REACH = 12.0
 
 
 def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
-  # curvature of the circle through the origin, tangent to the car's heading, passing
-  # through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2)
+  # Fit curvature through the plan point at the requested lookahead.
   px, py = 0.0, 0.0
-  for x, y in zip(xs, ys):
+  for x, y in zip(xs, ys, strict=False):
+    try:
+      x, y = float(x), float(y)
+    except (TypeError, ValueError, OverflowError):
+      return 0.0
+    if not (math.isfinite(x) and math.isfinite(y)):
+      return 0.0
     px, py = x, y
     if math.hypot(x, y) >= lookahead:
       break
@@ -199,11 +200,7 @@ def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
 
 
 def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float:
-  # Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn
-  # starts immediately, which over-winds wide turns whose arc begins several meters out
-  # (wide multi-lane lefts): the near probe reads ~straight there and only grows as the
-  # car approaches the arc, so the readout self-scales to the turn geometry. Sign
-  # disagreement means no coherent turn ahead: contribute nothing.
+  # Use the smaller magnitude of near and far probes to avoid early turn bias.
   xs, ys = model_v2.position.x, model_v2.position.y
   near = _plan_circle_curvature(xs, ys, d_near)
   far = _plan_circle_curvature(xs, ys, d_far)
@@ -236,20 +233,37 @@ def get_plan_turn_onset_dist(model_v2) -> float:
 
 
 def get_plan_reach(model_v2) -> float:
-  xs = model_v2.position.x
-  return xs[-1] if len(xs) else 0.0
+  try:
+    xs = model_v2.position.x
+    return float(xs[-1]) if len(xs) else 0.0
+  except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
+    return 0.0
+
+
+def _plan_positions_are_finite(model_v2) -> bool:
+  try:
+    xs, ys = model_v2.position.x, model_v2.position.y
+    return len(xs) == len(ys) and all(
+      math.isfinite(float(x)) and math.isfinite(float(y)) for x, y in zip(xs, ys, strict=True)
+    )
+  except (AttributeError, TypeError, ValueError, OverflowError):
+    return False
 
 
 def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
-  # See TWITCH_GUARD_*. Magnitude only: the command is bounded, never reversed. FAR fit alone —
-  # the near probe swings with the car's heading error, so once a twitch has yawed the car it
-  # bends to correct it and licenses the very command that caused it (seg 10 t=53.1).
+  if not (math.isfinite(curvature) and math.isfinite(v_ego)):
+    return curvature
   if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0:
     return curvature
-  if get_plan_reach(model_v2) < TWITCH_GUARD_MIN_REACH:
+  if not _plan_positions_are_finite(model_v2):
+    return curvature
+  reach = get_plan_reach(model_v2)
+  if not math.isfinite(reach) or reach < TWITCH_GUARD_MIN_REACH:
     return curvature
   plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y,
                                     CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR))
+  if not math.isfinite(plan):
+    return curvature
   straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO)
   limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR)
   if abs(curvature) <= limit:
@@ -259,6 +273,14 @@ def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
   return curvature + (math.copysign(limit, curvature) - curvature) * fade
 
 
+def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float:
+  if not (math.isfinite(remaining) and math.isfinite(v_ego)):
+    return 0.0
+  if standstill or abs(v_ego) <= 0.3:
+    return TWITCH_GUARD_DURATION
+  return max(remaining - DT_CTRL, 0.0)
+
+
 def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
   if brand == "rivian" or (brand == "subaru" and vehicle_smooth_seconds > 0.0):
     return get_car_lateral_smooth_seconds(brand, v_ego, vehicle_smooth_seconds)
@@ -379,6 +401,7 @@ class Controls:
     self.turn_hold_handoff_t = 0.0
     self.turn_hold_done = False
     self.turn_blinker_swept = 0.0
+    self.twitch_guard_remaining = 0.0
     self.kona_non_scc_lateral_active = False
 
     self.pose_calibrator = PoseCalibrator()
@@ -434,6 +457,7 @@ class Controls:
 
   def state_control(self):
     CS = self.sm['carState']
+    self.twitch_guard_remaining = update_twitch_guard(self.twitch_guard_remaining, CS.vEgo, CS.standstill)
 
     # Update VehicleModel
     lp = self.sm['liveParameters']
@@ -493,7 +517,11 @@ class Controls:
     # EcuDisableFailed is set when car started in READY mode (ECU disable was rejected)
     # Disable longitudinal so stock ACC works instead
     self.update_ecu_disable_failed()
-    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
+    CC.longActive = (
+      CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and
+      not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and
+      not self.ecu_disable_failed
+    )
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
@@ -532,9 +560,8 @@ class Controls:
     # here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering
     # angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1.
     blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
-    # Pull-away twitch guard (see TWITCH_GUARD_*). Requires no turn intent in play, so the
-    # pre-wind ratchet, turn lead and exit opposite-release never see a reduced command.
-    if CC.latActive and blinker_dir == 0.0 and self.turn_hold_curvature == 0.0:
+    if (CC.latActive and self.twitch_guard_remaining > 0.0 and
+        blinker_dir == 0.0 and self.turn_hold_curvature == 0.0):
       new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
     # heading swept in the blinker's direction over the whole blinker cycle (any speed):
     # discriminates a turn not yet made from one being exited (see the re-arm below)

selfdrive/controls/lib/lane_centering.py

@@ -85,7 +85,8 @@ class LaneCenteringController:
   def _covers(x, distance: float) -> bool:
     return bool(x[0] <= distance <= x[-1])
 
-  def _raw_correction(self, model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]:
+  @staticmethod
+  def _raw_correction(model_v2, v_ego: float, offset: float, e2e_authority: float) -> tuple[bool, float]:
     try:
       lane_lines = model_v2.laneLines
       probs = np.asarray(model_v2.laneLineProbs, dtype=float)
@@ -105,11 +106,13 @@ class LaneCenteringController:
       right_y = np.asarray(lane_lines[2].y, dtype=float)
       pos_x = np.asarray(model_v2.position.x, dtype=float)
       pos_y = np.asarray(model_v2.position.y, dtype=float)
-      if not (self._valid_path(left_x, left_y) and self._valid_path(right_x, right_y) and self._valid_path(pos_x, pos_y)):
+      if not (LaneCenteringController._valid_path(left_x, left_y) and
+              LaneCenteringController._valid_path(right_x, right_y) and
+              LaneCenteringController._valid_path(pos_x, pos_y)):
         return False, 0.0
 
       lookahead = float(np.clip(v_ego, 8.0, 35.0))
-      if not all(self._covers(x, lookahead) for x in (left_x, right_x, pos_x)):
+      if not all(LaneCenteringController._covers(x, lookahead) for x in (left_x, right_x, pos_x)):
         return False, 0.0
 
       left = float(np.interp(lookahead, left_x, left_y))
@@ -130,7 +133,7 @@ class LaneCenteringController:
 
       try:
         pos_y_std = np.asarray(model_v2.position.yStd, dtype=float)
-        if self._valid_path(pos_x, pos_y_std):
+        if LaneCenteringController._valid_path(pos_x, pos_y_std):
           path_std = float(np.interp(lookahead, pos_x, pos_y_std))
           if 0.0 <= path_std <= _E2E_MAX_PATH_STD:
             break_in = np.clip(
@@ -145,3 +148,43 @@ class LaneCenteringController:
       return True, float(2.0 * error / lookahead ** 2)
     except (AttributeError, IndexError, TypeError, ValueError):
       return False, 0.0
+
+
+def get_raw_lane_centering_correction(model_v2, v_ego: float, offset: float,
+                                      e2e_authority: float) -> tuple[bool, float]:
+  """Return the instantaneous lane-centering correction without controller filtering."""
+  return LaneCenteringController._raw_correction(model_v2, v_ego, offset, e2e_authority)
+
+
+def get_lane_centering_visual_direction(model_v2, v_ego: float, offset: float, e2e_authority: float,
+                                        enabled: bool, lat_active: bool, pause_on_signal: bool = False,
+                                        turn_signal_active: bool = False,
+                                        applied_correction: float | None = None) -> int:
+  """Return 1 for a right correction, -1 for left, and 0 when no correction is active."""
+  if not enabled or not lat_active or (pause_on_signal and turn_signal_active):
+    return 0
+
+  try:
+    v_ego = float(v_ego)
+    offset = float(offset)
+    e2e_authority = float(e2e_authority)
+    if not np.isfinite([v_ego, offset, e2e_authority]).all() or v_ego < _MIN_V_EGO:
+      return 0
+    if model_v2.meta.laneChangeState != log.LaneChangeState.off:
+      return 0
+  except (AttributeError, TypeError, ValueError):
+    return 0
+
+  valid, correction = get_raw_lane_centering_correction(
+    model_v2,
+    v_ego,
+    float(np.clip(offset, -_MAX_OFFSET, _MAX_OFFSET)),
+    float(np.clip(e2e_authority, 0.0, 1.0)),
+  )
+  if not valid or not np.isfinite(correction):
+    return 0
+  if correction == 0.0:
+    if applied_correction is None or not np.isfinite(applied_correction) or applied_correction == 0.0:
+      return 0
+    correction = applied_correction
+  return 1 if correction > 0.0 else -1

selfdrive/controls/lib/longitudinal_planner.py

@@ -9,7 +9,6 @@ from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.starpilot.common.model_versions import is_tinygrad_model_version
-from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
@@ -382,13 +381,6 @@ def get_vehicle_min_accel(CP, v_ego):
 
 # Restored planner constants retained by CEM, stop, and departure paths.
 A_CRUISE_MIN = -1.0
-# The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack
-# so it stays silent and deceleration sags. Trim pulls the obstacle in to close that.
-FORCE_STOP_OBSTACLE_TRIM = 0.85
-# ...but faded out before the line. The car parks where the obstacle sits, so a trim still
-# applied at 13 m aims the stop 4 m short and force stop then holds it there.
-FORCE_STOP_TRIM_FADE_LO = 15.0   # m — no trim at or below this
-FORCE_STOP_TRIM_FADE_HI = 40.0   # m — full trim at or above this
 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25
 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08
 STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1
@@ -2225,20 +2217,8 @@ class LongitudinalPlanner:
     force_stop_x = None
     force_stop_handoff_m = get_force_stop_handoff_distance(self.CP.carFingerprint)
     if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > force_stop_handoff_m:
-      stop_length = float(sm['starpilotPlan'].forcingStopLength)
-    else:
-      # pre-commit the envelope is only a speed ceiling, which the solver tracks with a lag;
-      # getattr so a stale cereal build degrades to the old behaviour instead of raising
-      stop_length = float(getattr(sm['starpilotPlan'], 'approachStopLength', 0.0))
-    if stop_length > force_stop_handoff_m:
-      # ForceStopDistanceOffset shifts the perceived line for the v_cruise ceiling, so it has
-      # to shift the obstacle too or the slider barely moves anything now that stop_x leads.
-      offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0)))
-      fade = np.clip((stop_length - FORCE_STOP_TRIM_FADE_LO) /
-                     (FORCE_STOP_TRIM_FADE_HI - FORCE_STOP_TRIM_FADE_LO), 0.0, 1.0)
-      trim = 1.0 - (1.0 - FORCE_STOP_OBSTACLE_TRIM) * float(fade)
       force_stop_x = (
-        stop_length * trim + offset_ft * FT_TO_M + STOP_DISTANCE +
+        float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE +
         get_force_stop_distance_bias(self.CP.carFingerprint)
       )
 

selfdrive/controls/radard.py

@@ -53,8 +53,6 @@ ADJACENT_STOP_REST_FRAMES = 15
 ADJACENT_STOP_MIN_Y = 1.8         # m — inside this is our own lane
 ADJACENT_STOP_MAX_Y = 7.5         # m — beyond this is roadside, not an adjacent lane
 ADJACENT_STOP_MAX_D = 110.0       # m
-ADJACENT_STOP_QUEUE_GAP_M = 5.0   # m — anything stopped beyond the furthest qualifier means
-                                  # the bar is past it too, so the hint would stop us short
 
 
 class KalmanParams:
@@ -181,10 +179,6 @@ class Track:
     if self.leadTrackID == self.identifier:
       return False
 
-    return self.in_adjacent_lane(model_data)
-
-  def in_adjacent_lane(self, model_data: capnp._DynamicStructReader):
-    """Lane geometry only, no deceleration history — also used to spot a queue ahead."""
     if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y):
       return False
 
@@ -404,10 +398,9 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca
 def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]:
   """Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane.
 
-  Takes the FARTHEST qualifying vehicle, then drops the hint entirely if a queue reaches
-  past it. Cars already stopped when we acquire them never show the moving -> stopped
-  transition, so the qualifying set is biased toward the back of a line; without this the
-  hint marks a mid-queue bumper and stops us short of the bar.
+  Takes the FARTHEST qualifying vehicle: in a queue the front car sits at the bar and the
+  rest are closer to us, so the nearest one underestimates the distance. The consumer only
+  shortens with this, so underestimating is the harmful direction.
   """
   if len(model_data.laneLines) < 4:
     return {'status': False}
@@ -417,11 +410,6 @@ def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStr
     return {'status': False}
 
   furthest = max(candidates, key=lambda c: c.dRel)
-  for c in tracks.values():
-    if (c.dRel > furthest.dRel + ADJACENT_STOP_QUEUE_GAP_M and
-        abs(c.vLead) < ADJACENT_STOP_REST_V and
-        c.in_adjacent_lane(model_data)):
-      return {'status': False}
   return {
     'status': True,
     'dRel': float(furthest.dRel),

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -1168,7 +1168,6 @@ def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch):
 
   try:
     monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 1.0))
-    monkeypatch.setattr(starpilot_planner_module, "extract_curve_profile", lambda model: ([], []))
     monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None)
     monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None)
     monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0)

selfdrive/controls/tests/test_curve_speed_controller.py

@@ -1,506 +0,0 @@
-import numpy as np
-import pytest
-
-from types import SimpleNamespace
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.starpilot.common.starpilot_variables import DEFAULT_LATERAL_ACCELERATION
-from openpilot.starpilot.controls.lib.curve_speed_controller import (
-  CSC_APPROACH_DECEL,
-  CSC_COMFORT_MARGIN,
-  CSC_COUNT_CAP,
-  CSC_EGO_HEADROOM,
-  CSC_FARFIELD_GAIN,
-  CSC_LAT_ACCEL_MAX,
-  CSC_MIN_SPEED,
-  MAX_CURVATURE,
-  PRIOR_CURVATURE_BP,
-  PRIOR_LAT_ACCEL_V,
-  CSC_NUDGE,
-  CSC_NUDGE_WEIGHT,
-  CSC_OVERRIDE_WATCH_TIME,
-  CSC_TARGET_UP_RATE,
-  CSC_TRAINING_SETTLE_TIME,
-  CurveSpeedController,
-  weighted_isotonic,
-)
-
-
-class FakeParams:
-  def __init__(self, values=None):
-    self.values = dict(values or {})
-
-  def get(self, *args, **kwargs):
-    key = args[0] if args else None
-    return self.values.get(key)
-
-  def put_nonblocking(self, key, value):
-    self.values[key] = value
-
-
-def make_controller(curve_profile=None, curvature_data=None, weather_id=0, reduce_lat=0.0, road_curvature=0.02, driving_in_curve=False):
-  if curve_profile is None:
-    curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
-
-  planner = SimpleNamespace(
-    params=FakeParams({"CurvatureData": curvature_data} if curvature_data is not None else None),
-    curve_profile=curve_profile,
-    starpilot_weather=SimpleNamespace(weather_id=weather_id, reduce_lateral_acceleration=reduce_lat),
-    road_curvature=road_curvature,
-    driving_in_curve=driving_in_curve,
-    tracking_lead=False,
-    lateral_acceleration=0.0,
-  )
-  controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
-  return planner, controller
-
-
-def make_sm(*, gas=False, brake=False, long_active=True, blinker=False, accel_pressed=False):
-  return {
-    "carControl": SimpleNamespace(longActive=long_active),
-    "carState": SimpleNamespace(gasPressed=gas, brakePressed=brake, leftBlinker=blinker, rightBlinker=False),
-    "starpilotCarState": SimpleNamespace(accelPressed=accel_pressed),
-    "onroadEvents": [],
-  }
-
-
-def single_apex_profile(curvature, distance):
-  distances = np.linspace(0.0, max(distance * 1.5, 1.0), 33)
-  curvatures = np.zeros(33)
-  index = int(np.argmin(np.abs(distances - distance)))
-  distances[index] = distance
-  curvatures[index] = curvature
-  return curvatures, distances
-
-
-def converge(controller, v_ego, v_cruise, frames=600):
-  for _ in range(frames):
-    controller.update_target(v_ego, v_cruise)
-  return controller.target
-
-
-def envelope_speed(controller, curvature, distance):
-  curve_speed = max(float(np.sqrt(controller.lat_accel_for_curvature(curvature) / curvature)), CSC_MIN_SPEED)
-  return float(np.sqrt(curve_speed**2 + 2.0 * CSC_APPROACH_DECEL * distance))
-
-
-def test_straight_road_target_is_cruise_speed():
-  _, controller = make_controller()
-
-  controller.update_target(30.0, 30.0)
-
-  assert controller.target == pytest.approx(30.0)
-
-
-def test_distant_apex_does_not_constrain_until_braking_is_due():
-  # derived from the shipped decel so retuning it doesn't silently invalidate the case
-  _, probe = make_controller()
-  curve_speed = max(float(np.sqrt(probe.lat_accel_for_curvature(0.02) / 0.02)), CSC_MIN_SPEED)
-  beyond_braking = 1.3 * (30.0**2 - curve_speed**2) / (2 * CSC_APPROACH_DECEL)
-
-  _, controller = make_controller(curve_profile=single_apex_profile(0.02, beyond_braking))
-
-  target = converge(controller, 30.0, 30.0)
-
-  assert target == pytest.approx(30.0)
-
-
-def test_apex_in_braking_range_constrains_to_kinematic_envelope():
-  _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0))
-
-  target = converge(controller, 30.0, 30.0)
-
-  assert target == pytest.approx(envelope_speed(controller, 0.02, 150.0), abs=0.1)
-  assert target < 30.0
-
-
-def test_exit_recovery_rises_immediately_without_freeze():
-  planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0))
-  low_target = converge(controller, 15.0, 30.0)
-  assert low_target < 20.0
-
-  planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
-  controller.update_target(15.0, 30.0)
-  assert controller.target > low_target          # rises on the very next frame, no freeze
-  assert controller.target - low_target == pytest.approx(CSC_TARGET_UP_RATE * DT_MDL)
-
-  # and it clears the car by the headroom within the time the up-rate needs
-  frames = int((15.0 + CSC_EGO_HEADROOM - controller.target) / (CSC_TARGET_UP_RATE * DT_MDL)) + 1
-  for _ in range(frames):
-    controller.update_target(15.0, 30.0)
-  assert controller.target >= 15.0 + CSC_EGO_HEADROOM
-
-  recovered = converge(controller, 15.0, 30.0)
-  assert recovered == pytest.approx(30.0)
-
-
-def test_upward_jitter_in_the_envelope_is_rate_limited():
-  # a sweeper the envelope only grazes: raw_target flicks between a mild cap and the
-  # set speed. The target must not chase the jumps, or the glow strobes.
-  planner, controller = make_controller(curve_profile=single_apex_profile(0.002, 40.0))
-  steady = converge(controller, 30.0, 32.0)
-  assert steady < 32.0
-
-  flat = (np.zeros(33), np.linspace(0.0, 300.0, 33))
-  grazing = planner.curve_profile
-  peak = steady
-  for i in range(40):
-    planner.curve_profile = flat if i % 2 else grazing
-    controller.update_target(30.0, 32.0)
-    assert controller.target - peak <= CSC_TARGET_UP_RATE * DT_MDL + 1e-6
-    peak = controller.target
-
-
-def test_firm_distant_curvature_is_corrected_for_the_model_under_read():
-  # the model reads ~0.81x actual at range, so a firm distant bend binds later than it should
-  distance = 90.0
-  _, plain = make_controller(curve_profile=single_apex_profile(0.0045, distance))
-  _, probe = make_controller()
-  corrected = probe._correct_far_field(*single_apex_profile(0.0045, distance))
-
-  assert corrected.max() == pytest.approx(0.0045 * CSC_FARFIELD_GAIN)
-  assert converge(plain, 30.0, 30.0) < envelope_speed(plain, 0.0045, distance) + 1e-6
-
-
-def test_weak_or_near_readings_are_left_alone():
-  _, probe = make_controller()
-
-  # too weak to carry usable magnitude at range
-  weak = probe._correct_far_field(*single_apex_profile(0.002, 90.0))
-  assert weak.max() == pytest.approx(0.002)
-
-  # firm, but close enough that the model is already accurate
-  near = probe._correct_far_field(*single_apex_profile(0.0045, 10.0))
-  assert near.max() == pytest.approx(0.0045)
-
-
-def test_far_field_correction_brings_the_slowdown_forward():
-  profile = single_apex_profile(0.0045, 120.0)
-  _, controller = make_controller(curve_profile=profile)
-
-  corrected = converge(controller, 30.0, 30.0)
-  raw_curvatures, distances = profile
-  uncorrected = float(np.sqrt(
-    max(np.sqrt(controller.lat_accel_for_curvature(0.0045) / 0.0045), CSC_MIN_SPEED) ** 2
-    + 2.0 * CSC_APPROACH_DECEL * 120.0))
-
-  assert corrected < uncorrected      # binds sooner than the model's own reading would
-
-
-def test_fresh_activation_seeds_at_envelope_not_cruise():
-  _, controller = make_controller(curve_profile=(np.full(33, 0.05), np.linspace(0.0, 60.0, 33)))
-
-  controller.update_target(6.0, 30.0)
-
-  assert controller.target < 15.0
-
-
-def test_target_never_trails_accelerating_car_when_unconstrained():
-  planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0))
-  converge(controller, 15.0, 30.0)
-
-  planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
-  v_ego = 15.0
-  caught_up = None
-  for frame in range(200):
-    v_ego = min(v_ego + 2.0 * DT_MDL, 30.0)
-    controller.update_target(v_ego, 30.0)
-    # the target climbs faster than the car can, so once it is ahead it stays ahead
-    if controller.target >= v_ego:
-      caught_up = caught_up if caught_up is not None else frame
-    assert caught_up is None or controller.target >= min(30.0, v_ego) - 1e-6
-
-  assert caught_up is not None and caught_up * DT_MDL < 2.0
-  assert controller.target == pytest.approx(30.0)
-
-
-def test_target_does_not_ratchet_down_with_ego_speed():
-  _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0))
-  target = converge(controller, 30.0, 30.0)
-  assert target > CSC_MIN_SPEED  # a real curve speed, not floored
-
-  controller.update_target(14.0, 30.0)
-
-  assert controller.target == pytest.approx(target, abs=0.2)
-
-
-def test_sharp_curve_target_floors_at_min_speed():
-  _, controller = make_controller(curve_profile=(np.full(33, 0.1), np.linspace(0.0, 100.0, 33)))
-
-  target = converge(controller, 15.0, 30.0)
-
-  assert target == pytest.approx(CSC_MIN_SPEED, abs=0.05)
-
-
-def test_weather_reduces_curve_speed():
-  _, dry = make_controller(curve_profile=single_apex_profile(0.01, 0.0))
-  _, wet = make_controller(curve_profile=single_apex_profile(0.01, 0.0), weather_id=1, reduce_lat=0.2)
-
-  dry_target = converge(dry, 20.0, 30.0)
-  wet_target = converge(wet, 20.0, 30.0)
-
-  assert wet_target < dry_target
-  assert wet_target == pytest.approx(dry_target * np.sqrt(0.8), abs=0.1)
-
-
-def test_prior_gives_higher_lat_accel_for_sharper_curves():
-  _, controller = make_controller()
-
-  assert controller.learned_lat_accel(0.001) == pytest.approx(1.5, abs=0.05)
-  assert controller.learned_lat_accel(MAX_CURVATURE) > controller.learned_lat_accel(0.001)
-  assert controller.learned_lat_accel(MAX_CURVATURE) == pytest.approx(
-    float(np.interp(MAX_CURVATURE, PRIOR_CURVATURE_BP, PRIOR_LAT_ACCEL_V)), abs=0.05)
-  assert controller.lateral_acceleration == pytest.approx(DEFAULT_LATERAL_ACCELERATION)
-
-
-def test_comfort_margin_matches_the_learned_habit():
-  # margin is fixed at 1.0 -- CSC targets exactly the driver's own learned comfort
-  _, controller = make_controller()
-
-  assert CSC_COMFORT_MARGIN == pytest.approx(1.0)
-  assert controller.lat_accel_for_curvature(0.01) == pytest.approx(controller.learned_lat_accel(0.01))
-
-
-def test_binding_distance_reports_the_constraining_point():
-  _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0))
-  converge(controller, 30.0, 30.0)
-
-  assert controller.binding_distance == pytest.approx(150.0, abs=1.0)
-
-
-def test_binding_distance_is_zero_when_unconstrained():
-  _, controller = make_controller()
-  converge(controller, 30.0, 30.0)
-
-  assert controller.binding_distance == 0.0
-
-
-def test_heavily_sampled_bucket_dominates_prior():
-  _, controller = make_controller(curvature_data={"0.05": {"average": 3.0, "count": 100000}})
-
-  assert controller.learned_lat_accel(0.05) == pytest.approx(3.0, abs=0.05)
-  assert controller.learned_lat_accel(0.08) >= controller.learned_lat_accel(0.05)
-
-
-def test_learned_curve_stays_monotonic_despite_low_outlier_bucket():
-  _, controller = make_controller(curvature_data={"0.05": {"average": 0.5, "count": 100000}})
-
-  assert controller.learned_lat_accel(0.05) >= controller.learned_lat_accel(0.03)
-
-
-def test_dense_bucket_is_not_overridden_by_sparse_neighbour():
-  # real device data: a running maximum ratcheted the 80-sample bucket up to the 20-sample neighbour
-  _, dense_low = make_controller(curvature_data={
-    "0.003": {"average": 1.95, "count": 20},
-    "0.005": {"average": 1.38, "count": 80},
-  })
-  _, dense_high = make_controller(curvature_data={
-    "0.003": {"average": 1.95, "count": 80},
-    "0.005": {"average": 1.38, "count": 20},
-  })
-
-  assert dense_low.learned_lat_accel(0.005) < 1.95      # not ratcheted to the sparse neighbour
-  assert dense_low.learned_lat_accel(0.005) >= dense_low.learned_lat_accel(0.003)
-  # whichever side is better sampled should pull the fit: swapping the counts must raise it
-  assert dense_high.learned_lat_accel(0.005) > dense_low.learned_lat_accel(0.005)
-
-
-def test_weighted_isotonic_pools_violators_by_weight():
-  fitted = weighted_isotonic(np.array([1.0, 3.0, 1.2]), np.array([1.0, 1.0, 1000.0]))
-
-  assert np.all(np.diff(fitted) >= -1e-9)
-  assert fitted[-1] == pytest.approx(1.2, abs=0.02)
-
-
-def test_weighted_isotonic_leaves_sorted_input_untouched():
-  values = np.array([1.0, 1.5, 2.0, 2.5])
-  fitted = weighted_isotonic(values, np.ones(4))
-
-  assert fitted == pytest.approx(values)
-
-
-def test_legacy_off_grid_curvature_data_merges_into_buckets():
-  _, controller = make_controller(curvature_data={
-    "0.0203": {"average": 2.5, "count": 10},
-    "0.02": {"average": 2.0, "count": 10},
-  })
-
-  assert controller.curvature_data["0.02"]["count"] == 20
-  assert controller.curvature_data["0.02"]["average"] == pytest.approx(2.25)
-
-
-def test_training_update_step_is_capped_by_ema_count():
-  planner, controller = make_controller(curvature_data={"0.02": {"average": 2.0, "count": 10000}}, driving_in_curve=True)
-  planner.lateral_acceleration = 3.0
-  controller.training_timer = CSC_TRAINING_SETTLE_TIME
-
-  controller.log_data(10.0, make_sm(long_active=False))
-
-  data = controller.curvature_data["0.02"]
-  assert data["count"] == 10001
-  assert data["average"] == pytest.approx((2.0 * CSC_COUNT_CAP + 3.0) / (CSC_COUNT_CAP + 1))
-
-
-def test_no_passive_training_right_after_csc_limited_speed():
-  planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0), driving_in_curve=True)
-  planner.lateral_acceleration = 3.0
-  converge(controller, 15.0, 30.0)
-  assert controller.training_quiet_timer > 0.0
-
-  controller.training_timer = CSC_TRAINING_SETTLE_TIME
-  controller.log_data(10.0, make_sm(long_active=False))
-  assert "0.02" not in controller.curvature_data
-  assert not controller.enable_training
-
-  controller.training_quiet_timer = 0.0
-  controller.training_timer = CSC_TRAINING_SETTLE_TIME
-  controller.log_data(10.0, make_sm(long_active=False))
-  assert controller.curvature_data["0.02"]["count"] == 1
-
-
-def test_training_settles_within_a_couple_of_seconds():
-  # a real drive rarely holds every eligibility condition for a whole model horizon,
-  # so the settle time has to be short enough that ordinary curves still teach it
-  planner, controller = make_controller(driving_in_curve=True)
-  planner.lateral_acceleration = 2.4
-  sm = make_sm(long_active=False)
-
-  for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) - 2):
-    controller.log_data(10.0, sm)
-  assert "0.02" not in controller.curvature_data
-
-  for _ in range(3):
-    controller.log_data(10.0, sm)
-  assert controller.curvature_data["0.02"]["count"] >= 1
-
-
-def test_brief_ineligibility_does_not_restart_the_settle_timer():
-  planner, controller = make_controller(driving_in_curve=True)
-  planner.lateral_acceleration = 2.4
-  sm = make_sm(long_active=False)
-  for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) + 1):
-    controller.log_data(10.0, sm)
-  trained = controller.curvature_data["0.02"]["count"]
-
-  # a lead flickers into the tracker for two frames, then leaves
-  planner.tracking_lead = True
-  controller.log_data(10.0, sm)
-  controller.log_data(10.0, sm)
-  planner.tracking_lead = False
-
-  controller.log_data(10.0, sm)
-  assert controller.curvature_data["0.02"]["count"] == trained + 1
-
-
-def test_sustained_ineligibility_still_drains_the_settle_timer():
-  planner, controller = make_controller(driving_in_curve=True)
-  planner.lateral_acceleration = 2.4
-  engaged = make_sm(long_active=True)
-  manual = make_sm(long_active=False)
-  for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) + 1):
-    controller.log_data(10.0, manual)
-
-  for _ in range(int(2 * CSC_TRAINING_SETTLE_TIME / DT_MDL)):
-    controller.log_data(10.0, engaged)
-  assert controller.training_timer == pytest.approx(0.0)
-
-  controller.log_data(10.0, manual)
-  assert not controller.enable_training
-
-
-def settle_override(controller, sm=None, frames=None):
-  """Run the post-override watch out so the pseudo-sample is committed."""
-  sm = sm if sm is not None else make_sm()
-  for _ in range(frames if frames is not None else int(CSC_OVERRIDE_WATCH_TIME / DT_MDL) + 1):
-    controller.handle_override(20.0, False, sm)
-
-
-def test_gas_override_nudges_bucket_up_once_per_episode():
-  _, controller = make_controller()
-  prior = controller.learned_lat_accel(0.02)
-  controller.target = 10.0
-
-  controller.handle_override(20.0, True, make_sm(gas=True))
-  controller.handle_override(20.0, True, make_sm(gas=True))
-  assert "0.02" not in controller.curvature_data  # still watching what the driver holds
-
-  settle_override(controller)
-  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
-  assert controller.curvature_data["0.02"]["average"] > prior
-
-  controller.handle_override(20.0, False, make_sm())
-  controller.target = 10.0
-  controller.handle_override(20.0, True, make_sm(gas=True))
-  settle_override(controller)
-  assert controller.curvature_data["0.02"]["count"] == 2 * CSC_NUDGE_WEIGHT
-
-
-def test_override_learns_the_cornering_the_driver_actually_held():
-  # the whole point: a fixed step needs several rejections to close a real disagreement,
-  # so record what they demonstrated instead
-  planner, observed = make_controller(driving_in_curve=True)
-  observed.target = 10.0
-  observed.handle_override(20.0, True, make_sm(gas=True))
-  planner.lateral_acceleration = 2.9          # they hold the curve much harder than CSC wanted
-  settle_override(observed, make_sm(gas=True))
-
-  _, stepped = make_controller(driving_in_curve=True)
-  stepped._apply_nudge(CSC_NUDGE)             # what the old fixed-step path would have recorded
-
-  assert observed.curvature_data["0.02"]["average"] == pytest.approx(2.9)
-  assert observed.curvature_data["0.02"]["average"] > stepped.curvature_data["0.02"]["average"]
-  assert observed.learned_lat_accel(0.02) > stepped.learned_lat_accel(0.02)
-
-
-def test_override_on_a_straight_still_registers_the_fixed_step():
-  planner, controller = make_controller()
-  prior = controller.learned_lat_accel(0.02)
-  controller.target = 10.0
-
-  controller.handle_override(20.0, True, make_sm(gas=True))
-  planner.lateral_acceleration = 0.0          # never reached a corner
-  settle_override(controller)
-
-  assert controller.curvature_data["0.02"]["average"] == pytest.approx(prior + CSC_NUDGE)
-
-
-def test_res_button_nudges_bucket_up_even_at_target_speed():
-  _, controller = make_controller()
-  prior = controller.learned_lat_accel(0.02)
-  controller.target = 20.0  # car tracking the target, so the gas-press condition would not fire
-
-  controller.handle_override(20.0, True, make_sm(), accel_button=True)
-  settle_override(controller)
-
-  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
-  assert controller.curvature_data["0.02"]["average"] > prior
-
-
-def test_brake_override_nudges_bucket_down():
-  _, controller = make_controller(driving_in_curve=True)
-  prior = controller.learned_lat_accel(0.02)
-
-  controller.handle_override(20.0, True, make_sm(brake=True))
-
-  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
-  assert controller.curvature_data["0.02"]["average"] < prior
-
-
-def test_calibrated_lateral_acceleration_param_is_written_on_flush():
-  planner, controller = make_controller(curvature_data={"0.02": {"average": 2.8, "count": 5000}})
-
-  assert "CalibratedLateralAcceleration" not in planner.params.values
-  controller.flush_data()
-
-  assert planner.params.values["CalibratedLateralAcceleration"] > DEFAULT_LATERAL_ACCELERATION
-  assert controller.lateral_acceleration == planner.params.values["CalibratedLateralAcceleration"]
-
-
-def test_stale_param_from_a_previous_build_is_republished_without_training():
-  # a stale value must not survive a restart just because this drive never trained
-  planner, controller = make_controller(curvature_data={"0.02": {"average": 2.8, "count": 5000}})
-  planner.params.values["CalibratedLateralAcceleration"] = 3.71
-
-  controller.log_data(0.0, make_sm())  # standstill: ineligible -> flush path
-
-  assert planner.params.values["CalibratedLateralAcceleration"] <= CSC_LAT_ACCEL_MAX

selfdrive/controls/tests/test_lane_centering.py

@@ -3,7 +3,7 @@ from types import SimpleNamespace
 import numpy as np
 import pytest
 
-from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController
+from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController, get_lane_centering_visual_direction
 
 
 _V_EGO = 20.0
@@ -174,3 +174,20 @@ def test_correction_is_smoothed_and_capped():
   _, steady = _converge(model, authority=0.0)
   assert 0.0 < first < steady
   assert np.isclose(steady, 0.004 * 0.30, atol=1e-6)
+
+
+def test_visual_direction_matches_curvature_sign():
+  # Positive curvature is right in this tree's convention.
+  assert get_lane_centering_visual_direction(_model(left=-1.5, right=2.1), _V_EGO, 0.0, 0.0, True, True) == 1
+  assert get_lane_centering_visual_direction(_model(left=-2.1, right=1.5), _V_EGO, 0.0, 0.0, True, True) == -1
+
+
+def test_visual_direction_requires_both_primary_lane_lines():
+  model = _model(left=-1.5, right=2.1)
+  model.laneLineProbs[2] = 0.2
+  assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True) == 0
+
+
+def test_visual_direction_uses_filtered_correction_in_deadband():
+  model = _model()
+  assert get_lane_centering_visual_direction(model, _V_EGO, 0.0, 0.0, True, True, applied_correction=0.001) == 1

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -526,7 +526,6 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta
       forcingStop=False,
       redLight=False,
       forcingStopLength=2,
-      approachStopLength=0.0,
     ),
   }
 
@@ -2991,6 +2990,39 @@ def test_modeld_action_uses_current_action_head_scaling_for_v15(monkeypatch):
   assert not action.shouldStop
 
 
+def test_modeld_action_uses_current_action_head_scaling_for_v16(monkeypatch):
+  monkeypatch.setenv("DEBUG", "0")
+  fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx")
+  fake_commonmodel.DrivingModelFrame = object
+  fake_commonmodel.CLContext = object
+  monkeypatch.setitem(sys.modules, fake_commonmodel.__name__, fake_commonmodel)
+
+  from openpilot.selfdrive.modeld import modeld
+
+  prev_action = log.ModelDataV2.Action.new_message()
+  prev_action.desiredCurvature = 0.05
+  prev_action.desiredAcceleration = -0.2
+  toggles = SimpleNamespace(vEgoStopping=0.42)
+
+  action = modeld.get_action_from_model(
+    {"action": np.array([[12.0, -0.8]], dtype=np.float32)},
+    prev_action,
+    lat_action_t=0.2,
+    long_action_t=0.73,
+    v_ego=5.0,
+    mlsim=True,
+    is_v9=False,
+    is_v14=False,
+    is_v15=False,
+    starpilot_toggles=toggles,
+    is_v16=True,
+  )
+
+  assert action.desiredCurvature == pytest.approx(modeld.smooth_value(0.48, prev_action.desiredCurvature, modeld.LAT_SMOOTH_SECONDS))
+  assert action.desiredAcceleration < -0.2
+  assert not action.shouldStop
+
+
 def test_publish_force_stop_handoff_sets_should_stop_when_vcruise_zero():
   class FakePM:
     def __init__(self):

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -5,9 +5,8 @@ import pytest
 from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME
-from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_GLOW_HOLD_TIME, CSC_GLOW_ON_DELTA
+from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
 from openpilot.starpilot.controls.lib.starpilot_vcruise import (
-  FORCE_STOP_CAP_SLACK_M,
   FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
   STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME,
   StarPilotVCruise,
@@ -54,14 +53,11 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
     raw_model_stopped=raw_model_stopped,
     road_curvature=road_curvature,
     road_curvature_detected=False,
-    lateral_acceleration=0.0,
   )
   vcruise = StarPilotVCruise(planner)
   vcruise.forcing_stop = forcing_stop
   vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0
   vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length
-  # what the not-committed branch would have left behind on the frame before commit
-  vcruise.force_stop_distance_cap = planner.model_length
   return planner, vcruise
 
 
@@ -84,12 +80,12 @@ def make_sm(*, standstill=True, min_steer_speed=0.0, car_fingerprint=""):
   }
 
 
-def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, v_cruise=20.0, controls_enabled=True):
+def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, controls_enabled=True):
   return vcruise.update(
     controls_enabled=controls_enabled,
     now=now,
     time_validated=True,
-    v_cruise=v_cruise,
+    v_cruise=20.0,
     v_ego=v_ego,
     sm=sm,
     starpilot_toggles=toggles,
@@ -147,56 +143,30 @@ def test_santa_fe_force_stop_tune_only_applies_to_that_car():
   assert get_force_stop_low_speed_hold(other) is None
 
 
-def test_curve_speed_controller_blinker_releases_the_cap_but_keeps_the_plan():
+def test_curve_speed_controller_holds_target_through_brief_detector_dropout():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)
   toggles = make_toggles()
   toggles.curve_speed_controller = True
 
-  calls = []
-
-  def set_curve_target(_v_ego, _v_cruise):
-    calls.append(_v_ego)
+  def set_curve_target(_v_ego):
+    vcruise.csc.target_set = True
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
+  planner.road_curvature_detected = True
   result = update_vcruise(vcruise, sm, toggles, now=10.0, v_ego=20.0)
   assert result == pytest.approx(14.0)
   assert vcruise.csc_controlling_speed
 
-  # the cap lifts so CSC can't fight the lane change, but the envelope keeps planning
-  # so the curve doesn't have to be re-discovered from the set speed afterwards
-  sm["carState"].leftBlinker = True
+  planner.road_curvature_detected = False
   result = update_vcruise(vcruise, sm, toggles, now=10.25, v_ego=20.0)
-  assert result == pytest.approx(20.0)
-  assert not vcruise.csc_controlling_speed
-  assert len(calls) == 2  # still planning, so nothing has to be rediscovered
-
-  # blinker off: the plan is already current, so the cap comes straight back
-  sm["carState"].leftBlinker = False
-  result = update_vcruise(vcruise, sm, toggles, now=10.5, v_ego=20.0)
   assert result == pytest.approx(14.0)
   assert vcruise.csc_controlling_speed
 
-
-def test_curve_speed_controller_reseeds_after_a_real_dropout():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 14.0
-
-  vcruise.csc.update_target = set_curve_target
-  update_vcruise(vcruise, sm, toggles, now=11.0, v_ego=20.0)
-  assert vcruise.csc_controlling_speed
-
-  # disengaging is a real dropout, not a momentary veto -- that still resets
-  sm["carControl"].longActive = False
-  update_vcruise(vcruise, sm, toggles, now=11.05, v_ego=20.0)
+  result = update_vcruise(vcruise, sm, toggles, now=10.8, v_ego=20.0)
+  assert result == pytest.approx(20.0)
   assert not vcruise.csc_controlling_speed
-  assert vcruise.csc.seed_pending
 
 
 def test_curve_speed_controller_releases_immediately_when_disabled():
@@ -205,13 +175,16 @@ def test_curve_speed_controller_releases_immediately_when_disabled():
   toggles = make_toggles()
   toggles.curve_speed_controller = True
 
-  def set_curve_target(_v_ego, _v_cruise):
+  def set_curve_target(_v_ego):
+    vcruise.csc.target_set = True
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
+  planner.road_curvature_detected = True
   update_vcruise(vcruise, sm, toggles, now=20.0, v_ego=20.0)
   assert vcruise.csc_controlling_speed
 
+  planner.road_curvature_detected = False
   toggles.curve_speed_controller = False
   result = update_vcruise(vcruise, sm, toggles, now=20.1, v_ego=20.0)
   assert result == pytest.approx(20.0)
@@ -225,10 +198,12 @@ def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead():
   toggles.curve_speed_controller = True
   toggles.csc_no_lead = True
 
-  def set_curve_target(_v_ego, _v_cruise):
+  def set_curve_target(_v_ego):
+    vcruise.csc.target_set = True
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
+  planner.road_curvature_detected = True
 
   result = update_vcruise(vcruise, sm, toggles, now=30.0, v_ego=20.0)
   assert result == pytest.approx(14.0)
@@ -246,8 +221,10 @@ def test_curve_speed_controller_stays_enabled_with_a_lead_by_default():
   toggles = make_toggles()
   toggles.curve_speed_controller = True
   planner.starpilot_following.following_lead = True
+  planner.road_curvature_detected = True
 
-  def set_curve_target(_v_ego, _v_cruise):
+  def set_curve_target(_v_ego):
+    vcruise.csc.target_set = True
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
@@ -297,276 +274,54 @@ def test_curve_speed_controller_persists_data_after_leaving_curve():
   assert any(key == "CurvatureData" for key, _ in planner.params.writes)
 
 
-def test_csc_res_press_cancels_for_episode_and_rearms():
-  planner, vcruise = make_vcruise()
+def test_curve_speed_controller_publishes_live_values_to_memory_params():
+  planner, vcruise = make_vcruise(road_curvature=0.02)
   sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  curve_target = {"v": 14.0}
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = curve_target["v"]
-
-  vcruise.csc.update_target = set_curve_target
-  result = update_vcruise(vcruise, sm, toggles, now=60.0, v_ego=20.0)
-  assert result == pytest.approx(14.0)
-  assert vcruise.csc_controlling_speed
-
-  sm["starpilotCarState"].accelPressed = True
-  result = update_vcruise(vcruise, sm, toggles, now=60.05, v_ego=20.0)
-  assert result == pytest.approx(20.0)
-  assert not vcruise.csc_controlling_speed
-  assert vcruise.csc_override
-
-  # latches for the rest of the episode, not just while pressed
-  sm["starpilotCarState"].accelPressed = False
-  result = update_vcruise(vcruise, sm, toggles, now=60.1, v_ego=20.0)
-  assert result == pytest.approx(20.0)
-  assert vcruise.csc_override
-
-  # curve ends -> re-arms
-  curve_target["v"] = 20.0
-  update_vcruise(vcruise, sm, toggles, now=60.15, v_ego=20.0)
-  assert not vcruise.csc_override
-
-  curve_target["v"] = 14.0
-  result = update_vcruise(vcruise, sm, toggles, now=60.2, v_ego=20.0)
-  assert result == pytest.approx(14.0)
-  assert vcruise.csc_controlling_speed
-
-
-def test_csc_res_press_does_not_latch_when_csc_was_not_active():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 14.0
-
-  vcruise.csc.update_target = set_curve_target
-  # press before CSC ever limited: suspends it while held, but must not latch a cancel
-  sm["starpilotCarState"].accelPressed = True
-  result = update_vcruise(vcruise, sm, toggles, now=70.0, v_ego=20.0)
-  assert result == pytest.approx(20.0)
-  assert not vcruise.csc_override
-
-  sm["starpilotCarState"].accelPressed = False
-  result = update_vcruise(vcruise, sm, toggles, now=70.05, v_ego=20.0)
-  assert result == pytest.approx(14.0)
-  assert vcruise.csc_controlling_speed
-
-
-def test_csc_res_press_defers_to_slc_confirmation():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 14.0
-
-  vcruise.csc.update_target = set_curve_target
-  update_vcruise(vcruise, sm, toggles, now=80.0, v_ego=20.0)
-  assert vcruise.csc_controlling_speed
-
-  # confirming a speed limit must not also cancel the curve slowdown
-  vcruise.slc.speed_limit_changed_timer = 1.0
-  vcruise.slc.unconfirmed_speed_limit = 25.0
-  sm["starpilotCarState"].accelPressed = True
-  update_vcruise(vcruise, sm, toggles, now=80.05, v_ego=20.0)
-  assert not vcruise.csc_override
-
-  sm["starpilotCarState"].accelPressed = False
-  result = update_vcruise(vcruise, sm, toggles, now=80.1, v_ego=20.0)
-  assert result == pytest.approx(14.0)
-  assert vcruise.csc_controlling_speed
-
-
-def test_curve_speed_controller_glow_ignores_a_trivial_graze():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  # a long gentle bend where the envelope only shaves a little: the target hovers either
-  # side of the threshold for the whole curve, so a low bar strobes the glow
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 20.0 - (CSC_GLOW_ON_DELTA / 2.0)
-
-  vcruise.csc.update_target = set_curve_target
-  result = update_vcruise(vcruise, sm, toggles, now=160.0, v_ego=20.0)
-
-  assert result < 20.0                      # the cap is still applied
-  assert not vcruise.csc_controlling_speed  # it just isn't worth announcing
-
-
-def test_curve_speed_controller_glow_holds_through_a_brief_release():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  curve_target = {"v": 14.0}
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = curve_target["v"]
-
-  vcruise.csc.update_target = set_curve_target
-  update_vcruise(vcruise, sm, toggles, now=130.0, v_ego=20.0)
-  assert vcruise.csc_controlling_speed
-
-  # one curve routinely lets go and re-engages; the glow must ride through it
-  curve_target["v"] = 20.0
-  now = 130.0
-  for _ in range(int((CSC_GLOW_HOLD_TIME - 0.2) / DT_MDL)):
-    now += DT_MDL
-    update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
-    assert vcruise.csc_controlling_speed
-
-  curve_target["v"] = 14.0
-  now += DT_MDL
-  update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
-  assert vcruise.csc_controlling_speed
-
-
-def test_curve_speed_controller_glow_clears_once_the_release_sticks():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  curve_target = {"v": 14.0}
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = curve_target["v"]
-
-  vcruise.csc.update_target = set_curve_target
-  update_vcruise(vcruise, sm, toggles, now=140.0, v_ego=20.0)
-  assert vcruise.csc_controlling_speed
-
-  curve_target["v"] = 20.0
-  now = 140.0
-  for _ in range(int(CSC_GLOW_HOLD_TIME / DT_MDL) + 1):
-    now += DT_MDL
-    update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
-  assert not vcruise.csc_controlling_speed
-
-
-def test_curve_speed_controller_keeps_the_cap_when_signalling_mid_curve():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 14.0
-
-  vcruise.csc.update_target = set_curve_target
-  result = update_vcruise(vcruise, sm, toggles, now=150.0, v_ego=20.0)
-  assert result == pytest.approx(14.0)
-
-  # a lane change taken inside a curve must not hand the speed back
+  sm["carControl"].longActive = False
   planner.driving_in_curve = True
-  sm["carState"].leftBlinker = True
-  result = update_vcruise(vcruise, sm, toggles, now=150.05, v_ego=20.0)
-  assert result == pytest.approx(14.0)
-  assert vcruise.csc_controlling_speed
+  planner.lateral_acceleration = 2.4
+  vcruise.csc.training_timer = PLANNER_TIME
 
-  # on a straight it still yields, so CSC can't fight the manoeuvre
-  planner.driving_in_curve = False
-  result = update_vcruise(vcruise, sm, toggles, now=150.1, v_ego=20.0)
-  assert result == pytest.approx(20.0)
-  assert not vcruise.csc_controlling_speed
+  vcruise.csc.log_data(20.0, sm)
+
+  assert any(key == "CalibratedLateralAcceleration" for key, _ in planner.params_memory.writes)
+  assert any(key == "CalibrationProgress" for key, _ in planner.params_memory.writes)
+  assert planner.params_memory.values["CalibrationProgress"] > 0.0
 
 
-def test_curve_speed_controller_glow_lights_when_the_car_arrives_at_the_cap_from_below():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
+def test_curve_speed_controller_ramps_toward_curve_speed_at_bounded_rate():
+  planner = SimpleNamespace(
+    params=FakeParams(),
+    road_curvature=0.004,
+    time_to_curve=2.0,
+    starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0),
+  )
+  controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
+  controller.lateral_acceleration = 2.0
+  controller.target_set = True
+  controller.target = 30.0
 
-  # accelerating out of a slow zone into a curve: the target is never under v_ego, but it
-  # is still the only thing stopping the car from reaching the set speed
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 22.0
+  controller.update_target(30.0)
 
-  vcruise.csc.update_target = set_curve_target
-
-  update_vcruise(vcruise, sm, toggles, now=120.0, v_ego=15.0, v_cruise=32.0)
-  assert not vcruise.csc_controlling_speed          # still climbing, CSC isn't holding it yet
-
-  result = update_vcruise(vcruise, sm, toggles, now=120.05, v_ego=22.0, v_cruise=32.0)
-  assert result == pytest.approx(22.0)
-  assert vcruise.csc_controlling_speed              # arrived at the cap, and it binds
+  assert controller.target == pytest.approx(30.0 - CSC_MAX_DECEL_RATE * DT_MDL)
+  assert controller.target > (controller.lateral_acceleration / planner.road_curvature) ** 0.5
 
 
-def test_curve_speed_controller_glow_stays_off_while_the_target_is_above_v_ego():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
+def test_curve_speed_controller_does_not_slow_for_curve_speed_above_ego():
+  planner = SimpleNamespace(
+    params=FakeParams(),
+    road_curvature=0.001,
+    time_to_curve=2.0,
+    starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0),
+  )
+  controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner))
+  controller.lateral_acceleration = 2.0
+  controller.target_set = True
+  controller.target = 28.0
 
-  # a highway sweeper trims the target well under the set speed but never under v_ego,
-  # so the car keeps accelerating and the driver feels nothing
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 26.0
-
-  vcruise.csc.update_target = set_curve_target
-  result = update_vcruise(vcruise, sm, toggles, now=90.0, v_ego=20.0, v_cruise=30.0)
-
-  assert result == pytest.approx(26.0)
-  assert not vcruise.csc_controlling_speed
-
-
-def test_curve_speed_controller_glow_holds_through_the_recovery_ramp():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  curve_target = {"v": 14.0}
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = curve_target["v"]
-
-  vcruise.csc.update_target = set_curve_target
-  update_vcruise(vcruise, sm, toggles, now=100.0, v_ego=20.0)
-  assert vcruise.csc_controlling_speed
-
-  # past the apex the target climbs back above v_ego while the car is still cornering
-  curve_target["v"] = 18.0
-  update_vcruise(vcruise, sm, toggles, now=100.05, v_ego=15.0)
-  assert vcruise.csc_controlling_speed
-
-  # fully released, but the glow only clears once the release has stuck
-  curve_target["v"] = 20.0
-  now = 100.1
-  update_vcruise(vcruise, sm, toggles, now=now, v_ego=17.0)
-  assert vcruise.csc_controlling_speed
-
-  for _ in range(int(CSC_GLOW_HOLD_TIME / DT_MDL) + 1):
-    now += DT_MDL
-    update_vcruise(vcruise, sm, toggles, now=now, v_ego=17.0)
-  assert not vcruise.csc_controlling_speed
-
-
-def test_curve_speed_controller_hysteresis_keeps_glow_off_for_marginal_targets():
-  planner, vcruise = make_vcruise()
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-
-  def set_curve_target(_v_ego, _v_cruise):
-    vcruise.csc.target = 19.7
-
-  vcruise.csc.update_target = set_curve_target
-  result = update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0)
-
-  assert result == pytest.approx(19.7)
-  assert not vcruise.csc_controlling_speed
+  controller.update_target(30.0)
 
+  assert controller.target == pytest.approx(30.0)
 
 
 def test_active_slc_control_target_applies_offset_and_cluster_diff():
@@ -769,7 +524,6 @@ def test_force_stop_reanchors_when_model_reopens_path_without_stop_action():
   planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
   planner.model_length = 90.0
   vcruise.tracked_model_length = 60.0
-  vcruise.force_stop_distance_cap = 90.0
   sm = make_sm(standstill=False)
   sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
 
@@ -819,37 +573,6 @@ def test_force_stop_does_not_reanchor_inside_reanchor_floor():
   assert vcruise.tracked_model_length < 25.0
 
 
-def test_force_stop_reanchor_bounded_by_distance_driven():
-  # The line can't recede: a ballooning horizon may not push the stop past where it was at
-  # commit minus the distance driven since.
-  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
-  planner.model_length = 200.0
-  vcruise.tracked_model_length = 60.0
-  vcruise.force_stop_distance_cap = 70.0
-  sm = make_sm(standstill=False)
-  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
-
-  update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.0)
-
-  assert vcruise.tracked_model_length <= 70.0 + FORCE_STOP_CAP_SLACK_M
-  assert vcruise.tracked_model_length < 100.0  # nowhere near the 200 m the horizon claimed
-
-
-def test_force_stop_cap_slack_tapers_near_the_line():
-  # Slack protects against an under-read at commit; held near the line it would just aim the
-  # solver that far past the stop bar.
-  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
-  planner.model_length = 200.0
-  vcruise.tracked_model_length = 60.0
-  vcruise.force_stop_distance_cap = 12.0
-  sm = make_sm(standstill=False)
-  sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False))
-
-  update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=5.0)
-
-  assert vcruise.tracked_model_length < 12.0 + FORCE_STOP_CAP_SLACK_M / 2.0
-
-
 def test_force_stop_does_not_reanchor_committed_model_stop():
   planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
   planner.model_length = 40.0

selfdrive/controls/tests/test_turn_lead.py

@@ -5,9 +5,16 @@ from cereal import car
 
 import pytest
 
-from openpilot.selfdrive.controls.controlsd import (TWITCH_GUARD_FLOOR, TWITCH_GUARD_MAX_SPEED,
-                                                    get_control_lateral_smooth_seconds,
-                                                    limit_curvature_to_plan, turn_lead_allowed)
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.controlsd import (
+  TWITCH_GUARD_DURATION,
+  TWITCH_GUARD_FLOOR,
+  TWITCH_GUARD_MAX_SPEED,
+  get_control_lateral_smooth_seconds,
+  limit_curvature_to_plan,
+  turn_lead_allowed,
+  update_twitch_guard,
+)
 
 
 LateralControlMode = car.CarControl.Actuators.LateralControlMode
@@ -18,15 +25,14 @@ def _plan(xs, ys):
 
 
 def _arc_plan(radius, n=200):
-  # constant-radius arc, ~1 rad of heading — long enough to clear the reach gate
   return _plan([radius * math.sin(i / n) for i in range(n)],
                [radius * (1.0 - math.cos(i / n)) for i in range(n)])
 
 
-STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200)  # 100 m dead straight
+STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200)
 STANDSTILL_STUB_PLAN = _plan([0.0, 0.3], [0.0, 0.0])
-TURN_PLAN = _arc_plan(30.0)          # 0.033 1/m (~81 deg of wheel), 25 m of reach
-GENTLE_BEND_PLAN = _arc_plan(143.0)  # 0.007 1/m, barely bending
+TURN_PLAN = _arc_plan(30.0)
+GENTLE_BEND_PLAN = _arc_plan(143.0)
 
 
 def test_turn_lead_is_suppressed_only_during_applied_angle_control():
@@ -82,7 +88,6 @@ def test_guard_fades_out_across_the_speed_band():
   assert full < half < 0.0155
 
 
-# turning authority must never be reduced: a real turn's action agrees with its own plan
 @pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 3.0])
 def test_real_turns_tracking_their_own_plan_are_untouched(ratio):
   action = (1.0 / 30.0) * ratio
@@ -101,3 +106,37 @@ def test_guard_stands_down_when_the_plan_is_too_short_to_judge(plan):
 
 def test_zero_command_stays_zero():
   assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0, 1.2) == 0.0
+
+
+@pytest.mark.parametrize("bad_plan", [
+  _plan([0.0, math.nan, 20.0], [0.0, 0.0, 0.0]),
+  _plan([0.0, math.inf, 20.0], [0.0, 0.0, 0.0]),
+  _plan([0.0, 20.0], [0.0]),
+])
+def test_invalid_plan_data_leaves_curvature_untouched(bad_plan):
+  assert limit_curvature_to_plan(bad_plan, 0.0155, 1.2) == pytest.approx(0.0155)
+
+
+@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf])
+def test_nonfinite_guard_inputs_disarm(value):
+  assert update_twitch_guard(value, 1.0, False) == 0.0
+  assert update_twitch_guard(TWITCH_GUARD_DURATION, value, False) == 0.0
+
+
+def test_twitch_guard_arms_at_standstill_or_creep_speed():
+  assert update_twitch_guard(0.0, 0.0, True) == TWITCH_GUARD_DURATION
+  assert update_twitch_guard(0.0, 0.3, False) == TWITCH_GUARD_DURATION
+
+
+def test_twitch_guard_decays_after_pullaway_and_expires():
+  remaining = update_twitch_guard(0.0, 0.0, True)
+  remaining = update_twitch_guard(remaining, 1.0, False)
+  assert remaining == pytest.approx(TWITCH_GUARD_DURATION - DT_CTRL)
+
+  for _ in range(int(TWITCH_GUARD_DURATION / DT_CTRL) + 1):
+    remaining = update_twitch_guard(remaining, 1.0, False)
+  assert remaining == 0.0
+
+
+def test_twitch_guard_does_not_arm_while_moving():
+  assert update_twitch_guard(0.0, 1.0, False) == 0.0

selfdrive/modeld/compile_modeld.py

@@ -204,13 +204,14 @@ def _packed_policy_shapes(input_shapes, include_prev_feature=False):
     shapes[key] = tuple(shape)
   if include_prev_feature:
     features_shape = input_shapes["features_buffer"]
-    shapes["prev_feat"] = (features_shape[0], features_shape[2])
+    shapes["prev_feat"] = (features_shape[0], math.prod(features_shape[2:]))
   return shapes, [math.prod(shape) for shape in shapes.values()]
 
 
 def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
   queues, npy = make_warp_input_queues(vision_input_shapes, frame_skip, device)
   features_shape = policy_input_shapes["features_buffer"]
+  feature_dim = math.prod(features_shape[2:])
   desire_key = _detect_desire_key(policy_input_shapes)
   desire_shape = policy_input_shapes[desire_key]
   packed_shapes, packed_sizes = _packed_policy_shapes(policy_input_shapes)
@@ -224,7 +225,7 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip
   })
   queues.update({
     "feat_q": Tensor(
-      np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], features_shape[2]), dtype=np.float32),
+      np.zeros((frame_skip * (features_shape[1] - 1) + 1, features_shape[0], feature_dim), dtype=np.float32),
       device=device,
     ).contiguous().realize(),
     "desire_q": Tensor(
@@ -239,6 +240,7 @@ def make_split_input_queues(vision_input_shapes, policy_input_shapes, frame_skip
 def make_supercombo_input_queues(input_shapes, frame_skip, device):
   queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)
   features_shape = input_shapes["features_buffer"]
+  feature_dim = math.prod(features_shape[2:])
   desire_key = _detect_desire_key(input_shapes)
   desire_shape = input_shapes[desire_key]
   packed_shapes, packed_sizes = _packed_policy_shapes(input_shapes, include_prev_feature=True)
@@ -252,7 +254,7 @@ def make_supercombo_input_queues(input_shapes, frame_skip, device):
   })
   queues.update({
     "feat_q": Tensor(
-      np.zeros((frame_skip * features_shape[1], features_shape[0], features_shape[2]), dtype=np.float32),
+      np.zeros((frame_skip * features_shape[1], features_shape[0], feature_dim), dtype=np.float32),
       device=device,
     ).contiguous().realize(),
     "desire_q": Tensor(
@@ -335,6 +337,7 @@ def make_run_split_policy(vision_runner, policy_runners, metadata, policy_order,
     vision_output = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast("float32")
     new_feature = vision_output[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
     features_buffer = shift_and_sample(feat_q, new_feature, sample_skip_fn)
+    features_buffer = features_buffer.reshape(policy_metadata["input_shapes"]["features_buffer"])
 
     policy_inputs = {
       "features_buffer": features_buffer,
@@ -388,6 +391,7 @@ def make_run_supercombo(model_runner, metadata, frame_skip, image_history_pipeli
     features_buffer = shift_and_sample(
       feat_q, previous_feature.reshape(1, 1, -1), sample_skip_fn,
     )
+    features_buffer = features_buffer.reshape(input_shapes["features_buffer"])
     model_inputs = {
       road_key: img,
       wide_key: big_img,

selfdrive/modeld/modeld.py

@@ -1,4 +1,6 @@
 #!/usr/bin/env python3
+from collections.abc import Callable
+import ctypes
 from functools import cached_property
 import os
 import struct
@@ -159,8 +161,10 @@ class ChestnutState:
     if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
       try:
         smu = Device["AMD"].iface.dev_impl.smu
+        metrics_t = smu.smu_mod.SmuMetricsExternal_t
         smu._send_msg(smu.smu_mod.PPSMC_MSG_TransferTableSmu2Dram, smu.smu_mod.TABLE_SMU_METRICS, timeout=100)
-        metrics = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics
+        metrics_buf = bytearray(smu.adev.vram.view(smu.driver_table_paddr, ctypes.sizeof(metrics_t))[:])
+        metrics = metrics_t.from_buffer(metrics_buf).SmuMetrics
         self.metrics = {
           "tempC": metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT],
           "memoryTempC": metrics.AvgTemperature[smu.smu_mod.TEMP_MEM],
@@ -277,10 +281,11 @@ def _close_tinygrad_disk_cache_connection() -> None:
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                           lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool,
                           is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles,
-                          lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS) -> log.ModelDataV2.Action:
-    if is_v14 or is_v15:
+                          lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS,
+                          is_v16: bool = False) -> log.ModelDataV2.Action:
+    if is_v14 or is_v15 or is_v16:
       desired_curv_unscaled, desired_accel = model_output['action'][0]
-      if is_v15:
+      if is_v15 or is_v16:
         desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2
       else:
         desired_curvature = float(desired_curv_unscaled) / 100.0
@@ -464,6 +469,7 @@ class ModelState:
     self.is_v9 = self.policy_generation == "v9"
     self.is_v14 = self.policy_generation == "v14"
     self.is_v15 = self.policy_generation == "v15"
+    self.is_v16 = self.policy_generation == "v16"
     self.mlsim = is_tinygrad_model_version(self.policy_generation)
     if write_model_version:
       params.put("ModelVersion", self.policy_generation)
@@ -567,7 +573,8 @@ class ModelState:
     self._reset_state()
 
   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:
+          inputs: dict[str, np.ndarray], prepare_only: bool,
+          after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray] | None:
     frames: dict[str, Tensor] = {}
     for key, buf in bufs.items():
       ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data
@@ -613,11 +620,12 @@ class ModelState:
         img=img,
         big_img=big_img,
       )
+    if after_enqueue is not None:
+      after_enqueue()
     outputs = [output.numpy().flatten() for output in output_tensors]
 
     if self.uses_external_gpu and any(not np.isfinite(output).all() for output in outputs):
-      cloudlog.error("external GPU model output not finite, dropping frame")
-      return None
+      raise RuntimeError("external GPU model output not finite")
 
     if self.model_type == "supercombo":
       model_output = outputs[0]
@@ -919,7 +927,17 @@ def main(demo=False):
 
     mt1 = time.perf_counter()
     try:
-      model_output = model.run(bufs, transforms, inputs, prepare_only)
+      send_chestnut = (
+        chestnut_state is not None and
+        run_count % round(ModelConstants.MODEL_FREQ / SERVICE_LIST["chestnutState"].frequency) == 0
+      )
+      model_output = model.run(
+        bufs,
+        transforms,
+        inputs,
+        prepare_only,
+        chestnut_state.send if send_chestnut else None,
+      )
     except Exception:
       if not external_gpu_active or small_model is None:
         raise
@@ -953,7 +971,7 @@ def main(demo=False):
         lat_action_t,
         long_action_t,
         v_ego, model.mlsim, model.is_v9, model.is_v14, model.is_v15, starpilot_toggles,
-        lat_smooth_seconds, long_smooth_seconds,
+        lat_smooth_seconds, long_smooth_seconds, is_v16=model.is_v16,
       )
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
@@ -982,9 +1000,6 @@ def main(demo=False):
     if sm.updated['starpilotPlan']:
       starpilot_toggles = get_starpilot_toggles(sm)
 
-    if chestnut_state is not None and run_count % round(ModelConstants.MODEL_FREQ / SERVICE_LIST["chestnutState"].frequency) == 0:
-      chestnut_state.send()
-
 if __name__ == "__main__":
   try:
     import argparse

selfdrive/modeld/tests/test_usbgpu_helpers.py

@@ -3,6 +3,7 @@ from types import MethodType
 from types import SimpleNamespace
 
 import numpy as np
+import pytest
 
 from openpilot.selfdrive.modeld import modeld
 from openpilot.selfdrive.modeld.helpers import dump_oob, load_oob, tinygrad_dev_config
@@ -37,17 +38,18 @@ def test_external_gpu_uses_a_longer_load_watchdog():
   assert modeld.BIG_MODEL_RUN_WAIT_TIMEOUT_MS == 3000
 
 
-def test_external_gpu_signal_wait_matches_upstream_busy_poll():
+def test_external_gpu_signal_wait_yields_between_usb_polls(monkeypatch):
   from tinygrad.runtime import ops_amd
 
   sleeps = []
+  monkeypatch.setattr(ops_amd.time, "sleep", sleeps.append)
   signal = ops_amd.AMDSignal.__new__(ops_amd.AMDSignal)
   signal.should_return = False
-  signal.owner = SimpleNamespace(is_usb=lambda: True, iface=SimpleNamespace(sleep=sleeps.append))
+  signal.owner = SimpleNamespace(is_usb=lambda: True, iface=SimpleNamespace(sleep=lambda _: None))
 
   signal._sleep(0)
 
-  assert sleeps == []
+  assert sleeps == [ops_amd.AMD_USB_POLL_US / 1e6]
 
 
 def test_native_amd_signal_keeps_existing_short_wait_behavior():
@@ -201,7 +203,7 @@ def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):
   ]
 
 
-def test_external_gpu_nonfinite_outputs_are_dropped_without_escalating(monkeypatch):
+def test_external_gpu_nonfinite_outputs_trigger_fallback(monkeypatch):
   class FakeTensor:
     @staticmethod
     def from_blob(*_args, **_kwargs):
@@ -235,13 +237,7 @@ def test_external_gpu_nonfinite_outputs_are_dropped_without_escalating(monkeypat
   state.image_history_pipeline = modeld.IMAGE_HISTORY_IN_POLICY
   state.warp_enqueue = lambda **_kwargs: object()
   state.run_policy = lambda **_kwargs: (FakeOutput(),)
-  state._reset_state = MethodType(
-    lambda self: (_ for _ in ()).throw(AssertionError("upstream does not reset or escalate transient non-finite output")),
-    state,
-  )
-
   monkeypatch.setattr(modeld, "Tensor", FakeTensor)
-  monkeypatch.setattr(modeld.cloudlog, "error", lambda *_args, **_kwargs: None)
   buffers = {
     "img": SimpleNamespace(data=bytearray(4)),
     "big_img": SimpleNamespace(data=bytearray(4)),
@@ -252,8 +248,10 @@ def test_external_gpu_nonfinite_outputs_are_dropped_without_escalating(monkeypat
   }
   inputs = {"desire_pulse": np.zeros(8, dtype=np.float32)}
 
-  for _ in range(10):
-    assert state.run(buffers, transforms, inputs, False) is None
+  callbacks = []
+  with pytest.raises(RuntimeError, match="external GPU model output not finite"):
+    state.run(buffers, transforms, inputs, False, lambda: callbacks.append("sent"))
+  assert callbacks == ["sent"]
 
 
 def test_out_of_band_artifact_round_trip():

selfdrive/ui/layouts/settings/starpilot/longitudinal.py

@@ -686,12 +686,12 @@ class StarPilotLongitudinalLayout(_SettingsPage):
     self._curve_speed_controller_rows = [
       SettingRow("CalibratedLatAccel", "value", tr_noop("Calibrated Lateral Accel"),
                  subtitle=tr_noop("The learned lateral acceleration from collected driving data. Higher values allow faster cornering."),
-                 get_value=lambda: f"{self._params.get_float('CalibratedLateralAcceleration'):.2f} m/s",
+                 get_value=lambda: f"{self._params_memory.get_float('CalibratedLateralAcceleration'):.2f} m/s",
                  on_click=None,
                  visible=csc_on),
       SettingRow("CalibrationProgress", "value", tr_noop("Calibration Progress"),
                  subtitle=tr_noop("How much curve data has been collected. Normal for the value to stay low."),
-                 get_value=lambda: f"{self._params.get_float('CalibrationProgress'):.2f}%",
+                 get_value=lambda: f"{self._params_memory.get_float('CalibrationProgress'):.2f}%",
                  on_click=None,
                  visible=csc_on),
       SettingRow("ResetCurve", "action", tr_noop("Reset Curve Data"),

selfdrive/ui/mici/layouts/settings/software.py

@@ -1,6 +1,11 @@
+import json
 import os
 import threading
+import time
+import urllib.error
+import urllib.request
 from collections.abc import Callable
+from dataclasses import dataclass
 from enum import IntEnum
 
 import pyray as rl
@@ -18,6 +23,40 @@ from openpilot.system.ui.widgets.label import UnifiedLabel
 from openpilot.system.ui.widgets.scroller import NavScroller
 
 UPDATER_TIMEOUT = 10.0
+FAST_UPDATE_HOLD_SECONDS = 0.8
+FAST_UPDATE_POLL_SECONDS = 0.5
+FAST_UPDATE_REQUEST_TIMEOUT = 5.0
+
+
+@dataclass
+class FastUpdateDisplayState:
+  stage: str = "idle"
+  message: str = ""
+  error: str = ""
+
+
+def _galaxy_api_url(path: str) -> str:
+  port = os.getenv("SP_GALAXY_PORT", "8082")
+  return f"http://127.0.0.1:{port}/api/update/fast{path}"
+
+
+def _galaxy_fast_update_request(path: str = "", method: str = "GET") -> dict:
+  request = urllib.request.Request(_galaxy_api_url(path), method=method)
+  try:
+    with urllib.request.urlopen(request, timeout=FAST_UPDATE_REQUEST_TIMEOUT) as response:
+      payload = json.loads(response.read().decode("utf-8"))
+  except urllib.error.HTTPError as error:
+    try:
+      payload = json.loads(error.read().decode("utf-8"))
+    except (json.JSONDecodeError, UnicodeDecodeError):
+      payload = {}
+    raise RuntimeError(payload.get("error") or str(error)) from error
+  except (OSError, urllib.error.URLError, json.JSONDecodeError, UnicodeDecodeError) as error:
+    raise RuntimeError(f"Galaxy fast update unavailable: {error}") from error
+
+  if not isinstance(payload, dict):
+    raise RuntimeError("Galaxy returned an invalid fast update response")
+  return payload
 
 
 def _split_description(desc: str) -> tuple[str, str, str, str] | None:
@@ -101,6 +140,10 @@ class CheckUpdateButton(BigButton):
     self._waiting_for_updater_t: float | None = None
     self._hide_value_t: float | None = None
     self._state: UpdaterState = UpdaterState.IDLE
+    self._press_start_t: float | None = None
+    self._press_action = ""
+    self._fast_update_state = FastUpdateDisplayState()
+    self._fast_update_state_lock = threading.Lock()
 
     ui_state.add_offroad_transition_callback(self.offroad_transition)
 
@@ -108,9 +151,24 @@ class CheckUpdateButton(BigButton):
     if ui_state.is_offroad():
       self.set_enabled(True)
 
+  def _handle_mouse_press(self, mouse_pos: MousePos):
+    super()._handle_mouse_press(mouse_pos)
+    self._press_start_t = rl.get_time()
+    self._press_action = self.get_value()
+
   def _handle_mouse_release(self, mouse_pos: MousePos):
+    held_for = 0.0 if self._press_start_t is None else rl.get_time() - self._press_start_t
+    self._press_start_t = None
     super()._handle_mouse_release(mouse_pos)
 
+    if held_for >= FAST_UPDATE_HOLD_SECONDS:
+      self._show_fast_update_confirmation()
+      return
+
+    if self._get_fast_update_state().stage == "error":
+      self._set_fast_update_state(stage="idle")
+      return
+
     if not system_time_valid():
       dlg = BigDialog("", tr("Please connect to Wi-Fi to update."))
       gui_app.push_widget(dlg)
@@ -121,13 +179,75 @@ class CheckUpdateButton(BigButton):
     self.set_icon(self._txt_update_icon)
 
     def run():
-      if self.get_value() == "download update":
+      if self._press_action == "download update":
         _request_update_download()
       else:
         _request_update_check()
 
     threading.Thread(target=run, daemon=True).start()
 
+  def _show_fast_update_confirmation(self):
+    if not system_time_valid():
+      gui_app.push_widget(BigDialog("", tr("Please connect to Wi-Fi to update.")))
+      return
+    if ui_state.started:
+      return
+
+    gui_app.push_widget(BigConfirmationDialog(
+      "slide to\nfast update",
+      self._txt_update_icon,
+      self._start_fast_update,
+      red=True,
+    ))
+
+  def _set_fast_update_state(self, *, stage: str, message: str = "", error: str = ""):
+    with self._fast_update_state_lock:
+      self._fast_update_state = FastUpdateDisplayState(stage, message, error)
+
+  def _get_fast_update_state(self) -> FastUpdateDisplayState:
+    with self._fast_update_state_lock:
+      state = self._fast_update_state
+      return FastUpdateDisplayState(state.stage, state.message, state.error)
+
+  def _start_fast_update(self):
+    if ui_state.started:
+      return
+
+    state = self._get_fast_update_state()
+    if state.stage not in ("idle", "error"):
+      return
+
+    self._set_fast_update_state(stage="starting", message="starting fast update...")
+    threading.Thread(target=self._run_fast_update, daemon=True).start()
+
+  def _run_fast_update(self):
+    try:
+      _galaxy_fast_update_request(method="POST")
+
+      while True:
+        status = _galaxy_fast_update_request("/status")
+        stage = str(status.get("stage") or "updating")
+        error = str(status.get("lastError") or "").strip()
+        message = str(
+          status.get("progressDetail") or
+          status.get("progressLabel") or
+          status.get("message") or
+          "fast update in progress..."
+        ).strip()
+
+        if error or stage == "error":
+          self._set_fast_update_state(stage="error", message="fast update failed", error=error or message)
+          return
+
+        self._set_fast_update_state(stage=stage, message=message)
+        if not bool(status.get("running")):
+          return
+        time.sleep(FAST_UPDATE_POLL_SECONDS)
+    except Exception as error:
+      current = self._get_fast_update_state()
+      if current.stage != "rebooting":
+        self._set_fast_update_state(stage="error", message="fast update failed", error=str(error))
+
   def set_value(self, value: str):
     super().set_value(value)
     self.set_text("" if value else "check for update")
@@ -136,9 +256,25 @@ class CheckUpdateButton(BigButton):
     super()._update_state()
 
     if ui_state.started:
+      self._press_start_t = None
       self.set_enabled(False)
       return
 
+    fast_update_state = self._get_fast_update_state()
+    if fast_update_state.stage != "idle":
+      self.set_rotate_icon(fast_update_state.stage not in ("error", "rebooting"))
+      if fast_update_state.stage == "error":
+        self.set_enabled(True)
+        self.set_value(fast_update_state.error or fast_update_state.message)
+      elif fast_update_state.stage == "rebooting":
+        self.set_enabled(False)
+        self.set_value("update complete\nrebooting...")
+      else:
+        self.set_enabled(False)
+        self.set_value(fast_update_state.message or "fast update in progress...")
+      self.set_text("fast update")
+      return
+
     updater_state = ui_state.params.get("UpdaterState") or ""
 
     if self._state == UpdaterState.WAITING_FOR_UPDATER:

selfdrive/ui/mici/layouts/settings/tests/test_software_fast_update.py

@@ -0,0 +1,98 @@
+from types import SimpleNamespace
+
+from openpilot.selfdrive.ui.mici.layouts.settings import software
+from openpilot.selfdrive.ui.mici.widgets.button import BigButton
+
+
+def _make_button() -> software.CheckUpdateButton:
+  button = object.__new__(software.CheckUpdateButton)
+  button._press_start_t = None
+  button._press_action = ""
+  button._fast_update_state = software.FastUpdateDisplayState()
+  button._fast_update_state_lock = software.threading.Lock()
+  return button
+
+
+def test_fast_update_uses_local_galaxy_api(monkeypatch):
+  monkeypatch.delenv("SP_GALAXY_PORT", raising=False)
+
+  assert software._galaxy_api_url("") == "http://127.0.0.1:8082/api/update/fast"
+  assert software._galaxy_api_url("/status") == "http://127.0.0.1:8082/api/update/fast/status"
+
+
+def test_long_press_opens_fast_update_confirmation(monkeypatch):
+  button = _make_button()
+  button._press_start_t = 1.0
+  confirmation_calls = []
+
+  monkeypatch.setattr(software.rl, "get_time", lambda: 1.0 + software.FAST_UPDATE_HOLD_SECONDS)
+  monkeypatch.setattr(BigButton, "_handle_mouse_release", lambda *_args: None)
+  monkeypatch.setattr(button, "_show_fast_update_confirmation", lambda: confirmation_calls.append(True))
+
+  button._handle_mouse_release(SimpleNamespace())
+
+  assert confirmation_calls == [True]
+
+
+def test_short_press_keeps_normal_updater_path(monkeypatch):
+  button = _make_button()
+  button._press_start_t = 1.0
+  button._press_action = "download update"
+  downloads = []
+  confirmations = []
+
+  monkeypatch.setattr(software.rl, "get_time", lambda: 1.0 + software.FAST_UPDATE_HOLD_SECONDS - 0.1)
+  monkeypatch.setattr(BigButton, "_handle_mouse_release", lambda *_args: None)
+  monkeypatch.setattr(software, "system_time_valid", lambda: True)
+  monkeypatch.setattr(software, "_request_update_download", lambda: downloads.append(True))
+  monkeypatch.setattr(button, "_show_fast_update_confirmation", lambda: confirmations.append(True))
+
+  class ImmediateThread:
+    def __init__(self, target, daemon):
+      self.target = target
+
+    def start(self):
+      self.target()
+
+  monkeypatch.setattr(software.threading, "Thread", ImmediateThread)
+  button.set_enabled = lambda *_args: None
+  button.set_icon = lambda *_args: None
+  button._state = software.UpdaterState.IDLE
+  button._txt_update_icon = None
+
+  button._handle_mouse_release(SimpleNamespace())
+
+  assert downloads == [True]
+  assert confirmations == []
+
+
+def test_fast_update_worker_uses_galaxy_endpoint(monkeypatch):
+  button = _make_button()
+  requests = []
+  responses = [
+    {"message": "started"},
+    {
+      "running": True,
+      "stage": "updating",
+      "progressDetail": "Fetching latest shallow commit...",
+      "lastError": "",
+    },
+    {
+      "running": False,
+      "stage": "rebooting",
+      "progressDetail": "Update complete. Please wait for device to reboot.",
+      "lastError": "",
+    },
+  ]
+
+  def request(path="", method="GET"):
+    requests.append((path, method))
+    return responses.pop(0)
+
+  monkeypatch.setattr(software, "_galaxy_fast_update_request", request)
+  monkeypatch.setattr(software.time, "sleep", lambda *_args: None)
+
+  button._run_fast_update()
+
+  assert requests == [("", "POST"), ("/status", "GET"), ("/status", "GET")]
+  assert button._get_fast_update_state().stage == "rebooting"

selfdrive/ui/mici/onroad/hud_renderer.py

@@ -158,7 +158,7 @@ class HudRenderer(Widget):
     self._txt_exclamation_point: rl.Texture = gui_app.texture('icons_mici/exclamation_point.png', 44, 44)
     self._txt_egpu_loading: rl.Texture = gui_app.texture('icons_mici/egpu_loading.png', 60, 44)
     self._txt_egpu_green: rl.Texture = gui_app.texture('icons_mici/egpu_green.png', 60, 44)
-    self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 60, 44)
+    self._txt_egpu_orange: rl.Texture = gui_app.texture('icons_mici/egpu_orange.png', 75, 44)
     self._txt_egpu_crossed: rl.Texture = gui_app.texture('icons_mici/egpu_crossed.png', 60, 52)
     self._egpu_icon: rl.Texture | None = None
 

selfdrive/ui/mici/onroad/model_renderer.py

@@ -6,6 +6,7 @@ from cereal import messaging, car
 from dataclasses import dataclass, field
 from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.controls.lib.lane_centering import get_lane_centering_visual_direction
 from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
 from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha
 from openpilot.selfdrive.ui.onroad.starpilot.rainbow_path import RainbowPath
@@ -361,7 +362,31 @@ class ModelRenderer(Widget):
 
     return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
 
-  def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, bool]:
+  def _lane_centering_direction(self) -> int:
+    toggles = ui_state.starpilot_toggles
+    sm = ui_state.sm
+    if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
+      return 0
+
+    car_state = sm["carState"]
+    applied_correction = None
+    if sm.valid.get("controlsState", False):
+      try:
+        applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
+      except (AttributeError, TypeError, ValueError):
+        pass
+    return get_lane_centering_visual_direction(
+      sm["modelV2"], car_state.vEgo,
+      toggles.get("lane_center_offset", 0.0),
+      toggles.get("lane_centering_e2e_authority", 1.0),
+      bool(toggles.get("lane_centering", False)),
+      ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active,
+      bool(toggles.get("lane_centering_pause_on_signal", True)),
+      bool(car_state.leftBlinker or car_state.rightBlinker),
+      applied_correction,
+    )
+
+  def _lane_line_palette(self) -> tuple[bool, rl.Color, rl.Color, int]:
     stock_scheme = is_stock_color_scheme(self._params)
     line_status = UIStatus.ENGAGED if ui_state.status == UIStatus.DISENGAGED and ui_state.always_on_lateral_active else ui_state.status
 
@@ -374,15 +399,15 @@ class ModelRenderer(Widget):
     if lane_color is None:
       lane_color = STOCK_LANE_LINES_COLOR if stock_scheme else get_theme_color("LaneLines", STOCK_LANE_LINES_COLOR)
 
-    lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and (
-      ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active
-    )
-    return stock_scheme, edge_color, lane_color, lane_centering_active
+    lane_centering_direction = self._lane_centering_direction()
+    return stock_scheme, edge_color, lane_color, lane_centering_direction
 
   def _get_ll_color(self, prob: float, adjacent: bool, left: bool, stock_scheme: bool,
-                    edge_color: rl.Color, lane_color: rl.Color, lane_centering_active: bool = False):
+                    edge_color: rl.Color, lane_color: rl.Color, lane_centering_direction: int = 0):
     alpha = np.clip(prob, 0.0, 0.7)
-    if lane_centering_active:
+    lane_centering_line = adjacent and ((lane_centering_direction > 0 and not left) or
+                                        (lane_centering_direction < 0 and left))
+    if lane_centering_line:
       color = rl.Color(OCEAN_BLUE_LANE_LINES_COLOR.r, OCEAN_BLUE_LANE_LINES_COLOR.g,
                        OCEAN_BLUE_LANE_LINES_COLOR.b, int(alpha * OCEAN_BLUE_LANE_LINES_COLOR.a))
     elif adjacent:
@@ -408,13 +433,13 @@ class ModelRenderer(Widget):
   def _draw_lane_lines(self):
     """Draw lane lines and road edges"""
     """Two closest lines should be green (lane line or road edges)"""
-    stock_scheme, edge_color, lane_color, lane_centering_active = self._lane_line_palette()
+    stock_scheme, edge_color, lane_color, lane_centering_direction = self._lane_line_palette()
     for i, lane_line in enumerate(self._lane_lines):
       if lane_line.projected_points.size == 0:
         continue
 
       color = self._get_ll_color(float(self._lane_line_probs[i]), i in (1, 2), i in (0, 1),
-                                 stock_scheme, edge_color, lane_color, lane_centering_active)
+                                 stock_scheme, edge_color, lane_color, lane_centering_direction)
       draw_polygon(self._rect, lane_line.projected_points, color)
 
     for i, road_edge in enumerate(self._road_edges):

selfdrive/ui/onroad/model_renderer.py

@@ -5,6 +5,7 @@ from cereal import messaging, car
 from dataclasses import dataclass, field
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.constants import CV
+from openpilot.selfdrive.controls.lib.lane_centering import get_lane_centering_visual_direction
 from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
 from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha
 from openpilot.selfdrive.ui.onroad.radar_tracks import project_radar_points
@@ -369,15 +370,35 @@ class ModelRenderer(Widget):
 
     return LeadVehicle(glow=glow, chevron=chevron, fill_alpha=int(fill_alpha))
 
+  def _lane_centering_direction(self) -> int:
+    toggles = ui_state.starpilot_toggles
+    sm = ui_state.sm
+    if not sm.valid.get("modelV2", False) or not sm.valid.get("carState", False):
+      return 0
+
+    car_state = sm["carState"]
+    applied_correction = None
+    if sm.valid.get("controlsState", False):
+      try:
+        applied_correction = sm["controlsState"].desiredCurvature - sm["modelV2"].action.desiredCurvature
+      except (AttributeError, TypeError, ValueError):
+        pass
+    return get_lane_centering_visual_direction(
+      sm["modelV2"], car_state.vEgo,
+      toggles.get("lane_center_offset", 0.0),
+      toggles.get("lane_centering_e2e_authority", 1.0),
+      bool(toggles.get("lane_centering", False)),
+      ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active,
+      bool(toggles.get("lane_centering_pause_on_signal", True)),
+      bool(car_state.leftBlinker or car_state.rightBlinker),
+      applied_correction,
+    )
+
   def _draw_lane_lines(self):
     """Draw lane lines and road edges"""
-    lane_centering_active = bool(ui_state.starpilot_toggles.get("lane_centering", False)) and (
-      ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active
-    )
+    lane_centering_direction = self._lane_centering_direction()
     lane_lines_override = get_param_color(self._params, "LaneLinesColor", STOCK_LANE_LINES_COLOR.a)
-    if lane_centering_active:
-      lane_lines_color = OCEAN_BLUE_LANE_LINES_COLOR
-    elif lane_lines_override is not None:
+    if lane_lines_override is not None:
       lane_lines_color = lane_lines_override
     elif is_stock_color_scheme(self._params):
       lane_lines_color = STOCK_LANE_LINES_COLOR
@@ -389,7 +410,10 @@ class ModelRenderer(Widget):
         continue
 
       alpha = np.clip(self._lane_line_probs[i], 0.0, 0.7)
-      color = with_alpha(lane_lines_color, int(alpha * lane_lines_color.a))
+      lane_centering_line = (lane_centering_direction > 0 and i == 2) or \
+                            (lane_centering_direction < 0 and i == 1)
+      line_color = OCEAN_BLUE_LANE_LINES_COLOR if lane_centering_line else lane_lines_color
+      color = with_alpha(line_color, int(alpha * line_color.a))
       draw_polygon(self._rect, lane_line.projected_points, color)
 
     for i, road_edge in enumerate(self._road_edges):

selfdrive/ui/onroad/starpilot/starpilot_border.py

@@ -57,9 +57,7 @@ def _csc_state():
 
   plan = sm["starpilotPlan"]
   params = ui_state.ui_params
-  # A pending speed limit flashes the speed limit sign, not the border -- it has no reason
-  # to blank this, and doing so hid real curve slowdowns for the whole confirmation window.
-  if not params.get_bool("ShowCSCStatus"):
+  if plan.speedLimitChanged or not params.get_bool("ShowCSCStatus"):
     return None
 
   car_state = sm["carState"]

starpilot/common/accel_profile.py

@@ -3,6 +3,8 @@ from __future__ import annotations
 
 import math
 
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel
+
 ACCELERATION_PROFILES = {
   "STANDARD": 0,
   "ECO": 1,

starpilot/common/assets/device_settings_layout.json

@@ -1043,30 +1043,6 @@
         "parent_key": "CurveSpeedController",
         "settings_tier": "simple"
       },
-      {
-        "key": "CalibratedLateralAcceleration",
-        "label": "Calibrated Lateral Accel",
-        "description": "The learned lateral acceleration from collected driving data. Higher values allow faster cornering.",
-        "picker_description": "Learned cornering comfort from your driving data.",
-        "data_type": "float",
-        "ui_type": "readout",
-        "precision": 2,
-        "unit": " m/s²",
-        "parent_key": "CurveSpeedController",
-        "settings_tier": "simple"
-      },
-      {
-        "key": "CalibrationProgress",
-        "label": "Calibration Progress",
-        "description": "How much curve data has been collected. Normal for the value to stay low.",
-        "picker_description": "How much curve data has been collected.",
-        "data_type": "float",
-        "ui_type": "readout",
-        "precision": 2,
-        "unit": "%",
-        "parent_key": "CurveSpeedController",
-        "settings_tier": "simple"
-      },
       {
         "key": "ResetCurveData",
         "label": "Reset Curve Data",

starpilot/common/starpilot_utilities.py

@@ -138,23 +138,6 @@ def calculate_road_curvature(modelData, v_ego):
   return float(predicted_lateral_acc / max(v_ego, 1)**2), max(time_to_curve, 1)
 
 
-PROFILE_MIN_SPEED = 3.0     # m/s — model points planned near standstill have unusable curvature
-PROFILE_MAX_CURVATURE = 0.1
-
-
-def extract_curve_profile(modelData):
-  orientation_rate = np.abs(np.array(modelData.orientationRate.z))
-  velocity = np.array(modelData.velocity.x)
-  distances = np.array(modelData.position.x)
-
-  # k = psi_dot / v per point, against the model's own planned speed so its
-  # slowdowns don't inflate the curvature
-  curvatures = orientation_rate / np.clip(velocity, PROFILE_MIN_SPEED, None)
-  curvatures = np.where(velocity < PROFILE_MIN_SPEED, 0.0, np.minimum(curvatures, PROFILE_MAX_CURVATURE))
-
-  return curvatures, distances
-
-
 def clean_model_name(name):
   return name.replace("(Default)", "").strip()
 

starpilot/controls/lib/curve_speed_controller.py

@@ -2,97 +2,18 @@
 import numpy as np
 
 from openpilot.common.constants import CV
-from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 
-from openpilot.starpilot.common.starpilot_variables import (
-  CITY_SPEED_LIMIT,
-  CRUISING_SPEED,
-  DEFAULT_LATERAL_ACCELERATION,
-  PLANNER_TIME,
-)
+from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME
 
 CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL
 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
-
-# braking distance is (v^2 - v_curve^2) / (2 * this), so lower starts the slowdown
-# sooner and spreads it further.
-CSC_APPROACH_DECEL = 0.3
-CSC_TARGET_UP_RATE = 3.0
-CSC_TARGET_DOWN_RATE = 2.5
-CSC_TARGET_FILTER_RC = 0.4
-CSC_EGO_HEADROOM = 2.0            # target never trails below v_ego, so CSC can't drag re-acceleration
-CSC_RELEASE_DEBOUNCE = 0.25       # s the envelope must stay clear before that floor applies
-CSC_ACTIVE_ON_DELTA = 0.5
-CSC_ACTIVE_OFF_DELTA = 0.25
-CSC_GLOW_ON_DELTA = 1.0           # ~2.2 mph; separate from CSC_ACTIVE_ON_DELTA (training) so a trivial graze doesn't light the glow
-CSC_GLOW_HOLD_TIME = 3.0          # s the cap must stay released before the glow clears, so it doesn't flicker on/off across one curve
-
-CSC_COUNT_CAP = 600               # EMA floor: samples beyond this stop shrinking the update step
-CSC_PRIOR_COUNT = 100             # bucket count at which learned data and the prior have equal weight
-CSC_LAT_ACCEL_MIN = 1.2
-CSC_LAT_ACCEL_MAX = 3.2
-CSC_NUDGE = 0.15
-CSC_NUDGE_WEIGHT = 20             # counts a single override pseudo-sample is worth
-CSC_OVERRIDE_WATCH_TIME = 6.0     # s to keep watching what the driver holds after they reject a cut
-CSC_TRAINING_QUIET_TIME = 5.0     # blocks passive samples after CSC limited speed, so it can't learn its own cap
-CSC_TRAINING_SETTLE_TIME = 2.0    # driver-owned seconds before a sample counts, so it isn't openpilot's leftover speed
-CSC_COMFORT_MARGIN = 1.0          # 1.0 = matches the driver's own learned cornering, no extra cushion
-
-# The model under-reads curvature at range: measured 0.81x actual beyond ~75 m. That holds
-# only where the reading is already firm -- weak distant readings carry no usable magnitude
-# (0.40x median with a 14:1 spread), so scaling those would amplify noise, not signal.
-CSC_FARFIELD_MIN_CURVATURE = 0.004   # ~R 250 m; at this strength range readings were 85%+ reliable
-CSC_FARFIELD_MIN_DISTANCE = 30.0     # inside this the model is already accurate
-CSC_FARFIELD_GAIN = 1.23             # 1 / 0.81
-
-# Buckets are spaced geometrically, not linearly: comfort is a speed and v = sqrt(a/k), so equal
-# steps in k give wildly uneven speed resolution. Regridding is safe -- _normalize_curvature_data
-# re-buckets stored keys on load.
-MIN_CURVATURE = 0.0005            # R 2000 m — gentler than this never constrains anything
-MAX_CURVATURE = 0.02              # R 50 m — already well below the CSC_MIN_SPEED floor
-CURVATURE_BUCKETS = 24            # keeps every bucket under ~7 mph wide without over-thinning the data
-ROUNDING_PRECISION = 6
-CURVATURE_GRID = MIN_CURVATURE * np.power(MAX_CURVATURE / MIN_CURVATURE,
-                                          np.arange(CURVATURE_BUCKETS) / (CURVATURE_BUCKETS - 1))
-LOG_CURVATURE_GRID = np.log(CURVATURE_GRID)
-
-# Drivers accept more lateral acceleration in sharp slow corners than in highway sweepers.
-PRIOR_CURVATURE_BP = [0.001, 0.003, 0.01, 0.03, 0.1]
-PRIOR_LAT_ACCEL_V = [1.5, 1.8, 2.2, 2.6, 2.9]
-
-
-def weighted_isotonic(values, weights):
-  """Weighted non-decreasing fit (pool adjacent violators).
-
-  Keeps comfort from falling as curves tighten, without letting a sparse bucket
-  overrule a well-sampled neighbour the way a running maximum would.
-  """
-  block_values: list[float] = []
-  block_weights: list[float] = []
-  block_sizes: list[int] = []
-
-  for value, weight in zip(values, weights, strict=True):
-    block_values.append(float(value))
-    block_weights.append(float(weight))
-    block_sizes.append(1)
-
-    while len(block_values) > 1 and block_values[-2] > block_values[-1]:
-      merged_weight = block_weights[-2] + block_weights[-1]
-      merged_value = ((block_values[-2] * block_weights[-2]) + (block_values[-1] * block_weights[-1])) / merged_weight
-      block_values.pop()
-      block_weights.pop()
-      merged_size = block_sizes.pop()
-      block_values[-1] = merged_value
-      block_weights[-1] = merged_weight
-      block_sizes[-1] += merged_size
-
-  fitted = np.empty(len(values))
-  index = 0
-  for value, size in zip(block_values, block_sizes, strict=True):
-    fitted[index:index + size] = value
-    index += size
-  return fitted
+CSC_MAX_DECEL_RATE = 1.5
+MAX_CURVATURE = 0.1
+MIN_CURVATURE = 0.001
+PERCENTILE = 90
+ROUNDING_PRECISION = 5
+STEP = 0.001
 
 
 def is_user_overriding_longitudinal(sm):
@@ -121,42 +42,26 @@ class CurveSpeedController:
     self.starpilot_planner = StarPilotVCruise.starpilot_planner
 
     self.enable_training = False
-    self.nudge_applied = False
-
-    self.override_watch_key = None
-    self.override_watch_peak = 0.0
-    self.override_watch_timer = 0.0
+    self.target_set = False
 
     self.training_timer = 0.0
     self.persistence_timer = 0.0
-    self.training_quiet_timer = 0.0
     self.data_dirty = False
 
-    self.target = 0.0
-    self.binding_distance = 0.0
-    self.release_timer = 0.0
-    self.target_filter = FirstOrderFilter(0.0, CSC_TARGET_FILTER_RC, DT_MDL, initialized=False)
-    self.seed_pending = True
-
-    self._long_active_prev = False
-
     curvature_data = self.starpilot_planner.params.get("CurvatureData")
     self.curvature_data = self._normalize_curvature_data(curvature_data)
 
-    # built through the bucketer so the keys are byte-identical to what training writes
-    self.required_curvatures = [self._bucket_curvature(curvature) for curvature in CURVATURE_GRID]
+    self.required_curvatures = [str(round(road_curvature, ROUNDING_PRECISION)) for road_curvature in np.arange(MIN_CURVATURE, MAX_CURVATURE + STEP, STEP)]
 
-    self.rebuild_lat_accel_curve()
-    # publish on the first flush even if this drive never trains, or the readout
-    # keeps showing whatever a previous build left behind
-    self.data_dirty = True
+    self.update_lateral_acceleration()
+    self._publish_calibration_progress()
 
   @staticmethod
   def _bucket_curvature(road_curvature):
-    clipped_curvature = float(np.clip(abs(road_curvature), MIN_CURVATURE, MAX_CURVATURE))
-    # nearest in log space, so a bucket is a constant speed step rather than a constant radius one
-    bucket_index = int(np.argmin(np.abs(LOG_CURVATURE_GRID - np.log(clipped_curvature))))
-    return str(round(float(CURVATURE_GRID[bucket_index]), ROUNDING_PRECISION))
+    clipped_curvature = float(np.clip(road_curvature, MIN_CURVATURE, MAX_CURVATURE))
+    bucket_index = round((clipped_curvature - MIN_CURVATURE) / STEP)
+    bucketed_curvature = MIN_CURVATURE + (bucket_index * STEP)
+    return str(round(bucketed_curvature, ROUNDING_PRECISION))
 
   @classmethod
   def _normalize_curvature_data(cls, curvature_data):
@@ -198,36 +103,42 @@ class CurveSpeedController:
     if not self.data_dirty:
       return
 
+    progress = self._calibration_progress()
+    self.starpilot_planner.params.put_nonblocking("CalibrationProgress", progress)
+    self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data)
+    self._put_memory_param("CalibrationProgress", progress)
+    self.data_dirty = False
+    self.persistence_timer = 0.0
+
+  def _calibration_progress(self):
     progress = 0.0
     for key in self.required_curvatures:
       if key in self.curvature_data:
         progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0)
+    return (progress / len(self.required_curvatures)) * 100
 
-    self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
-    self.starpilot_planner.params.put_nonblocking("CalibrationProgress", (progress / len(self.required_curvatures)) * 100)
-    self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data)
-    self.data_dirty = False
-    self.persistence_timer = 0.0
+  def _publish_calibration_progress(self):
+    self._put_memory_param("CalibrationProgress", self._calibration_progress())
+
+  def _put_memory_param(self, key, value):
+    params_memory = getattr(self.starpilot_planner, "params_memory", None)
+    if params_memory is not None:
+      params_memory.put_nonblocking(key, value)
 
   def flush_data(self):
     self._persist_data()
 
   def log_data(self, v_ego, sm):
-    self.training_quiet_timer = max(self.training_quiet_timer - DT_MDL, 0.0)
-
     eligible = (
       v_ego > CRUISING_SPEED and
       not self.starpilot_planner.tracking_lead and
-      is_manual_speed_control(sm) and
-      self.training_quiet_timer <= 0.0
+      is_manual_speed_control(sm)
     )
     self.enable_training = False
 
     if not eligible:
       self.flush_data()
-      # decay instead of resetting: a lead flickering in and out of the tracker used to
-      # cost the full re-arm, which left almost nothing to learn from on a real drive
-      self.training_timer = max(self.training_timer - DT_MDL, 0.0)
+      self.training_timer = 0.0
       self.persistence_timer = 0.0
       return
 
@@ -236,7 +147,7 @@ class CurveSpeedController:
       self.persistence_timer += DT_MDL
 
     in_curve = (
-      self.training_timer >= CSC_TRAINING_SETTLE_TIME and
+      self.training_timer >= PLANNER_TIME and
       self.starpilot_planner.driving_in_curve and
       not (sm["carState"].leftBlinker or sm["carState"].rightBlinker)
     )
@@ -246,11 +157,11 @@ class CurveSpeedController:
 
       if road_curvature in self.curvature_data:
         data = self.curvature_data[road_curvature]
-        # capped so an established bucket still tracks a change in driving style
-        effective_count = min(data["count"], CSC_COUNT_CAP)
+        average = data["average"]
+        count = data["count"]
         self.curvature_data[road_curvature] = {
-          "average": ((data["average"] * effective_count) + lateral_acceleration) / (effective_count + 1),
-          "count": data["count"] + 1
+          "average": ((average * count) + lateral_acceleration) / (count + 1),
+          "count": count + 1
         }
       else:
         self.curvature_data[road_curvature] = {
@@ -259,7 +170,8 @@ class CurveSpeedController:
         }
 
       self.data_dirty = True
-      self.rebuild_lat_accel_curve()
+      self.update_lateral_acceleration()
+      self._publish_calibration_progress()
       self.enable_training = True
 
       if self.persistence_timer >= PLANNER_TIME:
@@ -267,166 +179,30 @@ class CurveSpeedController:
     elif self.data_dirty:
       self.flush_data()
 
-  def handle_override(self, v_ego, was_controlling, sm, accel_button=False):
-    long_active = bool(sm["carControl"].longActive)
-    long_dropped = self._long_active_prev and not long_active
-    self._long_active_prev = long_active
-
-    self._update_override_watch(sm)
-
-    if not was_controlling:
-      self.nudge_applied = False
-      return
-
-    if self.nudge_applied:
-      return
-
-    if accel_button or (sm["carState"].gasPressed and self.target < v_ego - 0.5):
-      # Watch what the driver actually holds instead of stepping by a fixed amount -- CSC is
-      # suspended while overridden, so their cornering now measures their real comfort.
-      self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
-      self.override_watch_peak = abs(self.starpilot_planner.lateral_acceleration)
-      self.override_watch_timer = CSC_OVERRIDE_WATCH_TIME
-      self.nudge_applied = True
-    elif (getattr(sm["carState"], "brakePressed", False) or long_dropped) and self.starpilot_planner.driving_in_curve:
-      self._apply_nudge(-CSC_NUDGE)
-
-  def _update_override_watch(self, sm):
-    if self.override_watch_key is None:
-      return
-
-    lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration)
-    if lateral_acceleration > self.override_watch_peak:
-      # credit the bucket the peak actually happened in, not the one at the button press
-      self.override_watch_peak = lateral_acceleration
-      self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
-
-    self.override_watch_timer -= DT_MDL
-    if self.override_watch_timer > 0.0 and (is_user_overriding_longitudinal(sm) or
-                                            self.starpilot_planner.driving_in_curve):
-      return
-
-    key = self.override_watch_key
-    self.override_watch_key = None
-    # floored at the old fixed step, so a rejection that never reaches a corner still counts
-    # and this path can only ever raise the bucket
-    self._record_pseudo_sample(key, max(self.override_watch_peak,
-                                        self.learned_lat_accel(float(key)) + CSC_NUDGE))
-
-  def _apply_nudge(self, offset):
-    key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
-    # relative to the learned value, not the margined one, or repeated overrides walk the bucket down
-    self._record_pseudo_sample(key, self.learned_lat_accel(float(key)) + offset)
-    self.nudge_applied = True
-
-  def _record_pseudo_sample(self, key, sample):
-    sample = float(np.clip(sample, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX))
-
-    data = self.curvature_data.get(key, {"average": sample, "count": 0})
-    effective_count = min(data["count"], CSC_COUNT_CAP)
-    total = effective_count + CSC_NUDGE_WEIGHT
-    self.curvature_data[key] = {
-      "average": ((data["average"] * effective_count) + (sample * CSC_NUDGE_WEIGHT)) / total,
-      "count": data["count"] + CSC_NUDGE_WEIGHT,
-    }
-
-    self.rebuild_lat_accel_curve()
-    self.data_dirty = True
-    self.flush_data()
-
-  def rebuild_lat_accel_curve(self):
-    grid_k = np.array([float(key) for key in self.required_curvatures])
-    prior = np.interp(grid_k, PRIOR_CURVATURE_BP, PRIOR_LAT_ACCEL_V)
-
-    blended = prior.copy()
-    counts = np.zeros(len(grid_k))
-    for i, key in enumerate(self.required_curvatures):
-      data = self.curvature_data.get(key)
-      if data:
-        confidence = data["count"] / (data["count"] + CSC_PRIOR_COUNT)
-        blended[i] = confidence * data["average"] + (1.0 - confidence) * prior[i]
-        counts[i] = data["count"]
-
-    blended = np.clip(blended, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX)
-    blended = weighted_isotonic(blended, counts + CSC_PRIOR_COUNT)
-
-    self._curve_k = grid_k
-    self._curve_a = blended
-
-    if counts.sum() > 0:
-      self.lateral_acceleration = float(np.average(blended, weights=counts))
+  def update_lateral_acceleration(self):
+    if self.curvature_data:
+      all_samples = [data["average"] for data in self.curvature_data.values()]
+      self.lateral_acceleration = float(np.percentile(all_samples, PERCENTILE))
     else:
       self.lateral_acceleration = DEFAULT_LATERAL_ACCELERATION
 
-  def learned_lat_accel(self, curvature):
-    """Comfort level learned for this curvature, before any control margin."""
-    return float(np.interp(abs(curvature), self._curve_k, self._curve_a))
+    self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
+    self._put_memory_param("CalibratedLateralAcceleration", self.lateral_acceleration)
 
-  def lat_accel_for_curvature(self, curvature):
-    lat_accel = np.interp(np.abs(curvature), self._curve_k, self._curve_a) * CSC_COMFORT_MARGIN
+  def update_target(self, v_ego):
+    lateral_acceleration = self.lateral_acceleration
+    if self.starpilot_planner.starpilot_weather.weather_id != 0:
+      lateral_acceleration -= self.lateral_acceleration * self.starpilot_planner.starpilot_weather.reduce_lateral_acceleration
 
-    weather = self.starpilot_planner.starpilot_weather
-    if weather.weather_id != 0:
-      lat_accel = lat_accel * (1.0 - weather.reduce_lateral_acceleration)
-
-    return lat_accel
-
-  @staticmethod
-  def _correct_far_field(curvatures, distances):
-    """Undo the model's known under-read of distant curvature, where the reading is firm."""
-    firm = (curvatures >= CSC_FARFIELD_MIN_CURVATURE) & (distances >= CSC_FARFIELD_MIN_DISTANCE)
-    return np.minimum(np.where(firm, curvatures * CSC_FARFIELD_GAIN, curvatures), MAX_CURVATURE)
-
-  def reset(self, v_cruise):
-    self.target = float(v_cruise)
-    self.release_timer = 0.0
-    self.target_filter.x = float(v_cruise)
-    self.target_filter.initialized = True
-    self.seed_pending = True
-
-  def update_target(self, v_ego, v_cruise):
-    if not self.target_filter.initialized:
-      self.reset(v_cruise)
-
-    curvatures, distances = self.starpilot_planner.curve_profile
-    if len(curvatures) == 0:
-      raw_target = float(v_cruise)
-      self.binding_distance = 0.0
+    if self.target_set:
+      csc_speed = (lateral_acceleration / abs(self.starpilot_planner.road_curvature))**0.5
+      csc_speed = max(float(csc_speed), CSC_MIN_SPEED)
+      if csc_speed >= v_ego:
+        self.target = v_ego
+      else:
+        time_to_curve = max(float(self.starpilot_planner.time_to_curve), DT_MDL)
+        decel_rate = float(np.clip((v_ego - csc_speed) / time_to_curve, 0.0, CSC_MAX_DECEL_RATE))
+        self.target = float(np.clip(self.target - decel_rate * DT_MDL, csc_speed, v_ego))
     else:
-      curvatures = self._correct_far_field(curvatures, distances)
-      lat_accel = self.lat_accel_for_curvature(curvatures)
-      point_speeds = np.sqrt(lat_accel / np.maximum(curvatures, 1e-4))
-      point_speeds = np.maximum(point_speeds, CSC_MIN_SPEED)
-      allowed_speeds = np.sqrt(point_speeds**2 + 2.0 * CSC_APPROACH_DECEL * np.maximum(distances, 0.0))
-      binding_index = int(np.argmin(allowed_speeds))
-      raw_target = min(float(allowed_speeds[binding_index]), float(v_cruise))
-      self.binding_distance = float(distances[binding_index]) if raw_target < v_cruise else 0.0
-
-    # a fresh activation starts at the envelope, or it spends seconds ramping down
-    # toward a curve it already sees (engaging or launching into a turn)
-    if self.seed_pending:
-      seed = min(float(v_cruise), max(raw_target, v_ego + CSC_EGO_HEADROOM))
-      self.target = seed
-      self.target_filter.x = seed
-      self.seed_pending = False
-
-    if raw_target >= v_ego:
-      self.release_timer += DT_MDL
-    else:
-      self.release_timer = 0.0
-
-    # The headroom aim goes through the rate limiter with everything else; applying it
-    # after the clamp let every upward jitter in raw_target reach the target unsmoothed.
-    filtered = self.target_filter.update(raw_target)
-    self.target = float(np.clip(max(filtered, min(raw_target, v_ego + CSC_EGO_HEADROOM)),
-                                self.target - CSC_TARGET_DOWN_RATE * DT_MDL,
-                                self.target + CSC_TARGET_UP_RATE * DT_MDL))
-
-    # Once the envelope really has released, the target must not sit under the car or it
-    # drags re-acceleration. Debounced, because a single jittery frame doing this yanks a
-    # legitimate cut back up to v_ego and strobes the glow on sweepers.
-    if self.release_timer >= CSC_RELEASE_DEBOUNCE:
-      self.target = max(self.target, min(raw_target, v_ego))
-
-    if self.target < v_cruise - CSC_ACTIVE_ON_DELTA:
-      self.training_quiet_timer = CSC_TRAINING_QUIET_TIME
+      self.target_set = True
+      self.target = v_ego

starpilot/controls/lib/starpilot_vcruise.py

@@ -6,13 +6,7 @@ from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 
 from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
-from openpilot.starpilot.controls.lib.curve_speed_controller import (
-  CSC_ACTIVE_OFF_DELTA,
-  CSC_GLOW_HOLD_TIME,
-  CSC_GLOW_ON_DELTA,
-  CurveSpeedController,
-  is_manual_speed_control,
-)
+from openpilot.starpilot.controls.lib.curve_speed_controller import CurveSpeedController, is_manual_speed_control
 from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_force_stop_distance_bias,
@@ -22,6 +16,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
 )
 
 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
+CSC_CURVE_RELEASE_HOLD_TIME = 0.75
 OVERRIDE_FORCE_STOP_TIMER = 10
 STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75
 # Open-loop — green is undetectable at standstill, so this only needs to cover the
@@ -64,8 +59,6 @@ LEAD_VETO_M_OVERRIDES = {
 }
 FORCE_STOP_APPROACH_DECEL = 0.65  # m/s^2 — speed ceiling before commit. LOWER = more early
                           # braking; don't go under FORCE_STOP_MODEL_APPROACH_DECEL
-# approachStopLength is published RAW: model_length converges from above, so rate-limiting
-# it inward freezes it far out and the constraint never binds. Tried, measured, don't re-add.
 ADAS_MAX_MS = 17.88       # 40 mph — cross-street ADAS guard
 DASH_SEED_M = 27.0        # ~88 ft — typical ADAS detection distance, used to snap
                           # tracked length closer when dashboard confirms a sign
@@ -83,14 +76,7 @@ FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0
 FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003
 FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0
 FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0  # m — ignore small model-horizon noise
-FORCE_STOP_REANCHOR_MIN_M = 40.0  # m — inside this only ratchet down; shouldStop doesn't
-                          # assert until ~10 m, so horizon jitter would release the stop
-FORCE_STOP_CAP_SLACK_M = 15.0  # m — the line can't move away, so tracked can never exceed
-                          # what it was at commit minus distance driven. Slack covers an
-                          # under-read at commit; without it that would stop us short.
-FORCE_STOP_CAP_TAPER_M = 60.0  # m — slack fades to 0 as the cap closes. The solver aims at
-                          # tracked, so slack held near the line is braking for a stop bar
-                          # that far past the real one.
+FORCE_STOP_REANCHOR_MIN_M = 40.0
 
 # Knob bounds (mirror of UI slider; defense in depth)
 OFFSET_FT_MIN = -20
@@ -194,11 +180,9 @@ class StarPilotVCruise:
     self.force_stop_from_light = False
     self.force_stop_light_clear_since = None
     self.controls_enabled_previously = False
-    self.approach_stop_length = 0.0  # published as starpilotPlan.approachStopLength
     # Kinematic distance estimator. Same attribute also published as
     # starpilotPlan.forcingStopLength, so the existing reader keeps working.
     self.tracked_model_length = 0.0
-    self.force_stop_distance_cap = 0.0  # odometry ceiling, re-seeded until commit
 
     self.stop_sign_confirmed = False
     self.stop_seen_on_approach_at = None
@@ -207,9 +191,8 @@ class StarPilotVCruise:
     self._nav_instruction_state = {}
     self._applied_slc_control_target = 0.0
     self.csc_controlling_speed = False
-    self.csc_glow_release_timer = 0.0
-    self.csc_override = False
     self.csc_target = 0.0
+    self.csc_curve_last_seen_at = None
 
   def _update_nav_instruction_state(self):
     raw = self.starpilot_planner.params_memory.get("NavInstructionState") or {}
@@ -575,62 +558,28 @@ class StarPilotVCruise:
       starpilot_toggles.curve_speed_controller and
       (not getattr(starpilot_toggles, "csc_no_lead", False) or not following_lead)
     )
-    # The blinker veto is for lane changes/turns, not for an already-real curve -- releasing it
-    # there let the car accelerate into the bend, then claw the speed back once the blinker cleared.
-    csc_blinker_on = ((sm["carState"].leftBlinker or sm["carState"].rightBlinker) and
-                      not self.starpilot_planner.driving_in_curve)
-    csc_was_controlling = self.csc_controlling_speed
-    # a pending SLC confirmation owns the accel button
-    slc_confirmation_pending = self.slc.speed_limit_changed_timer > DT_MDL and self.slc.unconfirmed_speed_limit >= 1
-    csc_accel_button = bool(sm["starpilotCarState"].accelPressed) and not slc_confirmation_pending
+    csc_curve_detected = csc_available and self.starpilot_planner.road_curvature_detected
+    if csc_curve_detected:
+      self.csc.update_target(v_ego)
 
-    # Latched outside the availability branch: the press itself suspends CSC this frame, so
-    # latching inside it would never see the press, and the slowdown would return on release.
-    if csc_was_controlling and csc_accel_button:
-      self.csc_override = True
-    if not (long_control_active and starpilot_toggles.curve_speed_controller):
-      self.csc_override = False
-
-    if csc_available and not csc_blinker_on:
-      self.csc.update_target(v_ego, v_cruise)
-
-      if self.csc_override and self.csc.target > v_cruise - CSC_ACTIVE_OFF_DELTA:
-        self.csc_override = False
-
-      if self.csc_override:
-        self.csc_controlling_speed = False
-        self.csc_glow_release_timer = 0.0
-        self.csc_target = v_cruise
-      else:
-        self.csc_target = self.csc.target
-        # A low target alone means nothing until the car has actually reached it (slowed down
-        # to it, or accelerated up into it). Release still waits for the set speed, so the glow
-        # spans the hold and the recovery, not just the braking.
-        if self.csc_target < v_cruise - CSC_GLOW_ON_DELTA and v_ego >= self.csc_target - CSC_ACTIVE_OFF_DELTA:
-          self.csc_controlling_speed = True
-          self.csc_glow_release_timer = 0.0
-        elif self.csc_target > v_cruise - CSC_ACTIVE_OFF_DELTA:
-          # hold through a brief release: one curve routinely lets go and re-engages
-          self.csc_glow_release_timer += DT_MDL
-          if self.csc_glow_release_timer >= CSC_GLOW_HOLD_TIME:
-            self.csc_controlling_speed = False
-        else:
-          self.csc_glow_release_timer = 0.0
-    elif csc_available:
-      # Release the cap so CSC can't fight the lane change, but keep planning -- resetting here
-      # threw the braking plan away and re-planned from the set speed with the curve closer.
-      self.csc.update_target(v_ego, v_cruise)
-      self.csc_controlling_speed = False
-      self.csc_glow_release_timer = 0.0
-      self.csc_target = v_cruise
+      self.csc_controlling_speed = True
+      self.csc_target = self.csc.target
+      self.csc_curve_last_seen_at = now
     else:
-      self.csc.reset(v_cruise)
-      self.csc_controlling_speed = False
-      self.csc_glow_release_timer = 0.0
-      self.csc_target = v_cruise
+      csc_release_hold = bool(
+        csc_available and
+        self.csc_controlling_speed and
+        self.csc_curve_last_seen_at is not None and
+        self._elapsed_seconds(now, self.csc_curve_last_seen_at) < CSC_CURVE_RELEASE_HOLD_TIME
+      )
+      if not csc_release_hold:
+        self.csc.log_data(v_ego, sm)
 
-    self.csc.handle_override(v_ego, csc_was_controlling, sm, accel_button=csc_accel_button)
-    self.csc.log_data(v_ego, sm)
+        self.csc_controlling_speed = False
+        self.csc.target_set = False
+        self.csc_curve_last_seen_at = None
+
+        self.csc_target = v_cruise
 
     # Pfeiferj's Speed Limit Controller
     self.slc.starpilot_toggles = starpilot_toggles
@@ -657,9 +606,6 @@ class StarPilotVCruise:
     offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw))
     offset_m = offset_ft * FT_TO_M
 
-    # cleared on every path; only the far-approach envelope below republishes it
-    self.approach_stop_length = 0.0
-
     if force_standstill_enabled and not self.override_force_standstill:
       self.forcing_stop = True
       self.tracked_model_length = 0.0
@@ -699,12 +645,6 @@ class StarPilotVCruise:
           self.tracked_model_length = model_length
         else:
           self.tracked_model_length = min(self.tracked_model_length, model_length)
-        # Odometry ceiling: the line can't recede, so a re-anchor may never exceed what we
-        # had at commit minus what we've driven. Bounds a ballooning horizon (seen +95 m)
-        # that the REANCHOR_MIN floor can't catch, since that floor trusts the estimate.
-        self.force_stop_distance_cap = max(self.force_stop_distance_cap - (v_ego * DT_MDL), 0.0)
-        cap_slack = FORCE_STOP_CAP_SLACK_M * min(self.force_stop_distance_cap / FORCE_STOP_CAP_TAPER_M, 1.0)
-        self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap + cap_slack)
         if dash_active:
           if model_length < DASH_MODEL_AGREE_M:
             self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
@@ -738,7 +678,6 @@ class StarPilotVCruise:
       self.stop_sign_confirmed = False
 
       self.tracked_model_length = self.starpilot_planner.model_length
-      self.force_stop_distance_cap = self.tracked_model_length
 
       targets = [v_cruise]
       if self.csc_target >= CSC_MIN_SPEED:
@@ -784,8 +723,6 @@ class StarPilotVCruise:
         adjacent_stop_d = self._get_adjacent_stop_distance(sm)
         if adjacent_stop_d is not None:
           approach_d = min(approach_d, adjacent_stop_d)
-        # pre-offset, so it hands off to forcingStopLength at commit without a step
-        self.approach_stop_length = max(approach_d, 0.0)
         approach_d += offset_m + force_stop_distance_bias_m
         if approach_d > force_stop_handoff_m:
           targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - force_stop_handoff_m)))

starpilot/controls/starpilot_planner.py

@@ -20,7 +20,7 @@ from openpilot.selfdrive.controls.lib.lead_behavior import (
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_lead_follow_jerk_scale
 
-from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature, extract_curve_profile
+from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature
 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
@@ -31,10 +31,7 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
 from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
 
 RADARLESS_TRACK_HOLD_TIME = 0.45
-FORCE_STOP_JERK_SCALE = 0.20  # accel-change cost multiplier for the whole stop approach,
-                              # envelope included (125 -> 25). Lower = reaches the braking
-                              # target sooner; it does not make the target deeper. Response
-                              # is super-linear here, so raise it if onset feels like a step.
+FORCE_STOP_JERK_SCALE = 0.32  # accel-change cost multiplier while forcing_stop (125 -> ~40)
 FORCE_STOP_JERK_SCALE_OVERRIDES = {
   # The Elantra's current force-stop ramp is smooth, but it waits too long
   # before building decel and then arrives at the initial brake too abruptly.
@@ -214,7 +211,6 @@ class StarPilotPlanner:
     self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop
 
     self.road_curvature, self.time_to_curve = calculate_road_curvature(sm["modelV2"], v_ego)
-    self.curve_profile = extract_curve_profile(sm["modelV2"])
 
     self.road_curvature_detected = (1 / abs(self.road_curvature))**0.5 < v_ego > CRUISING_SPEED and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker)
 
@@ -312,9 +308,7 @@ class StarPilotPlanner:
     except (KeyError, IndexError, TypeError, AttributeError):
       car_params = None
 
-    # Also while the far-approach envelope is running: at onset the ramp reaches only
-    # ~-0.5 m/s^2 after a second, so the first seconds of a detected red are mostly lost.
-    if self.starpilot_vcruise.forcing_stop or self.starpilot_vcruise.approach_stop_length > 0.0:
+    if self.starpilot_vcruise.forcing_stop:
       jerk_scale = get_force_stop_jerk_scale(car_params)
     elif self.tracking_lead:
       # Elantra vision leads can hand off from cruise to lead0 while closing
@@ -333,9 +327,6 @@ class StarPilotPlanner:
     starpilotPlan.cscControllingSpeed = self.starpilot_vcruise.csc_controlling_speed
     starpilotPlan.cscSpeed = float(self.starpilot_vcruise.csc_target)
     starpilotPlan.cscTraining = self.starpilot_vcruise.csc.enable_training
-    starpilotPlan.cscOverridden = self.starpilot_vcruise.csc_override
-    starpilotPlan.cscLearnedLatAccel = float(self.starpilot_vcruise.csc.learned_lat_accel(self.road_curvature))
-    starpilotPlan.cscBindingDistance = float(self.starpilot_vcruise.csc.binding_distance)
 
     starpilotPlan.desiredFollowDistance = int(self.starpilot_following.desired_follow_distance)
     starpilotPlan.disableThrottle = (
@@ -355,7 +346,6 @@ class StarPilotPlanner:
 
     starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop
     starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length
-    starpilotPlan.approachStopLength = float(self.starpilot_vcruise.approach_stop_length)
     starpilotPlan.stopSignConfirmed = self.starpilot_vcruise.stop_sign_confirmed
 
     starpilotPlan.starpilotEvents = self.starpilot_events.events.to_msg()

starpilot/system/the_galaxy/assets/components/tools/device_settings.css

@@ -384,14 +384,6 @@
   padding: 0.2rem 0.6rem;
 }
 
-/* read-only: no border, since there is nothing here to click or edit */
-.ds-row-readout {
-  background-color: transparent;
-  border: none;
-  color: var(--text-muted);
-  font-style: italic;
-}
-
 .ds-stepper-container {
   width: 100%;
 }

starpilot/system/the_galaxy/assets/components/tools/device_settings.js

@@ -478,16 +478,6 @@ function formatSliderValue(val, stepStr, precisionInt, key) {
   return Number(v.toFixed(dec)).toString()
 }
 
-function formatReadoutValue(p) {
-  const raw = state.values[p.key]
-  const v = parseFloat(raw)
-  if (raw === undefined || raw === null || Number.isNaN(v)) return "--"
-
-  const precision = p.precision !== undefined && p.precision !== null ? Number(p.precision) : 2
-  const formatted = Number(v.toFixed(Math.max(0, precision))).toString()
-  return p.unit ? `${formatted}${p.unit}` : formatted
-}
-
 function formatNumericForInput(value, precision) {
   const n = Number(value)
   if (!Number.isFinite(n)) return ""
@@ -1502,7 +1492,6 @@ function renderSettingRow(p) {
   const isText = p.ui_type === "text"
   const isColor = p.ui_type === "color"
   const isAction = p.ui_type === "action"
-  const isReadout = p.ui_type === "readout"
   const isGroup = isGroupParam(p)
   const isChild = p.parent_key ? "ds-child-modifier" : ""
   const lockReason = () => getSettingLockReason(p)
@@ -1653,7 +1642,7 @@ function renderSettingRow(p) {
           @click="${() => resetColorParam(p)}">Stock</button>
       </div>
     `
-  } else if (!isGroup && !isReadout) {
+  } else if (!isGroup) {
     if (p.key === "IsRHD") {
       rowControl = html`
         <div style="display:flex; align-items:center; gap:0.75rem;">
@@ -1723,9 +1712,8 @@ function renderSettingRow(p) {
             </div>
           ` : ""}
         </div>
-        ${(isNumeric || isColor || isReadout) ? html`<span class="ds-row-value ${isReadout ? "ds-row-readout" : ""}" id="ds-display-${p.key}">${() => {
+        ${(isNumeric || isColor) ? html`<span class="ds-row-value" id="ds-display-${p.key}">${() => {
             if (isColor) return formatColorDisplayValue(p)
-            if (isReadout) return formatReadoutValue(p)
             const currentValue = state.sliderPreviewValues[p.key] ?? state.values[p.key]
             const bounds = numericBounds(p)
             return currentValue !== undefined ? formatSliderValue(currentValue, String(bounds.step), p.precision, p.key) : ".."

starpilot/system/the_galaxy/the_galaxy.py

@@ -5483,16 +5483,6 @@ def setup(app):
     result["VehicleParked"] = _get_vehicle_parked()
     result["AlphaLongitudinalAvailable"] = _get_alpha_longitudinal_available()
     result["HasRivianAngleHarness"] = _get_has_rivian_angle_harness()
-    # read-only: excluded from allowed_keys (and so from the write paths) but still
-    # worth surfacing as a display-only readout
-    try:
-      result["CalibratedLateralAcceleration"] = _get_current_param_value("CalibratedLateralAcceleration", float, defaults_lookup)
-    except Exception:
-      result["CalibratedLateralAcceleration"] = None
-    try:
-      result["CalibrationProgress"] = _get_current_param_value("CalibrationProgress", float, defaults_lookup)
-    except Exception:
-      result["CalibrationProgress"] = None
 
     return jsonify(_sanitize_json_value(result)), 200
 

tinygrad_repo/tinygrad/runtime/ops_amd.py

@@ -1,6 +1,6 @@
 from __future__ import annotations
 from typing import cast
-import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit
+import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit, time
 assert sys.platform != 'win32'
 from dataclasses import dataclass
 from tinygrad.runtime.support.hcq import HCQCompiled, HCQAllocator, HCQBuffer, HWQueue, CLikeArgsState, HCQSignal, HCQProgram, FileIOInterface
@@ -25,6 +25,7 @@ SQTT = ContextVar("SQTT", abs(VIZ.value)>=2)
 SQTT_ITRACE_SE_MASK, SQTT_LIMIT_SE, SQTT_SIMD_SEL, SQTT_TOKEN_EXCLUDE = \
   ContextVar("SQTT_ITRACE_SE_MASK", 0b11), ContextVar("SQTT_LIMIT_SE", 0), ContextVar("SQTT_SIMD_SEL", 0), ContextVar("SQTT_TOKEN_EXCLUDE", 0)
 PMC = ContextVar("PMC", abs(VIZ.value)>=2)
+AMD_USB_POLL_US = getenv("AMD_USB_POLL_US", 500) # microseconds to sleep between USB signal polls. 0 disables
 EVENT_INDEX_PARTIAL_FLUSH = 4 # based on a comment in nvd.h
 WAIT_REG_MEM_FUNCTION_EQ  = 3 # ==
 WAIT_REG_MEM_FUNCTION_NEQ = 4 # !=
@@ -45,8 +46,9 @@ class AMDSignal(HCQSignal):
   def __init__(self, *args, **kwargs): super().__init__(*args, **{**kwargs, 'timestamp_divider': 100})
 
   def _sleep(self, time_spent_since_last_sleep_ms:int):
-    # Reasonable to sleep for long workloads (which take more than 200ms) and only timeline signals.
-    if time_spent_since_last_sleep_ms > 200 and self.owner is not None: self.owner.iface.sleep(200)
+    # USB signals live in VRAM across the link, so yield between polls. Native AMD only blocks after 200 ms.
+    if self.owner is not None and self.owner.is_usb() and AMD_USB_POLL_US: time.sleep(AMD_USB_POLL_US / 1e6)
+    elif time_spent_since_last_sleep_ms > 200 and self.owner is not None: self.owner.iface.sleep(200)
 
 class AMDComputeQueue(HWQueue):
   def __init__(self, dev:AMDDevice):

tools/longitudinal/analyze_csc.py

@@ -1,295 +0,0 @@
-#!/usr/bin/env python3
-"""Curve Speed Controller field report: does it cut the lateral-accel tail, how
-often does it engage, and how often do drivers reject it.
-
-Usage:
-  ./analyze_csc.py <route-or-segment>            # e.g. a1b2c3d4e5f6g7h8|2026-08-14--10-30-00
-  ./analyze_csc.py <rlog-path> [<rlog-path> ...]
-  ./analyze_csc.py <route> --json report.json
-"""
-from __future__ import annotations
-
-import argparse
-import json
-import math
-from dataclasses import dataclass, field
-from pathlib import Path
-
-import numpy as np
-
-DT = 0.05                      # modelV2/starpilotPlan cadence
-MS_TO_MPH = 2.23694
-M_TO_MILES = 1.0 / 1609.34
-HIGHWAY_SPEED = 60.0 / MS_TO_MPH   # above this, engagement is the over-slowing regression risk
-CURVE_LAT_ACCEL = 1.3              # MINIMUM_LATERAL_ACCELERATION
-EPISODE_GAP_S = 1.0
-V_CRUISE_UNSET = 255
-
-
-@dataclass
-class Frame:
-  t: float = 0.0
-  v_ego: float = 0.0
-  a_ego: float = 0.0
-  curvature: float = 0.0
-  gas: bool = False
-  brake: bool = False
-  accel_pressed: bool = False
-  long_active: bool = False
-  blinker: bool = False     # CSC gating input: a blinker suspends it entirely
-  csc_active: bool = False
-  csc_overridden: bool = False
-  csc_training: bool = False
-  csc_speed: float = 0.0
-  v_cruise: float = 0.0     # applied cruise speed, already reduced by CSC
-  set_speed: float = 0.0    # what the driver dialled in, so cuts are measurable
-  learned_lat_accel: float = 0.0
-  binding_distance: float = 0.0
-
-  @property
-  def lat_accel(self) -> float:
-    return self.v_ego ** 2 * abs(self.curvature)
-
-
-@dataclass
-class Episode:
-  start: float
-  end: float
-  peak_cut: float = 0.0
-  peak_lat_accel: float = 0.0
-  min_a_ego: float = 0.0
-  entry_speed: float = 0.0
-  binding_distance: float = 0.0
-  cancelled: bool = False
-  gas: bool = False
-  brake: bool = False
-
-  @property
-  def duration(self) -> float:
-    return self.end - self.start
-
-
-def read_events(identifier: str):
-  """A downloaded rlog reads directly; anything else goes through LogReader."""
-  path = Path(identifier)
-  if path.is_file():
-    from cereal import log as capnp_log
-
-    data = path.read_bytes()
-    if data[:4] == b"\x28\xb5\x2f\xfd":
-      import zstandard
-      data = zstandard.ZstdDecompressor().decompress(data, max_output_size=2 << 30)
-    return capnp_log.Event.read_multiple_bytes(data)
-
-  from openpilot.tools.lib.logreader import LogReader, ReadMode  # needs the device stack
-
-  return LogReader(identifier, default_mode=ReadMode.AUTO, sort_by_time=True)
-
-
-def read_frames(identifier: str) -> list[Frame]:
-  """Join carState/controlsState/starpilotPlan onto the plan's cadence."""
-  frames: list[Frame] = []
-  latest = Frame()
-  t0 = None
-  have_plan = False
-
-  for msg in read_events(identifier):
-    which = msg.which()
-    if which == "carState":
-      cs = msg.carState
-      latest.v_ego = float(cs.vEgo)
-      latest.a_ego = float(cs.aEgo)
-      latest.gas = bool(cs.gasPressed)
-      latest.brake = bool(cs.brakePressed)
-      latest.blinker = bool(cs.leftBlinker or cs.rightBlinker)
-      set_kph = float(cs.vCruise)
-      latest.set_speed = set_kph / 3.6 if 0 < set_kph < V_CRUISE_UNSET else 0.0
-    elif which == "carControl":
-      latest.long_active = bool(msg.carControl.longActive)
-    elif which == "controlsState":
-      latest.curvature = float(msg.controlsState.curvature)
-    elif which == "starpilotCarState":
-      latest.accel_pressed = bool(getattr(msg.starpilotCarState, "accelPressed", False))
-    elif which == "starpilotPlan":
-      plan = msg.starpilotPlan
-      have_plan = True
-      if t0 is None:
-        t0 = msg.logMonoTime / 1e9
-      latest.t = msg.logMonoTime / 1e9 - t0
-      latest.csc_active = bool(plan.cscControllingSpeed)
-      latest.csc_training = bool(plan.cscTraining)
-      latest.csc_speed = float(plan.cscSpeed)
-      latest.v_cruise = float(plan.vCruise)
-      # absent in older logs
-      latest.csc_overridden = bool(getattr(plan, "cscOverridden", False))
-      latest.learned_lat_accel = float(getattr(plan, "cscLearnedLatAccel", 0.0))
-      latest.binding_distance = float(getattr(plan, "cscBindingDistance", 0.0))
-      frames.append(Frame(**vars(latest)))
-
-  if not have_plan:
-    raise SystemExit(f"no starpilotPlan messages in {identifier} — is this a StarPilot route?")
-  return frames
-
-
-def build_episodes(frames: list[Frame]) -> list[Episode]:
-  episodes: list[Episode] = []
-  current: Episode | None = None
-  last_active_t = -math.inf
-
-  for f in frames:
-    if f.csc_active:
-      if current is None or (f.t - last_active_t) > EPISODE_GAP_S:
-        current = Episode(start=f.t, end=f.t, entry_speed=f.v_ego,
-                          binding_distance=f.binding_distance, min_a_ego=f.a_ego)
-        episodes.append(current)
-      current.end = f.t
-      if f.set_speed > 0:
-        current.peak_cut = max(current.peak_cut, f.set_speed - f.csc_speed)
-      current.peak_lat_accel = max(current.peak_lat_accel, f.lat_accel)
-      current.min_a_ego = min(current.min_a_ego, f.a_ego)
-      current.gas |= f.gas
-      current.brake |= f.brake
-      last_active_t = f.t
-    elif current is not None and (f.t - last_active_t) <= EPISODE_GAP_S:
-      # an override releases CSC on the same frame it registers, so the rejection
-      # always lands just past the end of the episode it rejected
-      current.cancelled |= f.csc_overridden or f.accel_pressed
-      current.gas |= f.gas
-      current.brake |= f.brake
-
-  return episodes
-
-
-def curve_lat_accel_peaks(frames: list[Frame]) -> list[float]:
-  """Peak lateral acceleration of each distinct curve, engaged driving only."""
-  peaks: list[float] = []
-  peak = 0.0
-  in_curve = False
-  for f in frames:
-    if not f.long_active:
-      continue
-    if f.lat_accel >= CURVE_LAT_ACCEL:
-      in_curve = True
-      peak = max(peak, f.lat_accel)
-    elif in_curve:
-      peaks.append(peak)
-      peak = 0.0
-      in_curve = False
-  if in_curve:
-    peaks.append(peak)
-  return peaks
-
-
-def summarize(frames: list[Frame], episodes: list[Episode]) -> dict:
-  driving = [f for f in frames if f.v_ego > 5.0]
-  engaged = [f for f in driving if f.long_active]
-  active = [f for f in engaged if f.csc_active]
-  distance_mi = sum(f.v_ego * DT for f in driving) * M_TO_MILES
-  peaks = curve_lat_accel_peaks(frames)
-  highway = [e for e in episodes if e.entry_speed >= HIGHWAY_SPEED]
-
-  def pct(n, d):
-    return 100.0 * n / d if d else 0.0
-
-  return {
-    "route": {
-      "duration_min": len(frames) * DT / 60.0,
-      "distance_mi": distance_mi,
-      "engaged_pct": pct(len(engaged), len(driving)),
-      "mean_speed_mph": float(np.mean([f.v_ego for f in driving]) * MS_TO_MPH) if driving else 0.0,
-    },
-    "engagement": {
-      "active_pct_of_engaged": pct(len(active), len(engaged)),
-      "episodes": len(episodes),
-      "episodes_per_mile": len(episodes) / distance_mi if distance_mi > 0.1 else 0.0,
-      "median_duration_s": float(np.median([e.duration for e in episodes])) if episodes else 0.0,
-      "max_duration_s": max((e.duration for e in episodes), default=0.0),
-      "median_cut_mph": float(np.median([e.peak_cut for e in episodes]) * MS_TO_MPH) if episodes else 0.0,
-      "max_cut_mph": max((e.peak_cut for e in episodes), default=0.0) * MS_TO_MPH,
-      "median_anticipation_m": float(np.median([e.binding_distance for e in episodes])) if episodes else 0.0,
-    },
-    "outcome_lat_accel": {
-      "curves_seen": len(peaks),
-      "median": float(np.median(peaks)) if peaks else 0.0,
-      "p90": float(np.percentile(peaks, 90)) if peaks else 0.0,
-      "p99": float(np.percentile(peaks, 99)) if peaks else 0.0,
-      "max": max(peaks, default=0.0),
-      "over_3_0_pct": pct(sum(1 for p in peaks if p > 3.0), len(peaks)),
-    },
-    "acceptance": {
-      "cancelled_episodes": sum(1 for e in episodes if e.cancelled),
-      "cancel_rate_pct": pct(sum(1 for e in episodes if e.cancelled), len(episodes)),
-      "gas_during_episode_pct": pct(sum(1 for e in episodes if e.gas), len(episodes)),
-      "brake_during_episode_pct": pct(sum(1 for e in episodes if e.brake), len(episodes)),
-    },
-    "comfort": {
-      "median_min_a_ego": float(np.median([e.min_a_ego for e in episodes])) if episodes else 0.0,
-      "hardest_decel": min((e.min_a_ego for e in episodes), default=0.0),
-    },
-    "highway_watch": {
-      "episodes_above_60mph": len(highway),
-      "max_cut_mph": max((e.peak_cut for e in highway), default=0.0) * MS_TO_MPH,
-    },
-    "learning": {
-      "training_pct_of_driving": pct(sum(1 for f in driving if f.csc_training), len(driving)),
-      "learned_lat_accel_min": min((f.learned_lat_accel for f in active), default=0.0),
-      "learned_lat_accel_max": max((f.learned_lat_accel for f in active), default=0.0),
-    },
-  }
-
-
-def print_report(name: str, s: dict) -> None:
-  r, e, o, a, c, h, l = (s["route"], s["engagement"], s["outcome_lat_accel"],
-                         s["acceptance"], s["comfort"], s["highway_watch"], s["learning"])
-  print(f"\n=== {name}")
-  print(f"  {r['duration_min']:.1f} min, {r['distance_mi']:.1f} mi, "
-        f"{r['mean_speed_mph']:.0f} mph avg, engaged {r['engaged_pct']:.0f}% of driving")
-
-  print("\n  DOES IT WORK -- peak lateral accel per curve (engaged)")
-  print(f"    {o['curves_seen']} curves   median {o['median']:.2f}   p90 {o['p90']:.2f}   "
-        f"p99 {o['p99']:.2f}   max {o['max']:.2f} m/s^2")
-  print(f"    curves over 3.0 m/s^2: {o['over_3_0_pct']:.1f}%   <-- this tail should shrink vs a CSC-off route")
-
-  print("\n  DO USERS ACCEPT IT")
-  print(f"    cancel rate (RES+) {a['cancel_rate_pct']:.0f}%   gas {a['gas_during_episode_pct']:.0f}%   "
-        f"brake {a['brake_during_episode_pct']:.0f}%   of {e['episodes']} episodes")
-  print("    cancels/gas high => too slow;  brake high => too fast")
-
-  print("\n  ENGAGEMENT")
-  print(f"    {e['active_pct_of_engaged']:.1f}% of engaged time, {e['episodes_per_mile']:.2f} episodes/mi, "
-        f"median {e['median_duration_s']:.1f}s (max {e['max_duration_s']:.1f}s)")
-  print(f"    speed cut median {e['median_cut_mph']:.1f} mph, max {e['max_cut_mph']:.1f} mph")
-  print(f"    braking begins {e['median_anticipation_m']:.0f} m ahead (median)")
-
-  print("\n  COMFORT / REGRESSION WATCH")
-  print(f"    decel median {c['median_min_a_ego']:.2f}, hardest {c['hardest_decel']:.2f} m/s^2")
-  print(f"    highway (>60 mph) episodes: {h['episodes_above_60mph']}, max cut {h['max_cut_mph']:.1f} mph"
-        f"   <-- over-slowing complaints start here")
-
-  print("\n  LEARNING")
-  print(f"    training {l['training_pct_of_driving']:.1f}% of driving; "
-        f"learned comfort in use {l['learned_lat_accel_min']:.2f}-{l['learned_lat_accel_max']:.2f} m/s^2")
-
-
-def main() -> None:
-  parser = argparse.ArgumentParser(description="Curve Speed Controller field report.")
-  parser.add_argument("routes", nargs="+", help="route/segment identifier(s) or rlog path(s)")
-  parser.add_argument("--json", type=Path, help="also write the raw numbers here")
-  args = parser.parse_args()
-
-  reports = {}
-  for identifier in args.routes:
-    name = Path(identifier).name if Path(identifier).exists() else identifier
-    frames = read_frames(identifier)
-    episodes = build_episodes(frames)
-    summary = summarize(frames, episodes)
-    reports[name] = summary
-    print_report(name, summary)
-
-  if args.json:
-    args.json.write_text(json.dumps(reports, indent=2))
-    print(f"\nwrote {args.json}")
-
-
-if __name__ == "__main__":
-  main()