Match C4 cooling curve

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -1168,6 +1168,7 @@ 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

@@ -0,0 +1,506 @@
+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_starpilot_vcruise.py

@@ -4,7 +4,8 @@ import pytest
 
 from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
-from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController, MIN_TRAINING_TIME
+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.starpilot_vcruise import (
   FORCE_STOP_CAP_SLACK_M,
   FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
@@ -53,6 +54,7 @@ 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
@@ -82,12 +84,12 @@ def make_sm(*, standstill=True, min_steer_speed=0.0, car_fingerprint=""):
   }
 
 
-def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, controls_enabled=True):
+def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, v_cruise=20.0, controls_enabled=True):
   return vcruise.update(
     controls_enabled=controls_enabled,
     now=now,
     time_validated=True,
-    v_cruise=20.0,
+    v_cruise=v_cruise,
     v_ego=v_ego,
     sm=sm,
     starpilot_toggles=toggles,
@@ -145,30 +147,56 @@ 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_holds_target_through_brief_detector_dropout():
+def test_curve_speed_controller_blinker_releases_the_cap_but_keeps_the_plan():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)
   toggles = make_toggles()
   toggles.curve_speed_controller = True
 
-  def set_curve_target(_v_ego):
-    vcruise.csc.target_set = True
+  calls = []
+
+  def set_curve_target(_v_ego, _v_cruise):
+    calls.append(_v_ego)
     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
 
-  planner.road_curvature_detected = False
+  # 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
   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
 
-  result = update_vcruise(vcruise, sm, toggles, now=10.8, v_ego=20.0)
-  assert result == pytest.approx(20.0)
+
+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)
   assert not vcruise.csc_controlling_speed
+  assert vcruise.csc.seed_pending
 
 
 def test_curve_speed_controller_releases_immediately_when_disabled():
@@ -177,53 +205,19 @@ def test_curve_speed_controller_releases_immediately_when_disabled():
   toggles = make_toggles()
   toggles.curve_speed_controller = True
 
-  def set_curve_target(_v_ego):
-    vcruise.csc.target_set = True
+  def set_curve_target(_v_ego, _v_cruise):
     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)
   assert not vcruise.csc_controlling_speed
 
 
-def test_curve_speed_controller_does_not_compete_with_force_stop():
-  planner, vcruise = make_vcruise(red_light=True, road_curvature=0.001)
-  sm = make_sm(standstill=False)
-  toggles = make_toggles()
-  toggles.curve_speed_controller = True
-  planner.road_curvature_detected = True
-  vcruise.csc.target_set = True
-  vcruise.csc.target = 12.0
-
-  update_vcruise(vcruise, sm, toggles, now=25.0, v_ego=20.0)
-
-  assert not vcruise.csc_controlling_speed
-  assert not vcruise.csc.target_set
-
-
-def test_curve_speed_controller_learns_through_a_signaled_curve():
-  planner, vcruise = make_vcruise(road_curvature=0.02)
-  sm = make_sm(standstill=False)
-  sm["carControl"].longActive = False
-  sm["carState"].leftBlinker = True
-  planner.driving_in_curve = True
-  planner.lateral_acceleration = 2.4
-  vcruise.csc.training_timer = MIN_TRAINING_TIME
-
-  vcruise.csc.log_data(20.0, sm)
-
-  assert vcruise.csc.enable_training
-  assert vcruise.csc.curvature_data["0.02"]["count"] == 1
-
-
-
 def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)
@@ -231,12 +225,10 @@ 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):
-    vcruise.csc.target_set = True
+  def set_curve_target(_v_ego, _v_cruise):
     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)
@@ -254,10 +246,8 @@ 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):
-    vcruise.csc.target_set = True
+  def set_curve_target(_v_ego, _v_cruise):
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
@@ -281,7 +271,7 @@ def test_curve_speed_controller_learns_when_speed_is_manually_controlled(long_ac
   planner.driving_in_curve = True
   planner.road_curvature_detected = True
   planner.lateral_acceleration = 2.4
-  vcruise.csc.training_timer = MIN_TRAINING_TIME
+  vcruise.csc.training_timer = PLANNER_TIME
 
   update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0)
 
@@ -299,7 +289,7 @@ def test_curve_speed_controller_learns_when_longitudinal_override_event_is_activ
   planner.driving_in_curve = True
   planner.road_curvature_detected = True
   planner.lateral_acceleration = 2.4
-  vcruise.csc.training_timer = MIN_TRAINING_TIME
+  vcruise.csc.training_timer = PLANNER_TIME
 
   update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0)
 
@@ -313,7 +303,7 @@ def test_curve_speed_controller_persists_data_after_leaving_curve():
   sm["carControl"].longActive = False
   planner.driving_in_curve = True
   planner.lateral_acceleration = 2.4
-  vcruise.csc.training_timer = MIN_TRAINING_TIME
+  vcruise.csc.training_timer = PLANNER_TIME
 
   vcruise.csc.log_data(20.0, sm)
   assert not any(key == "CurvatureData" for key, _ in planner.params.writes)
@@ -324,54 +314,276 @@ def test_curve_speed_controller_persists_data_after_leaving_curve():
   assert any(key == "CurvatureData" for key, _ in planner.params.writes)
 
 
-def test_curve_speed_controller_publishes_live_values_to_memory_params():
-  planner, vcruise = make_vcruise(road_curvature=0.02)
+def test_csc_res_press_cancels_for_episode_and_rearms():
+  planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)
-  sm["carControl"].longActive = 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
   planner.driving_in_curve = True
-  planner.lateral_acceleration = 2.4
-  vcruise.csc.training_timer = MIN_TRAINING_TIME
+  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
 
-  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
+  # 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
 
 
-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
+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
 
-  controller.update_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
 
-  assert controller.target == pytest.approx(30.0 - CSC_MAX_DECEL_RATE * DT_MDL)
-  assert controller.target > (controller.lateral_acceleration / planner.road_curvature) ** 0.5
+  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
 
 
-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
+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
 
-  controller.update_target(30.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
 
-  assert controller.target == pytest.approx(30.0)
 
 
 def test_active_slc_control_target_applies_offset_and_cluster_diff():

selfdrive/ui/onroad/starpilot/starpilot_border.py

@@ -58,6 +58,8 @@ 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"):
     return None
 

starpilot/common/starpilot_utilities.py

@@ -138,6 +138,23 @@ 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,19 +2,97 @@
 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
-MIN_TRAINING_TIME = 5.0
 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
-CSC_MAX_DECEL_RATE = 1.5
-MAX_CURVATURE = 0.1
-MIN_CURVATURE = 0.001
-PERCENTILE = 90
-ROUNDING_PRECISION = 5
-STEP = 0.001
+
+# 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
 
 
 def is_user_overriding_longitudinal(sm):
@@ -43,26 +121,45 @@ class CurveSpeedController:
     self.starpilot_planner = StarPilotVCruise.starpilot_planner
 
     self.enable_training = False
-    self.target_set = False
+    self.nudge_applied = False
+
+    self.override_watch_key = None
+    self.override_watch_peak = 0.0
+    self.override_watch_timer = 0.0
 
     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)
 
-    self.required_curvatures = [str(round(road_curvature, ROUNDING_PRECISION)) for road_curvature in np.arange(MIN_CURVATURE, MAX_CURVATURE + STEP, STEP)]
+    # 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.update_lateral_acceleration()
-    self._publish_calibration_progress(persist=True)
+    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
+    # the Settings screen reads the memory param for a live readout -- seed it now, or it
+    # shows nothing until the first disk flush
+    self._publish_live_values()
 
   @staticmethod
   def _bucket_curvature(road_curvature):
-    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))
+    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))
 
   @classmethod
   def _normalize_curvature_data(cls, curvature_data):
@@ -100,17 +197,6 @@ class CurveSpeedController:
 
     return normalized
 
-  def _persist_data(self):
-    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:
@@ -118,31 +204,48 @@ class CurveSpeedController:
         progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0)
     return (progress / len(self.required_curvatures)) * 100
 
-  def _publish_calibration_progress(self, persist=False):
-    progress = self._calibration_progress()
-    if persist:
-      self.starpilot_planner.params.put_nonblocking("CalibrationProgress", progress)
-    self._put_memory_param("CalibrationProgress", progress)
-
-  def _put_memory_param(self, key, value):
+  def _publish_live_values(self, progress=None):
+    # memory-only and cheap, so this can run every frame training touches the data --
+    # it's what the on-device Settings screen reads for a live readout between disk flushes
     params_memory = getattr(self.starpilot_planner, "params_memory", None)
-    if params_memory is not None:
-      params_memory.put_nonblocking(key, value)
+    if params_memory is None:
+      return
+    if progress is None:
+      progress = self._calibration_progress()
+    params_memory.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
+    params_memory.put_nonblocking("CalibrationProgress", progress)
+
+  def _persist_data(self):
+    if not self.data_dirty:
+      return
+
+    progress = self._calibration_progress()
+    self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
+    self.starpilot_planner.params.put_nonblocking("CalibrationProgress", progress)
+    self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data)
+    self._publish_live_values(progress)
+    self.data_dirty = False
+    self.persistence_timer = 0.0
 
   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)
+      is_manual_speed_control(sm) and
+      self.training_quiet_timer <= 0.0
     )
     self.enable_training = False
 
     if not eligible:
       self.flush_data()
-      self.training_timer = 0.0
+      # 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.persistence_timer = 0.0
       return
 
@@ -151,8 +254,9 @@ class CurveSpeedController:
       self.persistence_timer += DT_MDL
 
     in_curve = (
-      self.training_timer >= MIN_TRAINING_TIME and
-      self.starpilot_planner.driving_in_curve
+      self.training_timer >= CSC_TRAINING_SETTLE_TIME and
+      self.starpilot_planner.driving_in_curve and
+      not (sm["carState"].leftBlinker or sm["carState"].rightBlinker)
     )
     if in_curve:
       lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration)
@@ -160,11 +264,11 @@ class CurveSpeedController:
 
       if road_curvature in self.curvature_data:
         data = self.curvature_data[road_curvature]
-        average = data["average"]
-        count = data["count"]
+        # capped so an established bucket still tracks a change in driving style
+        effective_count = min(data["count"], CSC_COUNT_CAP)
         self.curvature_data[road_curvature] = {
-          "average": ((average * count) + lateral_acceleration) / (count + 1),
-          "count": count + 1
+          "average": ((data["average"] * effective_count) + lateral_acceleration) / (effective_count + 1),
+          "count": data["count"] + 1
         }
       else:
         self.curvature_data[road_curvature] = {
@@ -173,8 +277,8 @@ class CurveSpeedController:
         }
 
       self.data_dirty = True
-      self.update_lateral_acceleration()
-      self._publish_calibration_progress()
+      self.rebuild_lat_accel_curve()
+      self._publish_live_values()
       self.enable_training = True
 
       if self.persistence_timer >= PLANNER_TIME:
@@ -182,30 +286,166 @@ class CurveSpeedController:
     elif self.data_dirty:
       self.flush_data()
 
-  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))
+  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))
     else:
       self.lateral_acceleration = DEFAULT_LATERAL_ACCELERATION
 
-    self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
-    self._put_memory_param("CalibratedLateralAcceleration", self.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))
 
-  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
+  def lat_accel_for_curvature(self, curvature):
+    lat_accel = np.interp(np.abs(curvature), self._curve_k, self._curve_a) * CSC_COMFORT_MARGIN
 
-    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))
+    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
     else:
-      self.target_set = True
-      self.target = v_ego
+      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

starpilot/controls/lib/starpilot_vcruise.py

@@ -6,7 +6,13 @@ 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 CurveSpeedController, is_manual_speed_control
+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.speed_limit_controller import SpeedLimitController
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_force_stop_distance_bias,
@@ -16,7 +22,6 @@ 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
@@ -202,8 +207,9 @@ 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 {}
@@ -571,28 +577,62 @@ class StarPilotVCruise:
       starpilot_toggles.curve_speed_controller and
       (not getattr(starpilot_toggles, "csc_no_lead", False) or not following_lead)
     )
-    csc_curve_detected = csc_available and self.starpilot_planner.road_curvature_detected
-    if csc_curve_detected:
-      self.csc.update_target(v_ego)
+    # 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
 
-      self.csc_controlling_speed = True
-      self.csc_target = self.csc.target
-      self.csc_curve_last_seen_at = now
-    else:
-      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)
+    # 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.target_set = False
-        self.csc_curve_last_seen_at = None
-
+        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
+    else:
+      self.csc.reset(v_cruise)
+      self.csc_controlling_speed = False
+      self.csc_glow_release_timer = 0.0
+      self.csc_target = v_cruise
+
+    self.csc.handle_override(v_ego, csc_was_controlling, sm, accel_button=csc_accel_button)
+    self.csc.log_data(v_ego, sm)
 
     # Pfeiferj's Speed Limit Controller
     self.slc.starpilot_toggles = starpilot_toggles

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
+from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature, extract_curve_profile
 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
@@ -214,6 +214,7 @@ 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)
 
@@ -328,6 +329,9 @@ 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 = (

starpilot/system/bluetooth/bluez.py

@@ -214,7 +214,7 @@ class BlueZClient:
         continue
       props = interfaces[DEVICE_IFACE]
       uuids = [str(value).lower() for value in props.get("UUIDs", [])]
-      audio, controller, serial = device_capabilities(uuids, int(props.get("Class", 0)), str(props.get("Icon", "")))
+      audio, controller = device_capabilities(uuids, int(props.get("Class", 0)), str(props.get("Icon", "")))
       device = {
         "path": path,
         "address": str(props.get("Address", "")),
@@ -227,10 +227,9 @@ class BlueZClient:
         "uuids": uuids,
         "audio": audio,
         "controller": controller,
-        "serial": serial,
       }
       if include_hidden or show_pairing_device(device["address"], device["name"], device["paired"], device["trusted"], device["connected"],
-                                               device["blocked"], audio, controller, serial, include_discovering):
+                                               device["blocked"], audio, controller, include_discovering):
         devices.append(device)
     return sorted(devices, key=lambda device: (not device["connected"], not device["paired"], -(device["rssi"] or -127), device["name"].lower()))
 

starpilot/system/bluetooth/daemon.py

@@ -9,13 +9,11 @@ from typing import Any
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
 from openpilot.starpilot.system.bluetooth.bluez import BlueZClient
-from openpilot.starpilot.system.bluetooth.elm327 import ELM327Session
 from openpilot.starpilot.system.bluetooth.protocol import BLUETOOTH_SOCKET_PATH
 from openpilot.starpilot.system.bluetooth.radio import BluetoothRadio
 
 
-OFFROAD_COMMANDS = {"set_power", "start_scan", "stop_scan", "pair", "forget", "test_audio", "pairing_response",
-                    "elm_open", "elm_command", "elm_read_dtcs"}
+OFFROAD_COMMANDS = {"set_power", "start_scan", "stop_scan", "pair", "forget", "test_audio", "pairing_response"}
 SCAN_DURATION = 20.0
 AUDIO_TEST_START_DELAY = 3.0
 AUDIO_TEST_HOLD_TIME = 3.0
@@ -23,15 +21,13 @@ AUDIO_TEST_HOLD_TIME = 3.0
 
 class BluetoothController:
   def __init__(self, params: Params | None = None, bluez_factory=BlueZClient, radio: BluetoothRadio | None = None,
-               params_memory: Params | None = None, sleep=time.sleep, elm_factory=ELM327Session):
+               params_memory: Params | None = None, sleep=time.sleep):
     self.params = params or Params()
     self.params_memory = params_memory or Params(memory=True)
     self._bluez_factory = bluez_factory
-    self._elm_factory = elm_factory
     self._radio = radio or BluetoothRadio()
     self._lock = threading.RLock()
     self._bluez: BlueZClient | None = None
-    self._elm: ELM327Session | None = None
     self._pairing_address = ""
     self._pairing_error = ""
     self._last_reconnect = 0.0
@@ -45,7 +41,6 @@ class BluetoothController:
     self.params.remove("BluetoothAudioTestActive")
     self.params_memory.remove("TestAlert")
     with self._lock:
-      self._close_elm()
       if self._bluez is not None:
         self._bluez.close()
         self._bluez = None
@@ -80,7 +75,6 @@ class BluetoothController:
 
   def _reset_client(self) -> None:
     with self._lock:
-      self._close_elm()
       if self._bluez is not None:
         try:
           self._bluez.close()
@@ -88,32 +82,6 @@ class BluetoothController:
           pass
         self._bluez = None
 
-  def _close_elm(self) -> None:
-    with self._lock:
-      session = self._elm
-      self._elm = None
-      if session is None:
-        return
-      try:
-        session.close()
-      except Exception:
-        cloudlog.warning("ELM327 session close failed")
-
-  def _invalidate_elm(self, session: ELM327Session) -> None:
-    with self._lock:
-      if self._elm is not session:
-        return
-      self._close_elm()
-
-  def _active_elm(self, address: str) -> ELM327Session:
-    with self._lock:
-      session = self._elm
-      if session is None:
-        raise RuntimeError("ELM327 session is not open")
-      if str(session.address).upper() != address.upper():
-        raise RuntimeError("ELM327 session is open for another device")
-      return session
-
   def _offroad(self) -> bool:
     return self.params.get_bool("IsOffroad")
 
@@ -210,7 +178,6 @@ class BluetoothController:
               pass
             raise
         else:
-          self._close_elm()
           try:
             client = self._bluez
             if client is not None:
@@ -242,9 +209,6 @@ class BluetoothController:
     elif command == "disconnect":
       self._client().disconnect(address)
     elif command == "forget":
-      with self._lock:
-        if self._elm is not None and str(self._elm.address).upper() == address.upper():
-          self._close_elm()
       self._client().remove(address)
       if (self.params.get("BluetoothAudioAddress", encoding="utf-8") or "").upper() == address.upper():
         self.params.remove("BluetoothAudioAddress")
@@ -270,50 +234,6 @@ class BluetoothController:
       self._audio_test_deadline = deadline
       threading.Thread(target=self._test_audio_worker, args=(address, deadline), daemon=True).start()
       return {"audio_test_delay_ms": max(0, round((deadline - time.monotonic()) * 1000))}
-    elif command == "elm_open":
-      if not address:
-        raise RuntimeError("Bluetooth device address is required")
-      if not self.params.get_bool("BluetoothEnabled"):
-        raise RuntimeError("Bluetooth is disabled")
-      with self._lock:
-        if self._elm is not None and str(self._elm.address).upper() == address.upper():
-          return {"adapter": self._elm.adapter_name}
-        device = self._client().device_for_address(address)
-        if not device.get("paired"):
-          raise RuntimeError("Pair the Bluetooth device before opening ELM327")
-        if not device.get("serial"):
-          raise RuntimeError("Bluetooth device does not advertise Serial Port Profile")
-        self._close_elm()
-        session = self._elm_factory(address)
-        try:
-          adapter = str(session.open())
-        except Exception:
-          try:
-            session.close()
-          except Exception:
-            pass
-          raise
-        session.adapter_name = adapter
-        self._elm = session
-        return {"adapter": adapter}
-    elif command == "elm_close":
-      with self._lock:
-        if self._elm is not None and str(self._elm.address).upper() == address.upper():
-          self._close_elm()
-    elif command == "elm_command":
-      session = self._active_elm(address)
-      try:
-        return {"response": session.command(str(request.get("value", "")))}
-      except Exception:
-        self._invalidate_elm(session)
-        raise
-    elif command == "elm_read_dtcs":
-      session = self._active_elm(address)
-      try:
-        return session.read_dtcs()
-      except Exception:
-        self._invalidate_elm(session)
-        raise
     elif command == "pairing_response":
       if not self._client().agent.respond(str(request.get("prompt_id", "")), bool(request.get("accepted", False)), str(request.get("value", ""))):
         raise RuntimeError("Pairing request is no longer active")
@@ -332,14 +252,10 @@ class BluetoothController:
     while True:
       time.sleep(2)
       if not self.params.get_bool("BluetoothEnabled"):
-        self._close_elm()
         continue
       try:
         status = self.status()
-        if self._elm is not None and not status["offroad"]:
-          self._close_elm()
         if not status["available"] or not status["powered"]:
-          self._close_elm()
           continue
         now = time.monotonic()
         self._maintain_scan(status, now)

starpilot/system/bluetooth/elm327.py

@@ -1,166 +0,0 @@
-from __future__ import annotations
-
-import re
-import socket
-import threading
-
-
-DEFAULT_CHANNEL = 1
-OPEN_TIMEOUT = 10.0
-DEFAULT_COMMAND_TIMEOUT = 10.0
-DTC_COMMAND_TIMEOUT = 25.0
-MAX_COMMAND_LENGTH = 256
-MAX_RESPONSE_SIZE = 64 * 1024
-RECV_SIZE = 4096
-
-
-class DTCParseError(ValueError):
-  def __init__(self, message: str, raw: str):
-    super().__init__(message)
-    self.raw = raw
-
-
-def decode_dtc(first: int, second: int) -> str:
-  prefixes = "PCBU"
-  prefix = prefixes[(first >> 6) & 0x03]
-  return f"{prefix}{(first >> 4) & 0x03:X}{first & 0x0F:X}{second >> 4:X}{second & 0x0F:X}"
-
-
-_HEX_BYTE = re.compile(r"(?i)(?<![0-9a-f])([0-9a-f]{2})(?![0-9a-f])")
-
-
-def parse_dtcs(raw: str) -> list[str]:
-  codes = []
-  seen = set()
-  for line in raw.splitlines():
-    values = [int(match, 16) for match in _HEX_BYTE.findall(line)]
-    try:
-      response_index = values.index(0x43)
-    except ValueError:
-      continue
-
-    payload = values[response_index + 1:]
-    if len(payload) % 2:
-      count = payload[0]
-      expected_length = count * 2
-      if len(payload) - 1 < expected_length:
-        raise DTCParseError(f"Mode 03 response claims {expected_length} DTC bytes, received {len(payload) - 1}", raw)
-      payload = payload[1:1 + expected_length]
-
-    for index in range(0, len(payload) - 1, 2):
-      first, second = payload[index:index + 2]
-      if first == 0 and second == 0:
-        continue
-      code = decode_dtc(first, second)
-      if code not in seen:
-        seen.add(code)
-        codes.append(code)
-  return codes
-
-
-class ELM327Session:
-  def __init__(self, address: str, channel: int = DEFAULT_CHANNEL):
-    self.address = address
-    self.channel = channel
-    self.socket: socket.socket | None = None
-    self.lock = threading.RLock()
-    self.adapter_name = ""
-
-  def _close_unlocked(self) -> None:
-    client_socket = self.socket
-    self.socket = None
-    self.adapter_name = ""
-    if client_socket is not None:
-      try:
-        client_socket.close()
-      except Exception:
-        pass
-
-  def close(self) -> None:
-    with self.lock:
-      self._close_unlocked()
-
-  def _receive_until_prompt_unlocked(self, timeout: float) -> bytes:
-    if self.socket is None:
-      raise RuntimeError("ELM327 session is not open")
-    self.socket.settimeout(timeout)
-    response = bytearray()
-    while True:
-      chunk = self.socket.recv(RECV_SIZE)
-      if not chunk:
-        raise RuntimeError("ELM327 connection closed")
-      response.extend(chunk)
-      if len(response) > MAX_RESPONSE_SIZE:
-        raise RuntimeError("ELM327 response exceeded 64 KiB")
-      if b">" in response:
-        return bytes(response)
-
-  @staticmethod
-  def _clean_response(raw: bytes, command: str) -> str:
-    response = raw.split(b">", 1)[0].decode("ascii", errors="replace")
-    response = response.replace("\r\n", "\n").replace("\r", "\n")
-    lines = response.split("\n")
-    while lines and not lines[0].strip():
-      lines.pop(0)
-    if lines and lines[0].strip() == command:
-      lines.pop(0)
-    return "\n".join(lines).strip()
-
-  def _exchange_unlocked(self, command: str, timeout: float) -> str:
-    if self.socket is None:
-      raise RuntimeError("ELM327 session is not open")
-    try:
-      self.socket.settimeout(timeout)
-      self.socket.sendall(command.encode("ascii") + b"\r")
-      return self._clean_response(self._receive_until_prompt_unlocked(timeout), command)
-    except Exception as error:
-      self._close_unlocked()
-      raise RuntimeError(f"ELM327 transport failed: {error}") from error
-
-  def open(self) -> str:
-    with self.lock:
-      if self.socket is not None:
-        return self.adapter_name
-      try:
-        self.socket = socket.socket(socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM)
-        self.socket.settimeout(OPEN_TIMEOUT)
-        self.socket.connect((self.address, self.channel))
-        adapter_name = self._exchange_unlocked("ATI", OPEN_TIMEOUT)
-        if not adapter_name or adapter_name.strip().upper() in {"?", "ERROR", "COMMAND UNKNOWN", "UNKNOWN COMMAND", "NO DATA"}:
-          raise RuntimeError("ELM327 adapter rejected ATI")
-        self.adapter_name = adapter_name
-        for setup_command in ("ATE0", "ATL0", "ATH0"):
-          self._exchange_unlocked(setup_command, OPEN_TIMEOUT)
-        return self.adapter_name
-      except Exception as error:
-        self._close_unlocked()
-        if isinstance(error, RuntimeError) and str(error).startswith("ELM327 open failed:"):
-          raise
-        raise RuntimeError(f"ELM327 open failed: {error}") from error
-
-  def command(self, command: str, timeout: float = DEFAULT_COMMAND_TIMEOUT) -> str:
-    if not isinstance(command, str):
-      raise ValueError("ELM327 command must be text")
-    if "\r" in command or "\n" in command:
-      raise ValueError("ELM327 command cannot contain carriage returns or newlines")
-    command = command.strip()
-    if not command:
-      raise ValueError("ELM327 command cannot be empty")
-    if len(command) > MAX_COMMAND_LENGTH:
-      raise ValueError("ELM327 command is too long")
-    try:
-      command.encode("ascii")
-    except UnicodeEncodeError as error:
-      raise ValueError("ELM327 command must contain ASCII characters") from error
-    if timeout <= 0:
-      raise ValueError("ELM327 command timeout must be positive")
-
-    with self.lock:
-      return self._exchange_unlocked(command, timeout)
-
-  def read_dtcs(self) -> dict[str, str | list[str]]:
-    with self.lock:
-      for setup_command in ("ATE0", "ATL0", "ATH0", "ATSP0"):
-        self.command(setup_command)
-      raw = self.command("03", timeout=DTC_COMMAND_TIMEOUT)
-    return {"codes": parse_dtcs(raw), "raw": raw}

starpilot/system/bluetooth/protocol.py

@@ -16,7 +16,6 @@ BLUETOOTH_RADIO_HELPER = "/usr/comma/bluetooth-radio"
 A2DP_SINK_UUID = "0000110b-0000-1000-8000-00805f9b34fb"
 HID_UUID = "00001124-0000-1000-8000-00805f9b34fb"
 HOG_UUID = "00001812-0000-1000-8000-00805f9b34fb"
-SPP_UUID = "00001101-0000-1000-8000-00805f9b34fb"
 COMMAND_TIMEOUTS = {
   "set_power": 90.0,
   "start_scan": 20.0,
@@ -25,10 +24,6 @@ COMMAND_TIMEOUTS = {
   "disconnect": 20.0,
   "forget": 20.0,
   "test_audio": 10.0,
-  "elm_open": 15.0,
-  "elm_close": 5.0,
-  "elm_command": 20.0,
-  "elm_read_dtcs": 30.0,
 }
 TRUE_VALUES = {"1", "true", "yes", "on"}
 
@@ -45,7 +40,6 @@ class BluetoothDevice:
   uuids: tuple[str, ...] = ()
   audio: bool = False
   controller: bool = False
-  serial: bool = False
 
   @classmethod
   def from_dict(cls, value: dict[str, Any]) -> "BluetoothDevice":
@@ -60,7 +54,6 @@ class BluetoothDevice:
       uuids=tuple(str(uuid).lower() for uuid in value.get("uuids", ())),
       audio=bool(value.get("audio", False)),
       controller=bool(value.get("controller", False)),
-      serial=bool(value.get("serial", False)),
     )
 
 
@@ -93,22 +86,21 @@ class BluetoothStatus:
     )
 
 
-def device_capabilities(uuids: list[str] | tuple[str, ...], bluetooth_class: int = 0, icon: str = "") -> tuple[bool, bool, bool]:
+def device_capabilities(uuids: list[str] | tuple[str, ...], bluetooth_class: int = 0, icon: str = "") -> tuple[bool, bool]:
   normalized = {str(uuid).lower() for uuid in uuids}
   major_class = (int(bluetooth_class) >> 8) & 0x1F
   audio = A2DP_SINK_UUID in normalized or major_class == 0x04 or icon in {"audio-card", "audio-headphones", "audio-headset"}
   controller = HID_UUID in normalized or HOG_UUID in normalized or major_class == 0x05 or icon in {"input-gaming", "input-mouse", "input-keyboard"}
-  serial = SPP_UUID in normalized
-  return audio, controller, serial
+  return audio, controller
 
 
 def show_pairing_device(address: str, name: str, paired: bool, trusted: bool, connected: bool, blocked: bool,
-                        audio: bool, controller: bool, serial: bool = False, discovering: bool = False) -> bool:
+                        audio: bool, controller: bool, discovering: bool = False) -> bool:
   known = paired or trusted or connected
   normalized_address = "".join(character for character in address.upper() if character.isalnum())
   normalized_name = "".join(character for character in name.upper() if character.isalnum())
   named = bool(name) and name != "Unknown device" and normalized_name != normalized_address
-  return known or (named and not blocked and (audio or controller or serial))
+  return known or (named and not blocked and (audio or controller))
 
 
 class _DesktopFakeBluetooth:
@@ -312,17 +304,5 @@ class BluetoothClient:
     result = self.call("test_audio", address=address)
     return max(0.0, float(result.get("audio_test_delay_ms", 0)) / 1000.0)
 
-  def elm_open(self, address: str) -> dict[str, Any]:
-    return self.call("elm_open", address=address)
-
-  def elm_close(self, address: str) -> dict[str, Any]:
-    return self.call("elm_close", address=address)
-
-  def elm_command(self, address: str, value: str) -> dict[str, Any]:
-    return self.call("elm_command", address=address, value=value)
-
-  def elm_read_dtcs(self, address: str) -> dict[str, Any]:
-    return self.call("elm_read_dtcs", address=address)
-
   def respond(self, prompt_id: str, accepted: bool, value: str = "") -> None:
     self.call("pairing_response", prompt_id=prompt_id, accepted=accepted, value=value)

starpilot/system/bluetooth/tests/test_bluetooth.py

@@ -6,11 +6,10 @@ import numpy as np
 import pytest
 
 from openpilot.starpilot.system.bluetooth.audio import BluetoothAudioSink
-import openpilot.starpilot.system.bluetooth.daemon as bluetooth_daemon
 from openpilot.starpilot.system.bluetooth.bluez import PairingAgent
 from openpilot.starpilot.system.bluetooth.daemon import BluetoothController
 from openpilot.starpilot.system.bluetooth.protocol import (A2DP_SINK_UUID, HID_UUID, BluetoothClient, BluetoothDevice, BluetoothStatus,
-                                                           SPP_UUID, device_capabilities, show_pairing_device)
+                                                           device_capabilities, show_pairing_device)
 from openpilot.system import hardware
 from openpilot.system.ui.lib.bluetooth_manager import BluetoothManager
 
@@ -65,7 +64,6 @@ class FakeBlueZ:
       "connected": False,
       "audio": True,
       "controller": False,
-      "serial": False,
     }
 
   def close(self):
@@ -165,38 +163,9 @@ class FakeProcess:
     self.stopped = True
 
 
-class FakeELM:
-  instances = []
-
-  def __init__(self, address):
-    self.address = address
-    self.adapter_name = "Fake ELM327"
-    self.closed = False
-    self.commands = []
-    self.opened = False
-    FakeELM.instances.append(self)
-
-  def open(self):
-    self.opened = True
-    return self.adapter_name
-
-  def close(self):
-    self.closed = True
-
-  def command(self, value):
-    self.commands.append(value)
-    if value == "fail":
-      raise RuntimeError("transport failed")
-    return f"response for {value}"
-
-  def read_dtcs(self):
-    return {"codes": ["P0133"], "raw": "43 01 33"}
-
-
 def test_protocol_round_trip_and_capabilities():
-  audio, controller, serial = device_capabilities([A2DP_SINK_UUID, HID_UUID])
+  audio, controller = device_capabilities([A2DP_SINK_UUID, HID_UUID])
   assert audio and controller
-  assert not serial
   status = BluetoothStatus.from_dict({
     "available": True,
     "enabled": True,
@@ -205,155 +174,12 @@ def test_protocol_round_trip_and_capabilities():
   assert status.devices == (BluetoothDevice("00:11:22:33:44:55", "Combo", uuids=(A2DP_SINK_UUID, HID_UUID), audio=True, controller=True),)
 
 
-def test_serial_capability_round_trips_and_is_discoverable():
-  audio, controller, serial = device_capabilities([SPP_UUID.upper()])
-  assert not audio and not controller and serial
-
-  status = BluetoothStatus.from_dict({
-    "devices": [{"address": "00:11:22:33:44:55", "name": "OBDII", "serial": True}],
-  })
-  assert status.devices[0].serial
-  assert show_pairing_device("00:11:22:33:44:55", "OBDII", False, False, False, False,
-                             audio=False, controller=False, serial=True)
-
-
-def test_serial_device_is_not_auto_reconnected_but_audio_device_is(monkeypatch):
-  class StopMaintenance(Exception):
-    pass
-
-  params = FakeParams(IsOffroad=True, BluetoothEnabled=True)
-  client = FakeBlueZ()
-  client.powered = True
-  client.device.update(paired=True, trusted=True, connected=False, audio=False, controller=False, serial=True)
-  sleeps = 0
-
-  def sleep(_delay):
-    nonlocal sleeps
-    sleeps += 1
-    if sleeps > 1:
-      raise StopMaintenance
-
-  monkeypatch.setattr(bluetooth_daemon.time, "sleep", sleep)
-  controller = BluetoothController(params, lambda: client, FakeRadio(), sleep=sleep, elm_factory=FakeELM)
-  controller._bluez = client
-  controller._last_reconnect = -100.0
-  with pytest.raises(StopMaintenance):
-    controller.maintain_connections()
-  assert client.actions == []
-
-  client.device.update(audio=True, serial=False)
-  sleeps = 0
-  controller._last_reconnect = -100.0
-  with pytest.raises(StopMaintenance):
-    controller.maintain_connections()
-  assert client.actions == [("connect", client.device["address"])]
-
-
-def make_elm_controller(elm_factory=FakeELM):
-  params = FakeParams(IsOffroad=True, BluetoothEnabled=True)
-  client = FakeBlueZ()
-  client.device.update(paired=True, trusted=True, serial=True)
-  controller = BluetoothController(params, lambda: client, FakeRadio(), elm_factory=elm_factory)
-  return controller, client, params
-
-
-def test_elm_is_lazy_and_requires_a_paired_serial_device():
-  FakeELM.instances = []
-  controller, client, params = make_elm_controller()
-  assert FakeELM.instances == []
-  controller.status()
-  assert FakeELM.instances == []
-
-  result = controller.handle({"command": "elm_open", "address": client.device["address"]})
-  assert result == {"adapter": "Fake ELM327"}
-  assert len(FakeELM.instances) == 1
-
-  params.values["IsOffroad"] = False
-  with pytest.raises(RuntimeError, match="offroad"):
-    controller.handle({"command": "elm_command", "address": client.device["address"], "value": "ATI"})
-
-  params.values["IsOffroad"] = True
-  client.device["paired"] = False
-  controller.handle({"command": "elm_close", "address": client.device["address"]})
-  with pytest.raises(RuntimeError, match="Pair"):
-    controller.handle({"command": "elm_open", "address": client.device["address"]})
-
-  client.device.update(paired=True, serial=False)
-  with pytest.raises(RuntimeError, match="Serial Port Profile"):
-    controller.handle({"command": "elm_open", "address": client.device["address"]})
-
-
-def test_elm_commands_use_one_session_and_close_on_transport_failure():
-  FakeELM.instances = []
-  controller, client, _ = make_elm_controller()
-  address = client.device["address"]
-  controller.handle({"command": "elm_open", "address": address})
-  assert controller.handle({"command": "elm_open", "address": address}) == {"adapter": "Fake ELM327"}
-  assert controller.handle({"command": "elm_command", "address": address, "value": "ATI"}) == {"response": "response for ATI"}
-  assert controller.handle({"command": "elm_read_dtcs", "address": address}) == {"codes": ["P0133"], "raw": "43 01 33"}
-  with pytest.raises(RuntimeError, match="transport"):
-    controller.handle({"command": "elm_command", "address": address, "value": "fail"})
-  assert controller._elm is None
-  assert FakeELM.instances[0].closed
-
-
-def test_elm_open_replaces_a_different_session_and_close_is_allowed_onroad():
-  FakeELM.instances = []
-  controller, client, params = make_elm_controller()
-  first = client.device["address"]
-  second = "AA:BB:CC:DD:EE:FF"
-  controller.handle({"command": "elm_open", "address": first})
-  controller.handle({"command": "elm_open", "address": second})
-  assert len(FakeELM.instances) == 2
-  assert FakeELM.instances[0].closed
-  assert not FakeELM.instances[1].closed
-
-  params.values["IsOffroad"] = False
-  controller.handle({"command": "elm_close", "address": second})
-  assert FakeELM.instances[1].closed and controller._elm is None
-
-
-def test_elm_cleanup_happens_before_poweroff_forget_shutdown_and_onroad(monkeypatch):
-  class StopMaintenance(Exception):
-    pass
-
-  for cleanup in ("power", "forget", "shutdown", "onroad"):
-    FakeELM.instances = []
-    controller, client, params = make_elm_controller()
-    address = client.device["address"]
-    controller.handle({"command": "elm_open", "address": address})
-    session = FakeELM.instances[0]
-
-    if cleanup == "power":
-      controller.handle({"command": "set_power", "enabled": False})
-    elif cleanup == "forget":
-      controller.handle({"command": "forget", "address": address})
-    elif cleanup == "shutdown":
-      controller.close()
-    else:
-      params.values["IsOffroad"] = False
-      sleeps = 0
-
-      def sleep(_delay):
-        nonlocal sleeps
-        sleeps += 1
-        if sleeps > 1:
-          raise StopMaintenance
-
-      monkeypatch.setattr(bluetooth_daemon.time, "sleep", sleep)
-      controller._sleep = sleep
-      with pytest.raises(StopMaintenance):
-        controller.maintain_connections()
-
-    assert session.closed and controller._elm is None
-
-
 def test_pairing_list_filters_anonymous_and_irrelevant_advertisements():
   assert not show_pairing_device("00:11:22:33:44:55", "00:11:22:33:44:55", False, False, False, False, False, False)
   assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False)
   assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True)
-  assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True, discovering=True)
-  assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False, discovering=True)
+  assert show_pairing_device("00:11:22:33:44:55", "Media Remote", False, False, False, False, False, True, True)
+  assert not show_pairing_device("00:11:22:33:44:55", "Nearby sensor", False, False, False, False, False, False, True)
   assert show_pairing_device("00:11:22:33:44:55", "Known device", True, True, False, False, False, False)
 
 

starpilot/system/bluetooth/tests/test_elm327.py

@@ -1,227 +0,0 @@
-from collections import deque
-import threading
-
-import pytest
-
-from openpilot.starpilot.system.bluetooth import elm327
-
-
-ADDRESS = "00:11:22:33:44:55"
-
-
-class FakeSocket:
-  def __init__(self, responses):
-    self.responses = deque(deque(response) for response in responses)
-    self.pending = deque()
-    self.sent = []
-    self.connected_to = None
-    self.timeouts = []
-    self.closed = False
-    self.close_calls = 0
-    self.recv_error = None
-    self.send_error = None
-
-  def settimeout(self, timeout):
-    self.timeouts.append(timeout)
-
-  def connect(self, address):
-    self.connected_to = address
-
-  def sendall(self, value):
-    if self.send_error is not None:
-      raise self.send_error
-    self.sent.append(value)
-    self.pending = self.responses.popleft() if self.responses else deque()
-
-  def recv(self, _size):
-    if self.recv_error is not None:
-      raise self.recv_error
-    return self.pending.popleft() if self.pending else b""
-
-  def close(self):
-    self.close_calls += 1
-    self.closed = True
-
-
-def startup_responses(identity=b"ELM327 v1.5"):
-  return [
-    [b"ATI\r\n", identity + b"\r\n>"],
-    [b"ATE0\r\nOK\r\n>"],
-    [b"ATL0\r\nOK\r\n>"],
-    [b"ATH0\r\nOK\r\n>"],
-  ]
-
-
-def make_session(monkeypatch, responses):
-  fake = FakeSocket(responses)
-  monkeypatch.setattr(elm327.socket, "AF_BLUETOOTH", 31, raising=False)
-  monkeypatch.setattr(elm327.socket, "BTPROTO_RFCOMM", 3, raising=False)
-  monkeypatch.setattr(elm327.socket, "socket", lambda *args: fake)
-  return elm327.ELM327Session(ADDRESS), fake
-
-
-def test_open_uses_rfccomm_channel_one_and_validates_ati(monkeypatch):
-  session, fake = make_session(monkeypatch, startup_responses())
-
-  assert session.open() == "ELM327 v1.5"
-  assert fake.connected_to == (ADDRESS, 1)
-  assert fake.sent == [b"ATI\r", b"ATE0\r", b"ATL0\r", b"ATH0\r"]
-  assert session.adapter_name == "ELM327 v1.5"
-
-
-def test_open_rejects_empty_or_obviously_rejected_ati(monkeypatch):
-  for identity in (b"", b"?", b"ERROR"):
-    session, fake = make_session(monkeypatch, startup_responses(identity))
-    with pytest.raises(RuntimeError, match="ATI"):
-      session.open()
-    assert session.socket is None and fake.closed
-
-
-def test_command_removes_exact_echo_and_reads_split_prompt_response(monkeypatch):
-  responses = startup_responses() + [[b"ATR", b"V\r\n12.4V\r", b"\n>"]]
-  session, fake = make_session(monkeypatch, responses)
-  session.open()
-
-  assert session.command(" ATRV ") == "12.4V"
-  assert fake.sent[-1] == b"ATRV\r"
-
-
-def test_malformed_response_bytes_decode_with_replacement(monkeypatch):
-  responses = startup_responses() + [[b"ATI\r\n\xffOK\r\n>"]]
-  session, _ = make_session(monkeypatch, responses)
-  session.open()
-
-  assert session.command("ATI") == "�OK"
-
-
-@pytest.mark.parametrize("command", ["", "   ", "ATI\r", "ATI\n", "AT\r\nI", "A" * (elm327.MAX_COMMAND_LENGTH + 1)])
-def test_command_rejects_invalid_input(monkeypatch, command):
-  session, _ = make_session(monkeypatch, [])
-  with pytest.raises(ValueError):
-    session.command(command)
-
-
-def test_command_rejects_non_ascii_input(monkeypatch):
-  session, _ = make_session(monkeypatch, [])
-  with pytest.raises(ValueError, match="ASCII"):
-    session.command("ATé")
-
-
-def test_response_size_limit_closes_session(monkeypatch):
-  responses = startup_responses() + [[b"x" * (elm327.MAX_RESPONSE_SIZE + 1)]]
-  session, fake = make_session(monkeypatch, responses)
-  session.open()
-
-  with pytest.raises(RuntimeError, match="64 KiB"):
-    session.command("ATI")
-  assert session.socket is None and fake.closed
-
-
-@pytest.mark.parametrize("error", [TimeoutError("timed out"), OSError("disconnected")])
-def test_timeout_or_eof_closes_session(monkeypatch, error):
-  responses = startup_responses() + [[]]
-  session, fake = make_session(monkeypatch, responses)
-  session.open()
-  fake.recv_error = error if isinstance(error, TimeoutError) else None
-
-  if isinstance(error, TimeoutError):
-    with pytest.raises(RuntimeError, match="transport"):
-      session.command("ATI")
-  else:
-    with pytest.raises(RuntimeError, match="connection closed"):
-      session.command("ATI")
-  assert session.socket is None and fake.closed
-
-
-def test_send_failure_closes_session(monkeypatch):
-  responses = startup_responses() + [[b"OK>"]]
-  session, fake = make_session(monkeypatch, responses)
-  session.open()
-  fake.send_error = OSError("send failed")
-
-  with pytest.raises(RuntimeError, match="transport"):
-    session.command("ATI")
-  assert session.socket is None and fake.closed
-
-
-def test_close_is_idempotent(monkeypatch):
-  session, fake = make_session(monkeypatch, startup_responses())
-  session.open()
-  session.close()
-  session.close()
-
-  assert fake.close_calls == 1
-  assert session.socket is None
-
-
-def test_simultaneous_commands_are_serialized(monkeypatch):
-  class SerializedSocket(FakeSocket):
-    def __init__(self, responses):
-      super().__init__(responses)
-      self.command_started = threading.Event()
-      self.release_command = threading.Event()
-      self._command_sends = 0
-
-    def sendall(self, value):
-      super().sendall(value)
-      self._command_sends += 1
-      if self._command_sends == 5:
-        self.command_started.set()
-        assert self.release_command.wait(timeout=1.0)
-
-  fake = SerializedSocket(startup_responses() + [[b"VALUE1>"], [b"VALUE2>"]])
-  monkeypatch.setattr(elm327.socket, "AF_BLUETOOTH", 31, raising=False)
-  monkeypatch.setattr(elm327.socket, "BTPROTO_RFCOMM", 3, raising=False)
-  monkeypatch.setattr(elm327.socket, "socket", lambda *args: fake)
-  session = elm327.ELM327Session(ADDRESS)
-  session.open()
-
-  results = []
-  first = threading.Thread(target=lambda: results.append(session.command("ONE")))
-  second = threading.Thread(target=lambda: results.append(session.command("TWO")))
-  first.start()
-  assert fake.command_started.wait(timeout=1.0)
-  second.start()
-  assert len(fake.sent) == 5
-  fake.release_command.set()
-  first.join(timeout=1.0)
-  second.join(timeout=1.0)
-
-  assert sorted(results) == ["VALUE1", "VALUE2"]
-  assert len(fake.sent) == 6
-
-
-def test_read_dtcs_runs_known_setup_and_returns_mode_three_result(monkeypatch):
-  responses = startup_responses() + [
-    [b"OK>"], [b"OK>"], [b"OK>"], [b"OK>"],
-    [b"43 01 33 04 20 00 00>"],
-  ]
-  session, fake = make_session(monkeypatch, responses)
-  session.open()
-
-  assert session.read_dtcs() == {"codes": ["P0133", "P0420"], "raw": "43 01 33 04 20 00 00"}
-  assert fake.sent[-5:] == [b"ATE0\r", b"ATL0\r", b"ATH0\r", b"ATSP0\r", b"03\r"]
-
-
-def test_non_can_dtc_is_decoded_and_padding_ignored():
-  assert elm327.parse_dtcs("43 01 33 00 00 00 00") == ["P0133"]
-
-
-def test_can_dtc_count_byte_is_skipped():
-  assert elm327.parse_dtcs("43 02 01 33 04 20") == ["P0133", "P0420"]
-
-
-def test_no_data_returns_no_codes():
-  assert elm327.parse_dtcs("NO DATA") == []
-
-
-def test_multiple_ecu_lines_are_ordered_and_deduplicated():
-  raw = "43 01 33 00 00\n43 04 20 00 00\n43 01 33 00 00"
-  assert elm327.parse_dtcs(raw) == ["P0133", "P0420"]
-
-
-def test_malformed_can_count_raises_with_raw_response():
-  raw = "43 02 01 33"
-  with pytest.raises(elm327.DTCParseError) as error:
-    elm327.parse_dtcs(raw)
-  assert error.value.raw == raw

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

@@ -398,116 +398,6 @@
   animation-delay: 0.28s;
 }
 
-.bluetoothElmPanel {
-  background: var(--secondary-bg);
-  border: 1px solid rgba(169, 140, 229, 0.45);
-  border-radius: var(--border-radius-lg);
-  box-shadow: var(--shadow-sm);
-  padding: 18px;
-}
-
-.bluetoothElmHeader,
-.bluetoothElmActions,
-.bluetoothElmCommand > div {
-  align-items: center;
-  display: flex;
-  gap: 10px;
-}
-
-.bluetoothElmHeader {
-  justify-content: space-between;
-  gap: 18px;
-}
-
-.bluetoothElmHeader h3,
-.bluetoothElmHeader p,
-.bluetoothElmCodes p,
-.bluetoothElmResponse pre,
-.bluetoothElmCommand label {
-  margin: 0;
-}
-
-.bluetoothElmHeader p,
-.bluetoothElmCodes p {
-  color: var(--text-muted);
-  margin-top: 4px;
-}
-
-.bluetoothElmHeader strong {
-  color: #cbb2fa;
-  overflow: hidden;
-  text-overflow: ellipsis;
-  white-space: nowrap;
-}
-
-.bluetoothElmActions {
-  margin-top: 16px;
-}
-
-.bluetoothElmActions button,
-.bluetoothElmCommand button {
-  background: linear-gradient(135deg, #765bb6, #9474ce);
-  border: 0;
-  border-radius: var(--border-radius-md);
-  color: #fff;
-  cursor: pointer;
-  font-weight: 700;
-  padding: 10px 14px;
-}
-
-.bluetoothElmActions .bluetoothSecondaryButton {
-  background: var(--input-bg);
-  border: 1px solid var(--sidebar-border-color);
-  color: var(--text-color);
-}
-
-.bluetoothElmActions button:disabled,
-.bluetoothElmCommand button:disabled,
-.bluetoothElmCommand input:disabled {
-  cursor: not-allowed;
-  opacity: 0.45;
-}
-
-.bluetoothElmCodes,
-.bluetoothElmCommand,
-.bluetoothElmResponse {
-  margin-top: 16px;
-}
-
-.bluetoothElmCommand label {
-  color: var(--text-muted);
-  display: block;
-  font-size: 0.86rem;
-  margin-bottom: 6px;
-}
-
-.bluetoothElmCommand > div {
-  align-items: stretch;
-}
-
-.bluetoothElmCommand input {
-  background: var(--input-bg);
-  border: 1px solid var(--sidebar-border-color);
-  border-radius: var(--border-radius-md);
-  color: var(--text-color);
-  flex: 1;
-  font: inherit;
-  min-width: 0;
-  padding: 10px 12px;
-}
-
-.bluetoothElmResponse pre {
-  background: var(--input-bg);
-  border: 1px solid var(--sidebar-border-color);
-  border-radius: var(--border-radius-md);
-  color: var(--text-color);
-  margin-top: 6px;
-  max-height: 220px;
-  overflow: auto;
-  padding: 12px;
-  white-space: pre-wrap;
-}
-
 @keyframes bluetoothSpin {
   to { transform: rotate(360deg); }
 }

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

@@ -20,11 +20,6 @@ const state = reactive({
   prompt: null,
   audioTestAddress: "",
   audioTestLabel: "",
-  elmAddress: "",
-  elmName: "",
-  elmAdapter: "",
-  elmResponse: "",
-  elmCodes: null,
   error: "",
 })
 
@@ -51,9 +46,7 @@ function schedulePoll(delay = pollDelay()) {
   pollTimer = setTimeout(async () => {
     pollTimer = null
     try {
-      if (state.elmAddress && (document.visibilityState === "hidden" || !bluetoothPageActive())) {
-        closeElm()
-      } else if (bluetoothPageActive() && document.visibilityState !== "hidden" && state.busy !== "power") {
+      if (bluetoothPageActive() && document.visibilityState !== "hidden" && state.busy !== "power") {
         await refresh()
       }
     } finally {
@@ -104,10 +97,8 @@ async function request(operation, body = {}) {
     }
     state.error = ""
     await refresh()
-    return payload
   } catch (error) {
     state.error = error?.message || "Bluetooth operation failed"
-    return null
   } finally {
     state.busy = ""
     if (operation === "power") state.powerTarget = null
@@ -115,54 +106,6 @@ async function request(operation, body = {}) {
   }
 }
 
-function clearElmState() {
-  state.elmAddress = ""
-  state.elmName = ""
-  state.elmAdapter = ""
-  state.elmResponse = ""
-  state.elmCodes = null
-}
-
-function closeElm() {
-  const address = state.elmAddress
-  if (!address) return
-  clearElmState()
-  request("elm_close", { address })
-}
-
-async function openElm(address) {
-  const device = state.devices.find((item) => normalizedAddress(item) === String(address || "").toUpperCase())
-  const payload = await request("elm_open", { address })
-  if (!payload || !device) return
-  state.elmAddress = address
-  state.elmName = device.name || address
-  state.elmAdapter = String(payload.adapter || "")
-  state.elmResponse = ""
-  state.elmCodes = null
-}
-
-async function readElmCodes() {
-  const address = state.elmAddress
-  if (!address) return
-  const payload = await request("elm_read_dtcs", { address })
-  if (!payload || state.elmAddress !== address) return
-  state.elmCodes = Array.isArray(payload.codes) ? payload.codes.map(String) : []
-  state.elmResponse = String(payload.raw || "")
-}
-
-async function sendElmCommand() {
-  const address = state.elmAddress
-  const input = document.getElementById("bluetoothElmCommand")
-  const command = input?.value.trim() || ""
-  if (!address || !command) {
-    state.error = "Enter an ELM327 command."
-    return
-  }
-  const payload = await request("elm_command", { address, value: command })
-  if (!payload || state.elmAddress !== address) return
-  state.elmResponse = `${command}\n${String(payload.response || "")}`.trim()
-}
-
 async function refreshOnce() {
   const statusUrl = `${galaxyPath("/api/bluetooth/status")}?_=${Date.now()}`
   const response = await fetch(statusUrl, { cache: "no-store" })
@@ -183,10 +126,6 @@ async function refreshOnce() {
     devices,
   })
   state.devices = devices
-  if (state.elmAddress && (!state.enabled || !state.offroad ||
-      !devices.some((device) => normalizedAddress(device) === state.elmAddress.toUpperCase() && device.paired))) {
-    closeElm()
-  }
   if (state.deviceSignature !== deviceSignature) {
     state.deviceSignature = deviceSignature
     state.revision++
@@ -284,11 +223,7 @@ function initialize() {
   window.addEventListener("focus", refresh)
   window.addEventListener("pageshow", refresh)
   document.addEventListener("visibilitychange", () => {
-    if (document.visibilityState === "hidden" || !bluetoothPageActive()) {
-      closeElm()
-    } else {
-      refresh()
-    }
+    if (document.visibilityState !== "hidden" && bluetoothPageActive()) refresh()
   })
   refresh()
   schedulePoll(0)
@@ -313,7 +248,6 @@ function deviceCapabilities(device) {
   const capabilities = []
   if (device.audio) capabilities.push("Audio")
   if (device.controller) capabilities.push("Controller")
-  if (device.serial) capabilities.push("Serial")
   return capabilities.join(" · ") || "Bluetooth device"
 }
 
@@ -419,14 +353,7 @@ function renderDeviceActions(device) {
     actions.push("<button data-bluetooth-operation=\"pair\" data-address=\"" + address + "\"" +
       renderDisabledAttribute(!state.offroad || !!state.busy || pairing) + ">" + (pairing ? "Pairing…" : "Pair") + "</button>")
   }
-  if (device.paired && device.serial) {
-    actions.push("<button data-bluetooth-operation=\"elm_open\" data-address=\"" + address + "\"" +
-      renderDisabledAttribute(!state.offroad || !!state.busy) + ">ELM327</button>")
-    actions.push("<button class=\"bluetoothIconButton bluetoothForgetButton\" data-bluetooth-operation=\"forget\" data-address=\"" +
-      address + "\" data-device-name=\"" + name + "\" title=\"Forget device\" aria-label=\"Forget " + name + "\"" +
-      renderDisabledAttribute(!state.offroad || !!state.busy) + "><i class=\"bi bi-trash3\" aria-hidden=\"true\"></i></button>")
-  }
-  if ((device.paired || device.connected) && !device.serial) {
+  if (device.paired || device.connected) {
     const operation = device.connected ? "disconnect" : "connect"
     actions.push("<button data-bluetooth-operation=\"" + operation + "\" data-address=\"" + address + "\"" +
       renderDisabledAttribute(!!state.busy) + ">" + (device.connected ? "Disconnect" : "Connect") + "</button>")
@@ -480,48 +407,7 @@ function handleDeviceListClick(event) {
   const operation = button.dataset.bluetoothOperation
   const address = button.dataset.address || ""
   if (operation === "forget" && !window.confirm("Forget " + (button.dataset.deviceName || "this device") + "?")) return
-  if (operation === "elm_open") {
-    openElm(address)
-  } else {
-    request(operation, { address })
-  }
-}
-
-function renderElmPanel() {
-  if (!state.elmAddress) return ""
-  return html`
-    <section class="bluetoothElmPanel">
-      <div class="bluetoothElmHeader">
-        <div>
-          <h3>ELM327</h3>
-          <p>${() => state.elmName || state.elmAddress}</p>
-        </div>
-        <strong>${() => state.elmAdapter || "Connecting…"}</strong>
-      </div>
-      <div class="bluetoothElmActions">
-        <button disabled="${() => !state.offroad || !!state.busy}" @click="${readElmCodes}">Read Codes</button>
-        <button class="bluetoothSecondaryButton" disabled="${() => !!state.busy}" @click="${closeElm}">Close</button>
-      </div>
-      ${() => state.elmCodes !== null ? html`
-        <div class="bluetoothElmCodes">
-          <strong>Stored Codes</strong>
-          <p>${() => state.elmCodes.length ? state.elmCodes.join(" · ") : "No stored codes reported."}</p>
-        </div>
-      ` : ""}
-      <div class="bluetoothElmCommand">
-        <label for="bluetoothElmCommand">Command</label>
-        <div>
-          <input id="bluetoothElmCommand" type="text" autocomplete="off" placeholder="ATI"
-                 disabled="${() => !state.offroad || !!state.busy}" />
-          <button disabled="${() => !state.offroad || !!state.busy}" @click="${sendElmCommand}">Send</button>
-        </div>
-      </div>
-      <div class="bluetoothElmResponse">
-        <strong>Response</strong>
-        <pre>${() => state.elmResponse || "—"}</pre>
-      </div>
-    </section>
-  `
+  request(operation, { address })
 }
 
 export function Bluetooth() {
@@ -554,7 +440,6 @@ export function Bluetooth() {
           <span>The test sound is sent at NOW. The audible gap is Bluetooth latency.</span>
         </div>
       ` : ""}
-      ${() => renderElmPanel()}
 
       <div class="bluetoothToolbar">
         <button disabled="${() => !state.offroad || !state.enabled || !!state.busy}"

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

@@ -384,6 +384,7 @@
   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;

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

@@ -484,11 +484,11 @@ function formatSliderValue(val, stepStr, precisionInt, key) {
 
 function formatReadoutValue(p) {
   const raw = state.values[p.key]
-  const value = parseFloat(raw)
-  if (raw === undefined || raw === null || Number.isNaN(value)) return "--"
+  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(value.toFixed(Math.max(0, precision))).toString()
+  const formatted = Number(v.toFixed(Math.max(0, precision))).toString()
   return p.unit ? `${formatted}${p.unit}` : formatted
 }
 

starpilot/system/the_galaxy/tests/test_navigation_params.py

@@ -132,15 +132,7 @@ class FakeBluetoothClient:
 
   def call(self, command, **payload):
     self.calls.append((command, payload))
-    if command == "test_audio":
-      return {"audio_test_delay_ms": 3000}
-    if command == "elm_open":
-      return {"adapter": "Fake ELM327"}
-    if command == "elm_command":
-      return {"response": "OK"}
-    if command == "elm_read_dtcs":
-      return {"codes": ["P0133"], "raw": "43 01 33"}
-    return {}
+    return {"audio_test_delay_ms": 3000} if command == "test_audio" else {}
 
 
 def test_bluetooth_status_api(monkeypatch):
@@ -211,26 +203,6 @@ def test_bluetooth_api_dispatches_operations(monkeypatch):
   ]
 
 
-def test_bluetooth_api_dispatches_elm_payload_and_allows_close_onroad(monkeypatch):
-  FakeBluetoothClient.calls = []
-  client, fake_params = _params_client(monkeypatch, {"IsOffroad": True}, "mici")
-  monkeypatch.setattr(the_galaxy, "BluetoothClient", FakeBluetoothClient)
-  address = "00:11:22:33:44:55"
-
-  assert client.post("/api/bluetooth/elm_open", json={"address": address}).get_json()["adapter"] == "Fake ELM327"
-  assert client.post("/api/bluetooth/elm_command", json={"address": address, "value": "ATI"}).get_json()["response"] == "OK"
-  assert client.post("/api/bluetooth/elm_read_dtcs", json={"address": address}).get_json()["codes"] == ["P0133"]
-  fake_params.values["IsOffroad"] = False
-  assert client.post("/api/bluetooth/elm_close", json={"address": address}).status_code == 200
-
-  assert FakeBluetoothClient.calls == [
-    ("elm_open", {"address": address}),
-    ("elm_command", {"address": address, "value": "ATI"}),
-    ("elm_read_dtcs", {"address": address}),
-    ("elm_close", {"address": address}),
-  ]
-
-
 def test_wheel_controls_status_includes_favorite_slots(monkeypatch):
   client, _ = _params_client(monkeypatch, {"IsOffroad": True, "FavoriteSlots": []}, "mici")
   monkeypatch.setattr(the_galaxy, "wheel_control_status", lambda *_args: {"available": True, "mappings": [], "devices": []})

starpilot/system/the_galaxy/the_galaxy.py

@@ -4994,16 +4994,11 @@ def setup(app):
       "select_audio": "select_audio",
       "test_audio": "test_audio",
       "pairing_response": "pairing_response",
-      "elm_open": "elm_open",
-      "elm_close": "elm_close",
-      "elm_command": "elm_command",
-      "elm_read_dtcs": "elm_read_dtcs",
     }
     command = commands.get(operation)
     if command is None:
       return jsonify({"error": "Unknown Bluetooth operation."}), 404
-    offroad_only = {"power", "scan", "stop_scan", "pair", "forget", "test_audio", "pairing_response",
-                    "elm_open", "elm_command", "elm_read_dtcs"}
+    offroad_only = {"power", "scan", "stop_scan", "pair", "forget", "test_audio", "pairing_response"}
     if operation in offroad_only and not params.get_bool("IsOffroad"):
       return jsonify({"error": "Bluetooth settings can only be changed offroad."}), 409
 
@@ -5021,8 +5016,6 @@ def setup(app):
       payload["address"] = str(data.get("address", ""))
       if not payload["address"] and command != "select_audio":
         return jsonify({"error": "Bluetooth device address is required."}), 400
-      if command == "elm_command":
-        payload["value"] = str(data.get("value", ""))
     try:
       client = BluetoothClient(timeout=10.0)
       if command == "set_power":
@@ -6002,6 +5995,16 @@ 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
 
     for key in ("CalibratedLateralAcceleration", "CalibrationProgress"):
       try:

system/hardware/fan_controller.py

@@ -7,9 +7,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.pid import PIDController
 from openpilot.system.hardware import HARDWARE
 
-# raise fan setpoint on tici/tizi to reduce noise
-# after raising LMH threshold in AGNOS 18.1 to prevent CPU throttling
-OFFSET = 0 if HARDWARE.get_device_type() == "mici" else 5
+OFFSET = 0
 
 class BaseFanController(ABC):
   @abstractmethod

system/ui/lib/bluetooth_manager.py

@@ -97,32 +97,6 @@ class BluetoothManager:
               self._operations.pop(normalized_address, None)
     threading.Thread(target=worker, daemon=True).start()
 
-  def _run_result(self, fn, *args, operation: str = "", address: str = "", callback=None) -> None:
-    normalized_address = address.upper()
-    if normalized_address:
-      with self._lock:
-        self._operations[normalized_address] = operation
-
-    def worker():
-      result = None
-      error = None
-      try:
-        result = fn(*args)
-      except Exception as exception:
-        error = str(exception)
-      finally:
-        if normalized_address:
-          with self._lock:
-            if self._operations.get(normalized_address) == operation:
-              self._operations.pop(normalized_address, None)
-      if callback is not None:
-        callback(result, error)
-      elif error:
-        with self._lock:
-          self._operation_error = error
-
-    threading.Thread(target=worker, daemon=True).start()
-
   def set_power(self, enabled: bool) -> None:
     with self._lock:
       if self._power_pending:
@@ -172,17 +146,5 @@ class BluetoothManager:
           self._audio_test_deadline = 0.0
     threading.Thread(target=worker, daemon=True).start()
 
-  def elm_open(self, address: str, callback=None) -> None:
-    self._run_result(self._client.elm_open, address, operation="elm_open", address=address, callback=callback)
-
-  def elm_close(self, address: str, callback=None) -> None:
-    self._run_result(self._client.elm_close, address, operation="elm_close", address=address, callback=callback)
-
-  def elm_command(self, address: str, value: str, callback=None) -> None:
-    self._run_result(self._client.elm_command, address, value, operation="elm_command", address=address, callback=callback)
-
-  def elm_read_dtcs(self, address: str, callback=None) -> None:
-    self._run_result(self._client.elm_read_dtcs, address, operation="elm_read_dtcs", address=address, callback=callback)
-
   def respond(self, prompt_id: str, accepted: bool, value: str = "") -> None:
     self._run(self._client.respond, prompt_id, accepted, value)

system/ui/widgets/bluetooth.py

@@ -2,7 +2,6 @@ from __future__ import annotations
 
 from dataclasses import dataclass
 from functools import partial
-import threading
 
 import pyray as rl
 
@@ -15,7 +14,7 @@ from openpilot.system.ui.widgets import DialogResult, Widget
 from openpilot.system.ui.widgets.button import Button, ButtonStyle
 from openpilot.system.ui.widgets.confirm_dialog import ConfirmDialog, alert_dialog
 from openpilot.system.ui.widgets.keyboard import Keyboard
-from openpilot.system.ui.widgets.label import gui_label, gui_text_box
+from openpilot.system.ui.widgets.label import gui_label
 from openpilot.system.ui.widgets.option_dialog import MultiOptionDialog
 from openpilot.system.ui.widgets.toggle import Toggle
 
@@ -46,15 +45,11 @@ def device_status_text(device: BluetoothDevice, operation: str, selected_audio:
     capabilities.append(tr("audio output") if selected_audio.upper() == device.address.upper() else tr("audio"))
   if device.controller:
     capabilities.append(tr("controller"))
-  if device.serial:
-    capabilities.append(tr("serial"))
   capability_text = " / ".join(capabilities)
 
   if device.connected:
     return tr("Connected") + (f" / {capability_text}" if capability_text else "")
   if device.paired:
-    if device.serial:
-      return tr("Paired - tap to use ELM327") + (f" / {capability_text}" if capability_text else "")
     return tr("Paired - tap to connect")
   return tr("Tap to pair") + (f" / {capability_text}" if capability_text else "")
 
@@ -63,8 +58,6 @@ def device_action_allowed(device: BluetoothDevice, operation: str, offroad: bool
   """Mirror the daemon's operation policy before a row can receive a tap."""
   if operation:
     return False
-  if device.serial and not offroad:
-    return False
   if not offroad and not device.paired:
     return False
   return True
@@ -182,139 +175,6 @@ class BluetoothAudioTestDialog(Widget):
     self._done_button.render(button_rect)
 
 
-class ELM327Dialog(Widget):
-  """Ephemeral, offroad-only ELM327 controls opened from the Bluetooth panel."""
-  def __init__(self, manager: BluetoothManager, device: BluetoothDevice):
-    super().__init__()
-    self._manager = manager
-    self._address = device.address
-    self._name = device.name
-    self._state_lock = threading.Lock()
-    self._closed = False
-    self._connecting = True
-    self._adapter = ""
-    self._response = ""
-    self._codes: list[str] | None = None
-    self._error = ""
-    self._keyboard = Keyboard(max_text_size=256, min_text_size=1, password_mode=False)
-    self._read_button = Button(tr("Read Codes"), self._read_codes, button_style=ButtonStyle.PRIMARY, font_size=42)
-    self._command_button = Button(tr("Send Command"), self._send_command, button_style=ButtonStyle.NORMAL, font_size=42)
-    self._done_button = Button(tr("Done"), gui_app.pop_widget, button_style=ButtonStyle.NORMAL, font_size=42)
-
-  def show_event(self):
-    super().show_event()
-    self._manager.elm_open(self._address, callback=self._on_open_result)
-
-  def hide_event(self):
-    with self._state_lock:
-      self._closed = True
-    self._manager.elm_close(self._address)
-    super().hide_event()
-
-  def _on_open_result(self, result, error):
-    should_close = False
-    with self._state_lock:
-      self._connecting = False
-      if error:
-        self._error = error
-      else:
-        self._adapter = str((result or {}).get("adapter", ""))
-        self._response = ""
-        self._codes = None
-        should_close = self._closed
-    if should_close:
-      self._manager.elm_close(self._address)
-
-  def _on_command_result(self, command: str, result, error):
-    with self._state_lock:
-      if error:
-        self._error = error
-      else:
-        response = str((result or {}).get("response", ""))
-        self._response = f"{command}\n{response}".strip()
-
-  def _on_read_result(self, result, error):
-    with self._state_lock:
-      if error:
-        self._error = error
-      else:
-        self._codes = [str(code) for code in (result or {}).get("codes", [])]
-        self._response = str((result or {}).get("raw", ""))
-
-  def _send_command(self):
-    with self._state_lock:
-      if self._connecting or self._error or self._closed:
-        return
-    self._keyboard.reset(min_text_size=1)
-    self._keyboard.set_title(tr("Send ELM327 command"), tr("Raw commands are available offroad only."))
-    self._keyboard.set_callback(self._on_command_entered)
-    gui_app.push_widget(self._keyboard)
-
-  def _on_command_entered(self, result: DialogResult):
-    command = self._keyboard.text.strip()
-    self._keyboard.clear()
-    if result != DialogResult.CONFIRM or not command:
-      return
-    with self._state_lock:
-      self._response = f"{command}\nSending..."
-      self._error = ""
-    self._manager.elm_command(self._address, command, callback=partial(self._on_command_result, command))
-
-  def _read_codes(self):
-    with self._state_lock:
-      self._codes = None
-      self._error = ""
-    self._manager.elm_read_dtcs(self._address, callback=self._on_read_result)
-
-  def _update_state(self):
-    with self._state_lock:
-      ready = bool(self._adapter) and not self._connecting and not self._error and not self._closed
-    status = self._manager.status
-    operation = self._manager.operation_for(self._address)
-    enabled = ready and status.offroad and not operation
-    self._read_button.set_enabled(enabled)
-    self._command_button.set_enabled(enabled)
-    self._done_button.set_enabled(True)
-
-  def _render(self, rect: rl.Rectangle):
-    dialog_rect = rl.Rectangle(rect.x + 90, rect.y + 60, rect.width - 180, rect.height - 120)
-    rl.draw_rectangle_rounded(dialog_rect, 0.03, 20, DIALOG_BACKGROUND)
-
-    with self._state_lock:
-      connecting = self._connecting
-      adapter = self._adapter
-      response = self._response
-      codes = self._codes
-      error = self._error
-    gui_label(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 35, dialog_rect.width - 90, 70), tr("ELM327"),
-              font_size=62, font_weight=FontWeight.BOLD)
-    gui_label(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 105, dialog_rect.width - 90, 52), self._name,
-              font_size=42, color=TEXT_SECONDARY)
-    identity = tr("Connecting...") if connecting else adapter
-    gui_label(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 157, dialog_rect.width - 90, 52), identity,
-              font_size=42, color=TEXT_CONNECTED if adapter else TEXT_SECONDARY)
-
-    button_y = dialog_rect.y + 225
-    button_width = (dialog_rect.width - 135) / 3
-    self._read_button.render(rl.Rectangle(dialog_rect.x + 45, button_y, button_width, 90))
-    self._command_button.render(rl.Rectangle(dialog_rect.x + 60 + button_width, button_y, button_width, 90))
-    self._done_button.render(rl.Rectangle(dialog_rect.x + 75 + button_width * 2, button_y, button_width, 90))
-
-    if error:
-      gui_text_box(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 330, dialog_rect.width - 90, 80), error,
-                   font_size=38, color=rl.Color(255, 150, 150, 255))
-    if codes is not None:
-      gui_label(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 410, 300, 45), tr("Stored Codes"),
-                font_size=38, font_weight=FontWeight.BOLD)
-      code_text = "\n".join(codes) if codes else tr("No stored codes reported.")
-      gui_text_box(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 455, dialog_rect.width - 90, 95), code_text,
-                   font_size=38, color=TEXT_SECONDARY)
-    gui_label(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 560, 300, 45), tr("Response"),
-              font_size=38, font_weight=FontWeight.BOLD)
-    gui_text_box(rl.Rectangle(dialog_rect.x + 45, dialog_rect.y + 605, dialog_rect.width - 90, dialog_rect.height - 650),
-                 response or "—", font_size=36, color=TEXT_SECONDARY)
-
-
 class BluetoothManagerUI(Widget):
   """Big UI Bluetooth settings panel backed by the existing Bluetooth manager daemon."""
   def __init__(self, manager: BluetoothManager):
@@ -404,8 +264,6 @@ class BluetoothManagerUI(Widget):
       return
     if not device.paired:
       self._manager.pair(device.address)
-    elif device.serial:
-      gui_app.push_widget(ELM327Dialog(self._manager, device))
     elif not device.connected:
       self._manager.connect(device.address)
     else:

tools/longitudinal/analyze_csc.py

@@ -0,0 +1,295 @@
+#!/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()