test4

cereal/custom.capnp

@@ -220,6 +220,10 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
   disableThrottle @35 :Bool;
   trackingLead @36 :Bool;
   stopSignConfirmed @37 :Bool;
+  # Curve Speed Controller diagnostics, for tuning and rollout validation
+  cscOverridden @38 :Bool;          # driver cancelled this curve with RES+
+  cscLearnedLatAccel @39 :Float32;  # learned comfort at the current curvature, before margin
+  cscBindingDistance @40 :Float32;  # distance to the horizon point setting the target, m
 }
 
 struct StarPilotRadarState @0xb86e6369214c01c8 {

common/params_keys.h

@@ -241,6 +241,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}},
     {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}},
     {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
+    {"CurveSpeedApproachDecel", {PERSISTENT, FLOAT, "0.6", "0.6", 2, SETTINGS_SIMPLE}},
+    {"CurveSpeedMargin", {PERSISTENT, INT, "85", "85", 2, SETTINGS_SIMPLE}},
     {"CustomAlerts", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
     {"CustomAccelProfile", {PERSISTENT, BOOL, "0", "0", 3}},
     {"CustomAccelProfileInitialized", {PERSISTENT, BOOL, "0", "0", 3}},

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-526f8e00-DEBUG";
+const uint8_t gitversion[19] = "DEV-9b9878ed-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-526f8e00-DEBUG
+DEV-9b9878ed-DEBUG

selfdrive/car/card.py

@@ -24,7 +24,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import (
   VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN,
-  is_speed_limit_confirmation_pending,
+  is_csc_override_pending, is_speed_limit_confirmation_pending,
 )
 from openpilot.selfdrive.car.redneck_cruise import RedneckCruise, select_redneck_target_speed
 from openpilot.selfdrive.car.car_specific import MockCarState
@@ -277,6 +277,7 @@ class Car:
     )
     if not preap_software_cruise:
       speed_limit_confirmation_pending = is_speed_limit_confirmation_pending(self.sm['starpilotPlan'])
+      csc_override_pending = is_csc_override_pending(self.sm['starpilotPlan'])
       self.v_cruise_helper.update_v_cruise(
         CS,
         self.sm['carControl'].enabled,
@@ -284,6 +285,7 @@ class Car:
         speed_limit_confirmation_pending,
         self.starpilot_toggles,
         FPCS,
+        csc_active=csc_override_pending,
       )
     else:
       preap_v_cruise_kph = float(CS.cruiseState.speed * CV.MS_TO_KPH)

selfdrive/car/cruise.py

@@ -37,6 +37,11 @@ def is_speed_limit_confirmation_pending(starpilot_plan) -> bool:
   return bool(starpilot_plan.speedLimitChanged and starpilot_plan.unconfirmedSlcSpeedLimit >= 1)
 
 
+def is_csc_override_pending(starpilot_plan) -> bool:
+  """Accel presses cancel an active curve slowdown instead of raising the set speed."""
+  return bool(starpilot_plan.cscControllingSpeed) and not is_speed_limit_confirmation_pending(starpilot_plan)
+
+
 class VCruiseHelper:
   def __init__(self, CP, FPCP=None):
     self.CP = CP
@@ -89,13 +94,13 @@ class VCruiseHelper:
       return bool(getattr(starpilot_car_state, "decelHardCruise", False))
     return False
 
-  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None):
+  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None, csc_active=False):
     self.v_cruise_kph_last = self.v_cruise_kph
 
     if CS.cruiseState.available:
       if self.gm_cc_only or self.redneck_non_pcm or not self.CP.pcmCruise:
         # if stock cruise is completely disabled, then we can use our own set speed logic
-        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state)
+        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state, csc_active)
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled, starpilot_car_state)
       else:
@@ -111,7 +116,7 @@ class VCruiseHelper:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
 
-  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None):
+  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None, csc_active=False):
     # handle button presses. TODO: this should be in state_control, but a decelCruise press
     # would have the effect of both enabling and changing speed is checked after the state transition
     if not enabled:
@@ -126,7 +131,9 @@ class VCruiseHelper:
     for b in CS.buttonEvents:
       event_button_type = b.type.raw
       if event_button_type in self.button_timers:
-        if speed_limit_changed and b.pressed:
+        # decel presses keep their normal meaning
+        consume_press = speed_limit_changed or (csc_active and event_button_type in ACCEL_CRUISE_BUTTONS)
+        if consume_press and b.pressed:
           self.confirmation_button_suppressed.add(event_button_type)
         elif not b.pressed and event_button_type in self.confirmation_button_suppressed:
           self.confirmation_button_suppressed.remove(event_button_type)

selfdrive/car/tests/test_csc_cruise_button.py

@@ -0,0 +1,74 @@
+import pytest
+
+from types import SimpleNamespace
+
+from openpilot.selfdrive.car.cruise import ButtonType, VCruiseHelper, is_csc_override_pending
+
+
+def make_helper():
+  CP = SimpleNamespace(carFingerprint="MOCK", flags=0, pcmCruise=False, brand="mock")
+  helper = VCruiseHelper(CP)
+  helper.v_cruise_kph = 40.0
+  return helper
+
+
+def make_toggles():
+  return SimpleNamespace(cruise_increase=1.0, cruise_increase_long=5.0, reverse_cruise_increase=False)
+
+
+def make_cs(button_events):
+  return SimpleNamespace(
+    buttonEvents=button_events,
+    cruiseState=SimpleNamespace(available=True, standstill=False, speed=0, speedCluster=0),
+    gasPressed=False,
+    vEgo=20.0,
+  )
+
+
+def press(button):
+  return SimpleNamespace(type=SimpleNamespace(raw=button), pressed=True)
+
+
+def release(button):
+  return SimpleNamespace(type=SimpleNamespace(raw=button), pressed=False)
+
+
+def press_and_release(helper, button, csc_active):
+  toggles = make_toggles()
+  helper.update_v_cruise(make_cs([press(button)]), True, True, False, toggles, None, csc_active=csc_active)
+  helper.update_v_cruise(make_cs([release(button)]), True, True, False, toggles, None, csc_active=csc_active)
+
+
+def test_accel_press_consumed_while_csc_active():
+  helper = make_helper()
+
+  press_and_release(helper, ButtonType.accelCruise, csc_active=True)
+
+  assert helper.v_cruise_kph == pytest.approx(40.0)
+
+
+def test_accel_press_adjusts_set_speed_when_csc_inactive():
+  helper = make_helper()
+
+  press_and_release(helper, ButtonType.accelCruise, csc_active=False)
+
+  assert helper.v_cruise_kph > 40.0
+
+
+def test_decel_press_still_works_while_csc_active():
+  helper = make_helper()
+
+  press_and_release(helper, ButtonType.decelCruise, csc_active=True)
+
+  assert helper.v_cruise_kph < 40.0
+
+
+def test_csc_override_pending_defers_to_slc_confirmation():
+  active_plan = SimpleNamespace(cscControllingSpeed=True, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0)
+  assert is_csc_override_pending(active_plan)
+
+  idle_plan = SimpleNamespace(cscControllingSpeed=False, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0)
+  assert not is_csc_override_pending(idle_plan)
+
+  slc_pending_plan = SimpleNamespace(cscControllingSpeed=True, speedLimitChanged=True, unconfirmedSlcSpeedLimit=25)
+  assert not is_csc_override_pending(slc_pending_plan)

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -1,6 +1,8 @@
 from pathlib import Path
 from types import SimpleNamespace
 
+import numpy as np
+
 from openpilot.common.constants import CV
 from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner
 import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module
@@ -1168,6 +1170,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: (np.zeros(33), np.zeros(33)))
     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,466 @@
+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_LAT_ACCEL_MAX,
+  CSC_MIN_SPEED,
+  CSC_NUDGE_WEIGHT,
+  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_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 > 15.0
+
+  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(0.1) == pytest.approx(2.9, abs=0.05)
+  assert controller.lateral_acceleration == pytest.approx(DEFAULT_LATERAL_ACCELERATION)
+
+
+def test_comfort_margin_aims_below_the_learned_habit():
+  _, controller = make_controller()
+
+  assert controller.lat_accel_for_curvature(0.01) == pytest.approx(
+    controller.learned_lat_accel(0.01) * CSC_COMFORT_MARGIN)
+  assert controller.lat_accel_for_curvature(0.01) < controller.learned_lat_accel(0.01)
+
+
+def test_margin_slider_overrides_the_default():
+  _, controller = make_controller()
+  controller.starpilot_toggles = SimpleNamespace(csc_margin=0.7)
+
+  assert controller.comfort_margin == pytest.approx(0.7)
+  assert controller.lat_accel_for_curvature(0.01) == pytest.approx(
+    controller.learned_lat_accel(0.01) * 0.7)
+
+
+def test_margin_falls_back_to_default_without_toggles():
+  _, controller = make_controller()
+
+  # toggles are only attached once vcruise runs
+  assert controller.comfort_margin == pytest.approx(CSC_COMFORT_MARGIN)
+  controller.starpilot_toggles = SimpleNamespace(csc_margin=0.0)
+  assert controller.comfort_margin == pytest.approx(CSC_COMFORT_MARGIN)
+
+
+def test_lower_margin_engages_on_gentler_curves():
+  planner, controller = make_controller(curve_profile=single_apex_profile(0.004, 0.0))
+  controller.starpilot_toggles = SimpleNamespace(csc_margin=1.0)
+  relaxed = converge(controller, 30.0, 30.0)
+
+  planner2, aggressive_controller = make_controller(curve_profile=single_apex_profile(0.004, 0.0))
+  aggressive_controller.starpilot_toggles = SimpleNamespace(csc_margin=0.7)
+  aggressive = converge(aggressive_controller, 30.0, 30.0)
+
+  assert aggressive < relaxed
+
+
+def test_approach_decel_slider_moves_where_braking_starts():
+  # the same curve must bind further out when the approach is planned gentler
+  def bind_distance(decel):
+    planner, controller = make_controller(curve_profile=single_apex_profile(0.01, 200.0))
+    controller.starpilot_toggles = SimpleNamespace(csc_approach_decel=decel)
+    converge(controller, 30.0, 30.0)
+    return controller.target
+
+  assert bind_distance(0.3) < bind_distance(1.5)
+
+
+def test_approach_decel_falls_back_to_default():
+  _, controller = make_controller()
+
+  assert controller.approach_decel == pytest.approx(CSC_APPROACH_DECEL)
+  controller.starpilot_toggles = SimpleNamespace(csc_approach_decel=0.0)
+  assert controller.approach_decel == pytest.approx(CSC_APPROACH_DECEL)
+
+
+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
+  _, controller = make_controller(curvature_data={
+    "0.003": {"average": 1.95, "count": 20},
+    "0.005": {"average": 1.38, "count": 80},
+  })
+
+  assert controller.learned_lat_accel(0.005) < 1.82
+  assert controller.learned_lat_accel(0.005) >= controller.learned_lat_accel(0.003)
+
+
+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 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))
+  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
+  assert controller.curvature_data["0.02"]["average"] > prior
+
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
+
+  controller.handle_override(20.0, False, make_sm())
+  controller.target = 10.0
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  assert controller.curvature_data["0.02"]["count"] == 2 * CSC_NUDGE_WEIGHT
+
+
+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)
+
+  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

@@ -3,9 +3,7 @@ import datetime
 import pytest
 
 from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME
-from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController
 from openpilot.starpilot.controls.lib.starpilot_vcruise import (
   FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
   StarPilotVCruise,
@@ -77,12 +75,12 @@ def make_sm(*, standstill=True, min_steer_speed=0.0):
   }
 
 
-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,
@@ -130,30 +128,56 @@ def test_camry_tss2_gets_forward_force_stop_bias_only():
   assert get_force_stop_distance_bias("TOYOTA_RAV4_TSS2") == pytest.approx(0.0)
 
 
-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():
@@ -162,16 +186,13 @@ 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)
@@ -185,12 +206,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)
@@ -208,10 +227,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
@@ -261,39 +278,150 @@ 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_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_csc_res_press_cancels_for_episode_and_rearms():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
 
-  controller.update_target(30.0)
+  curve_target = {"v": 14.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
+  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_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_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
 
-  controller.update_target(30.0)
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 14.0
 
-  assert controller.target == pytest.approx(30.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_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
+
+  # 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
+
+  curve_target["v"] = 20.0
+  update_vcruise(vcruise, sm, toggles, now=100.1, 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
 
 
 def test_active_slc_control_target_applies_offset_and_cluster_diff():

selfdrive/locationd/models/generated/car.cpp

@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
  *                                                                            *
  *                         This file is part of 'ekf'                         *
  ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_4175267327343220067) {
-   out_4175267327343220067[0] = delta_x[0] + nom_x[0];
-   out_4175267327343220067[1] = delta_x[1] + nom_x[1];
-   out_4175267327343220067[2] = delta_x[2] + nom_x[2];
-   out_4175267327343220067[3] = delta_x[3] + nom_x[3];
-   out_4175267327343220067[4] = delta_x[4] + nom_x[4];
-   out_4175267327343220067[5] = delta_x[5] + nom_x[5];
-   out_4175267327343220067[6] = delta_x[6] + nom_x[6];
-   out_4175267327343220067[7] = delta_x[7] + nom_x[7];
-   out_4175267327343220067[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_3258091067705697569) {
+   out_3258091067705697569[0] = delta_x[0] + nom_x[0];
+   out_3258091067705697569[1] = delta_x[1] + nom_x[1];
+   out_3258091067705697569[2] = delta_x[2] + nom_x[2];
+   out_3258091067705697569[3] = delta_x[3] + nom_x[3];
+   out_3258091067705697569[4] = delta_x[4] + nom_x[4];
+   out_3258091067705697569[5] = delta_x[5] + nom_x[5];
+   out_3258091067705697569[6] = delta_x[6] + nom_x[6];
+   out_3258091067705697569[7] = delta_x[7] + nom_x[7];
+   out_3258091067705697569[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_8001717605441520298) {
-   out_8001717605441520298[0] = -nom_x[0] + true_x[0];
-   out_8001717605441520298[1] = -nom_x[1] + true_x[1];
-   out_8001717605441520298[2] = -nom_x[2] + true_x[2];
-   out_8001717605441520298[3] = -nom_x[3] + true_x[3];
-   out_8001717605441520298[4] = -nom_x[4] + true_x[4];
-   out_8001717605441520298[5] = -nom_x[5] + true_x[5];
-   out_8001717605441520298[6] = -nom_x[6] + true_x[6];
-   out_8001717605441520298[7] = -nom_x[7] + true_x[7];
-   out_8001717605441520298[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_2755855052909692674) {
+   out_2755855052909692674[0] = -nom_x[0] + true_x[0];
+   out_2755855052909692674[1] = -nom_x[1] + true_x[1];
+   out_2755855052909692674[2] = -nom_x[2] + true_x[2];
+   out_2755855052909692674[3] = -nom_x[3] + true_x[3];
+   out_2755855052909692674[4] = -nom_x[4] + true_x[4];
+   out_2755855052909692674[5] = -nom_x[5] + true_x[5];
+   out_2755855052909692674[6] = -nom_x[6] + true_x[6];
+   out_2755855052909692674[7] = -nom_x[7] + true_x[7];
+   out_2755855052909692674[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_7971983971871194008) {
-   out_7971983971871194008[0] = 1.0;
-   out_7971983971871194008[1] = 0.0;
-   out_7971983971871194008[2] = 0.0;
-   out_7971983971871194008[3] = 0.0;
-   out_7971983971871194008[4] = 0.0;
-   out_7971983971871194008[5] = 0.0;
-   out_7971983971871194008[6] = 0.0;
-   out_7971983971871194008[7] = 0.0;
-   out_7971983971871194008[8] = 0.0;
-   out_7971983971871194008[9] = 0.0;
-   out_7971983971871194008[10] = 1.0;
-   out_7971983971871194008[11] = 0.0;
-   out_7971983971871194008[12] = 0.0;
-   out_7971983971871194008[13] = 0.0;
-   out_7971983971871194008[14] = 0.0;
-   out_7971983971871194008[15] = 0.0;
-   out_7971983971871194008[16] = 0.0;
-   out_7971983971871194008[17] = 0.0;
-   out_7971983971871194008[18] = 0.0;
-   out_7971983971871194008[19] = 0.0;
-   out_7971983971871194008[20] = 1.0;
-   out_7971983971871194008[21] = 0.0;
-   out_7971983971871194008[22] = 0.0;
-   out_7971983971871194008[23] = 0.0;
-   out_7971983971871194008[24] = 0.0;
-   out_7971983971871194008[25] = 0.0;
-   out_7971983971871194008[26] = 0.0;
-   out_7971983971871194008[27] = 0.0;
-   out_7971983971871194008[28] = 0.0;
-   out_7971983971871194008[29] = 0.0;
-   out_7971983971871194008[30] = 1.0;
-   out_7971983971871194008[31] = 0.0;
-   out_7971983971871194008[32] = 0.0;
-   out_7971983971871194008[33] = 0.0;
-   out_7971983971871194008[34] = 0.0;
-   out_7971983971871194008[35] = 0.0;
-   out_7971983971871194008[36] = 0.0;
-   out_7971983971871194008[37] = 0.0;
-   out_7971983971871194008[38] = 0.0;
-   out_7971983971871194008[39] = 0.0;
-   out_7971983971871194008[40] = 1.0;
-   out_7971983971871194008[41] = 0.0;
-   out_7971983971871194008[42] = 0.0;
-   out_7971983971871194008[43] = 0.0;
-   out_7971983971871194008[44] = 0.0;
-   out_7971983971871194008[45] = 0.0;
-   out_7971983971871194008[46] = 0.0;
-   out_7971983971871194008[47] = 0.0;
-   out_7971983971871194008[48] = 0.0;
-   out_7971983971871194008[49] = 0.0;
-   out_7971983971871194008[50] = 1.0;
-   out_7971983971871194008[51] = 0.0;
-   out_7971983971871194008[52] = 0.0;
-   out_7971983971871194008[53] = 0.0;
-   out_7971983971871194008[54] = 0.0;
-   out_7971983971871194008[55] = 0.0;
-   out_7971983971871194008[56] = 0.0;
-   out_7971983971871194008[57] = 0.0;
-   out_7971983971871194008[58] = 0.0;
-   out_7971983971871194008[59] = 0.0;
-   out_7971983971871194008[60] = 1.0;
-   out_7971983971871194008[61] = 0.0;
-   out_7971983971871194008[62] = 0.0;
-   out_7971983971871194008[63] = 0.0;
-   out_7971983971871194008[64] = 0.0;
-   out_7971983971871194008[65] = 0.0;
-   out_7971983971871194008[66] = 0.0;
-   out_7971983971871194008[67] = 0.0;
-   out_7971983971871194008[68] = 0.0;
-   out_7971983971871194008[69] = 0.0;
-   out_7971983971871194008[70] = 1.0;
-   out_7971983971871194008[71] = 0.0;
-   out_7971983971871194008[72] = 0.0;
-   out_7971983971871194008[73] = 0.0;
-   out_7971983971871194008[74] = 0.0;
-   out_7971983971871194008[75] = 0.0;
-   out_7971983971871194008[76] = 0.0;
-   out_7971983971871194008[77] = 0.0;
-   out_7971983971871194008[78] = 0.0;
-   out_7971983971871194008[79] = 0.0;
-   out_7971983971871194008[80] = 1.0;
+void H_mod_fun(double *state, double *out_7781028056435054793) {
+   out_7781028056435054793[0] = 1.0;
+   out_7781028056435054793[1] = 0.0;
+   out_7781028056435054793[2] = 0.0;
+   out_7781028056435054793[3] = 0.0;
+   out_7781028056435054793[4] = 0.0;
+   out_7781028056435054793[5] = 0.0;
+   out_7781028056435054793[6] = 0.0;
+   out_7781028056435054793[7] = 0.0;
+   out_7781028056435054793[8] = 0.0;
+   out_7781028056435054793[9] = 0.0;
+   out_7781028056435054793[10] = 1.0;
+   out_7781028056435054793[11] = 0.0;
+   out_7781028056435054793[12] = 0.0;
+   out_7781028056435054793[13] = 0.0;
+   out_7781028056435054793[14] = 0.0;
+   out_7781028056435054793[15] = 0.0;
+   out_7781028056435054793[16] = 0.0;
+   out_7781028056435054793[17] = 0.0;
+   out_7781028056435054793[18] = 0.0;
+   out_7781028056435054793[19] = 0.0;
+   out_7781028056435054793[20] = 1.0;
+   out_7781028056435054793[21] = 0.0;
+   out_7781028056435054793[22] = 0.0;
+   out_7781028056435054793[23] = 0.0;
+   out_7781028056435054793[24] = 0.0;
+   out_7781028056435054793[25] = 0.0;
+   out_7781028056435054793[26] = 0.0;
+   out_7781028056435054793[27] = 0.0;
+   out_7781028056435054793[28] = 0.0;
+   out_7781028056435054793[29] = 0.0;
+   out_7781028056435054793[30] = 1.0;
+   out_7781028056435054793[31] = 0.0;
+   out_7781028056435054793[32] = 0.0;
+   out_7781028056435054793[33] = 0.0;
+   out_7781028056435054793[34] = 0.0;
+   out_7781028056435054793[35] = 0.0;
+   out_7781028056435054793[36] = 0.0;
+   out_7781028056435054793[37] = 0.0;
+   out_7781028056435054793[38] = 0.0;
+   out_7781028056435054793[39] = 0.0;
+   out_7781028056435054793[40] = 1.0;
+   out_7781028056435054793[41] = 0.0;
+   out_7781028056435054793[42] = 0.0;
+   out_7781028056435054793[43] = 0.0;
+   out_7781028056435054793[44] = 0.0;
+   out_7781028056435054793[45] = 0.0;
+   out_7781028056435054793[46] = 0.0;
+   out_7781028056435054793[47] = 0.0;
+   out_7781028056435054793[48] = 0.0;
+   out_7781028056435054793[49] = 0.0;
+   out_7781028056435054793[50] = 1.0;
+   out_7781028056435054793[51] = 0.0;
+   out_7781028056435054793[52] = 0.0;
+   out_7781028056435054793[53] = 0.0;
+   out_7781028056435054793[54] = 0.0;
+   out_7781028056435054793[55] = 0.0;
+   out_7781028056435054793[56] = 0.0;
+   out_7781028056435054793[57] = 0.0;
+   out_7781028056435054793[58] = 0.0;
+   out_7781028056435054793[59] = 0.0;
+   out_7781028056435054793[60] = 1.0;
+   out_7781028056435054793[61] = 0.0;
+   out_7781028056435054793[62] = 0.0;
+   out_7781028056435054793[63] = 0.0;
+   out_7781028056435054793[64] = 0.0;
+   out_7781028056435054793[65] = 0.0;
+   out_7781028056435054793[66] = 0.0;
+   out_7781028056435054793[67] = 0.0;
+   out_7781028056435054793[68] = 0.0;
+   out_7781028056435054793[69] = 0.0;
+   out_7781028056435054793[70] = 1.0;
+   out_7781028056435054793[71] = 0.0;
+   out_7781028056435054793[72] = 0.0;
+   out_7781028056435054793[73] = 0.0;
+   out_7781028056435054793[74] = 0.0;
+   out_7781028056435054793[75] = 0.0;
+   out_7781028056435054793[76] = 0.0;
+   out_7781028056435054793[77] = 0.0;
+   out_7781028056435054793[78] = 0.0;
+   out_7781028056435054793[79] = 0.0;
+   out_7781028056435054793[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_1960663178760025601) {
-   out_1960663178760025601[0] = state[0];
-   out_1960663178760025601[1] = state[1];
-   out_1960663178760025601[2] = state[2];
-   out_1960663178760025601[3] = state[3];
-   out_1960663178760025601[4] = state[4];
-   out_1960663178760025601[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_1960663178760025601[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_1960663178760025601[7] = state[7];
-   out_1960663178760025601[8] = state[8];
+void f_fun(double *state, double dt, double *out_2481579766534270415) {
+   out_2481579766534270415[0] = state[0];
+   out_2481579766534270415[1] = state[1];
+   out_2481579766534270415[2] = state[2];
+   out_2481579766534270415[3] = state[3];
+   out_2481579766534270415[4] = state[4];
+   out_2481579766534270415[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_2481579766534270415[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_2481579766534270415[7] = state[7];
+   out_2481579766534270415[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_893498683805935900) {
-   out_893498683805935900[0] = 1;
-   out_893498683805935900[1] = 0;
-   out_893498683805935900[2] = 0;
-   out_893498683805935900[3] = 0;
-   out_893498683805935900[4] = 0;
-   out_893498683805935900[5] = 0;
-   out_893498683805935900[6] = 0;
-   out_893498683805935900[7] = 0;
-   out_893498683805935900[8] = 0;
-   out_893498683805935900[9] = 0;
-   out_893498683805935900[10] = 1;
-   out_893498683805935900[11] = 0;
-   out_893498683805935900[12] = 0;
-   out_893498683805935900[13] = 0;
-   out_893498683805935900[14] = 0;
-   out_893498683805935900[15] = 0;
-   out_893498683805935900[16] = 0;
-   out_893498683805935900[17] = 0;
-   out_893498683805935900[18] = 0;
-   out_893498683805935900[19] = 0;
-   out_893498683805935900[20] = 1;
-   out_893498683805935900[21] = 0;
-   out_893498683805935900[22] = 0;
-   out_893498683805935900[23] = 0;
-   out_893498683805935900[24] = 0;
-   out_893498683805935900[25] = 0;
-   out_893498683805935900[26] = 0;
-   out_893498683805935900[27] = 0;
-   out_893498683805935900[28] = 0;
-   out_893498683805935900[29] = 0;
-   out_893498683805935900[30] = 1;
-   out_893498683805935900[31] = 0;
-   out_893498683805935900[32] = 0;
-   out_893498683805935900[33] = 0;
-   out_893498683805935900[34] = 0;
-   out_893498683805935900[35] = 0;
-   out_893498683805935900[36] = 0;
-   out_893498683805935900[37] = 0;
-   out_893498683805935900[38] = 0;
-   out_893498683805935900[39] = 0;
-   out_893498683805935900[40] = 1;
-   out_893498683805935900[41] = 0;
-   out_893498683805935900[42] = 0;
-   out_893498683805935900[43] = 0;
-   out_893498683805935900[44] = 0;
-   out_893498683805935900[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_893498683805935900[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_893498683805935900[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_893498683805935900[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_893498683805935900[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_893498683805935900[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_893498683805935900[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_893498683805935900[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_893498683805935900[53] = -9.8100000000000005*dt;
-   out_893498683805935900[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_893498683805935900[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_893498683805935900[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_893498683805935900[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_893498683805935900[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_893498683805935900[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_893498683805935900[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_893498683805935900[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_893498683805935900[62] = 0;
-   out_893498683805935900[63] = 0;
-   out_893498683805935900[64] = 0;
-   out_893498683805935900[65] = 0;
-   out_893498683805935900[66] = 0;
-   out_893498683805935900[67] = 0;
-   out_893498683805935900[68] = 0;
-   out_893498683805935900[69] = 0;
-   out_893498683805935900[70] = 1;
-   out_893498683805935900[71] = 0;
-   out_893498683805935900[72] = 0;
-   out_893498683805935900[73] = 0;
-   out_893498683805935900[74] = 0;
-   out_893498683805935900[75] = 0;
-   out_893498683805935900[76] = 0;
-   out_893498683805935900[77] = 0;
-   out_893498683805935900[78] = 0;
-   out_893498683805935900[79] = 0;
-   out_893498683805935900[80] = 1;
+void F_fun(double *state, double dt, double *out_3144021140418811034) {
+   out_3144021140418811034[0] = 1;
+   out_3144021140418811034[1] = 0;
+   out_3144021140418811034[2] = 0;
+   out_3144021140418811034[3] = 0;
+   out_3144021140418811034[4] = 0;
+   out_3144021140418811034[5] = 0;
+   out_3144021140418811034[6] = 0;
+   out_3144021140418811034[7] = 0;
+   out_3144021140418811034[8] = 0;
+   out_3144021140418811034[9] = 0;
+   out_3144021140418811034[10] = 1;
+   out_3144021140418811034[11] = 0;
+   out_3144021140418811034[12] = 0;
+   out_3144021140418811034[13] = 0;
+   out_3144021140418811034[14] = 0;
+   out_3144021140418811034[15] = 0;
+   out_3144021140418811034[16] = 0;
+   out_3144021140418811034[17] = 0;
+   out_3144021140418811034[18] = 0;
+   out_3144021140418811034[19] = 0;
+   out_3144021140418811034[20] = 1;
+   out_3144021140418811034[21] = 0;
+   out_3144021140418811034[22] = 0;
+   out_3144021140418811034[23] = 0;
+   out_3144021140418811034[24] = 0;
+   out_3144021140418811034[25] = 0;
+   out_3144021140418811034[26] = 0;
+   out_3144021140418811034[27] = 0;
+   out_3144021140418811034[28] = 0;
+   out_3144021140418811034[29] = 0;
+   out_3144021140418811034[30] = 1;
+   out_3144021140418811034[31] = 0;
+   out_3144021140418811034[32] = 0;
+   out_3144021140418811034[33] = 0;
+   out_3144021140418811034[34] = 0;
+   out_3144021140418811034[35] = 0;
+   out_3144021140418811034[36] = 0;
+   out_3144021140418811034[37] = 0;
+   out_3144021140418811034[38] = 0;
+   out_3144021140418811034[39] = 0;
+   out_3144021140418811034[40] = 1;
+   out_3144021140418811034[41] = 0;
+   out_3144021140418811034[42] = 0;
+   out_3144021140418811034[43] = 0;
+   out_3144021140418811034[44] = 0;
+   out_3144021140418811034[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_3144021140418811034[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_3144021140418811034[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3144021140418811034[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3144021140418811034[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_3144021140418811034[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_3144021140418811034[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_3144021140418811034[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_3144021140418811034[53] = -9.8100000000000005*dt;
+   out_3144021140418811034[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_3144021140418811034[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_3144021140418811034[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3144021140418811034[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3144021140418811034[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_3144021140418811034[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_3144021140418811034[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_3144021140418811034[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_3144021140418811034[62] = 0;
+   out_3144021140418811034[63] = 0;
+   out_3144021140418811034[64] = 0;
+   out_3144021140418811034[65] = 0;
+   out_3144021140418811034[66] = 0;
+   out_3144021140418811034[67] = 0;
+   out_3144021140418811034[68] = 0;
+   out_3144021140418811034[69] = 0;
+   out_3144021140418811034[70] = 1;
+   out_3144021140418811034[71] = 0;
+   out_3144021140418811034[72] = 0;
+   out_3144021140418811034[73] = 0;
+   out_3144021140418811034[74] = 0;
+   out_3144021140418811034[75] = 0;
+   out_3144021140418811034[76] = 0;
+   out_3144021140418811034[77] = 0;
+   out_3144021140418811034[78] = 0;
+   out_3144021140418811034[79] = 0;
+   out_3144021140418811034[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_3235092958161781685) {
-   out_3235092958161781685[0] = state[6];
+void h_25(double *state, double *unused, double *out_6782490269790769427) {
+   out_6782490269790769427[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_4449003321881702331) {
-   out_4449003321881702331[0] = 0;
-   out_4449003321881702331[1] = 0;
-   out_4449003321881702331[2] = 0;
-   out_4449003321881702331[3] = 0;
-   out_4449003321881702331[4] = 0;
-   out_4449003321881702331[5] = 0;
-   out_4449003321881702331[6] = 1;
-   out_4449003321881702331[7] = 0;
-   out_4449003321881702331[8] = 0;
+void H_25(double *state, double *unused, double *out_4168693057384466872) {
+   out_4168693057384466872[0] = 0;
+   out_4168693057384466872[1] = 0;
+   out_4168693057384466872[2] = 0;
+   out_4168693057384466872[3] = 0;
+   out_4168693057384466872[4] = 0;
+   out_4168693057384466872[5] = 0;
+   out_4168693057384466872[6] = 1;
+   out_4168693057384466872[7] = 0;
+   out_4168693057384466872[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_5426063249126145013) {
-   out_5426063249126145013[0] = state[4];
-   out_5426063249126145013[1] = state[5];
+void h_24(double *state, double *unused, double *out_8556983568758949116) {
+   out_8556983568758949116[0] = state[4];
+   out_8556983568758949116[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_4845648626235561826) {
-   out_4845648626235561826[0] = 0;
-   out_4845648626235561826[1] = 0;
-   out_4845648626235561826[2] = 0;
-   out_4845648626235561826[3] = 0;
-   out_4845648626235561826[4] = 1;
-   out_4845648626235561826[5] = 0;
-   out_4845648626235561826[6] = 0;
-   out_4845648626235561826[7] = 0;
-   out_4845648626235561826[8] = 0;
-   out_4845648626235561826[9] = 0;
-   out_4845648626235561826[10] = 0;
-   out_4845648626235561826[11] = 0;
-   out_4845648626235561826[12] = 0;
-   out_4845648626235561826[13] = 0;
-   out_4845648626235561826[14] = 1;
-   out_4845648626235561826[15] = 0;
-   out_4845648626235561826[16] = 0;
-   out_4845648626235561826[17] = 0;
+void H_24(double *state, double *unused, double *out_9004556733412072150) {
+   out_9004556733412072150[0] = 0;
+   out_9004556733412072150[1] = 0;
+   out_9004556733412072150[2] = 0;
+   out_9004556733412072150[3] = 0;
+   out_9004556733412072150[4] = 1;
+   out_9004556733412072150[5] = 0;
+   out_9004556733412072150[6] = 0;
+   out_9004556733412072150[7] = 0;
+   out_9004556733412072150[8] = 0;
+   out_9004556733412072150[9] = 0;
+   out_9004556733412072150[10] = 0;
+   out_9004556733412072150[11] = 0;
+   out_9004556733412072150[12] = 0;
+   out_9004556733412072150[13] = 0;
+   out_9004556733412072150[14] = 1;
+   out_9004556733412072150[15] = 0;
+   out_9004556733412072150[16] = 0;
+   out_9004556733412072150[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_9056203919707109802) {
-   out_9056203919707109802[0] = state[4];
+void h_30(double *state, double *unused, double *out_3145040741536747077) {
+   out_3145040741536747077[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_1930670363374453704) {
-   out_1930670363374453704[0] = 0;
-   out_1930670363374453704[1] = 0;
-   out_1930670363374453704[2] = 0;
-   out_1930670363374453704[3] = 0;
-   out_1930670363374453704[4] = 1;
-   out_1930670363374453704[5] = 0;
-   out_1930670363374453704[6] = 0;
-   out_1930670363374453704[7] = 0;
-   out_1930670363374453704[8] = 0;
+void H_30(double *state, double *unused, double *out_359003272743141326) {
+   out_359003272743141326[0] = 0;
+   out_359003272743141326[1] = 0;
+   out_359003272743141326[2] = 0;
+   out_359003272743141326[3] = 0;
+   out_359003272743141326[4] = 1;
+   out_359003272743141326[5] = 0;
+   out_359003272743141326[6] = 0;
+   out_359003272743141326[7] = 0;
+   out_359003272743141326[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_6243651816437256419) {
-   out_6243651816437256419[0] = state[7];
+void h_26(double *state, double *unused, double *out_4127124834872045189) {
+   out_4127124834872045189[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_8190506640755758555) {
-   out_8190506640755758555[0] = 0;
-   out_8190506640755758555[1] = 0;
-   out_8190506640755758555[2] = 0;
-   out_8190506640755758555[3] = 0;
-   out_8190506640755758555[4] = 0;
-   out_8190506640755758555[5] = 0;
-   out_8190506640755758555[6] = 0;
-   out_8190506640755758555[7] = 1;
-   out_8190506640755758555[8] = 0;
+void H_26(double *state, double *unused, double *out_427189738510410648) {
+   out_427189738510410648[0] = 0;
+   out_427189738510410648[1] = 0;
+   out_427189738510410648[2] = 0;
+   out_427189738510410648[3] = 0;
+   out_427189738510410648[4] = 0;
+   out_427189738510410648[5] = 0;
+   out_427189738510410648[6] = 0;
+   out_427189738510410648[7] = 1;
+   out_427189738510410648[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_3838408842838259202) {
-   out_3838408842838259202[0] = state[3];
+void h_27(double *state, double *unused, double *out_1780817002129923782) {
+   out_1780817002129923782[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_292923707809489513) {
-   out_292923707809489513[0] = 0;
-   out_292923707809489513[1] = 0;
-   out_292923707809489513[2] = 0;
-   out_292923707809489513[3] = 1;
-   out_292923707809489513[4] = 0;
-   out_292923707809489513[5] = 0;
-   out_292923707809489513[6] = 0;
-   out_292923707809489513[7] = 0;
-   out_292923707809489513[8] = 0;
+void H_27(double *state, double *unused, double *out_2533766584543566237) {
+   out_2533766584543566237[0] = 0;
+   out_2533766584543566237[1] = 0;
+   out_2533766584543566237[2] = 0;
+   out_2533766584543566237[3] = 1;
+   out_2533766584543566237[4] = 0;
+   out_2533766584543566237[5] = 0;
+   out_2533766584543566237[6] = 0;
+   out_2533766584543566237[7] = 0;
+   out_2533766584543566237[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_259894455441112662) {
-   out_259894455441112662[0] = state[1];
+void h_29(double *state, double *unused, double *out_8902661814758955393) {
+   out_8902661814758955393[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_1420439019060061520) {
-   out_1420439019060061520[0] = 0;
-   out_1420439019060061520[1] = 1;
-   out_1420439019060061520[2] = 0;
-   out_1420439019060061520[3] = 0;
-   out_1420439019060061520[4] = 0;
-   out_1420439019060061520[5] = 0;
-   out_1420439019060061520[6] = 0;
-   out_1420439019060061520[7] = 0;
-   out_1420439019060061520[8] = 0;
+void H_29(double *state, double *unused, double *out_151228071571250858) {
+   out_151228071571250858[0] = 0;
+   out_151228071571250858[1] = 1;
+   out_151228071571250858[2] = 0;
+   out_151228071571250858[3] = 0;
+   out_151228071571250858[4] = 0;
+   out_151228071571250858[5] = 0;
+   out_151228071571250858[6] = 0;
+   out_151228071571250858[7] = 0;
+   out_151228071571250858[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_5143560329168581563) {
-   out_5143560329168581563[0] = state[0];
+void h_28(double *state, double *unused, double *out_7683957899572787069) {
+   out_7683957899572787069[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_6502838036129592094) {
-   out_6502838036129592094[0] = 1;
-   out_6502838036129592094[1] = 0;
-   out_6502838036129592094[2] = 0;
-   out_6502838036129592094[3] = 0;
-   out_6502838036129592094[4] = 0;
-   out_6502838036129592094[5] = 0;
-   out_6502838036129592094[6] = 0;
-   out_6502838036129592094[7] = 0;
-   out_6502838036129592094[8] = 0;
+void H_28(double *state, double *unused, double *out_4931170945498279716) {
+   out_4931170945498279716[0] = 1;
+   out_4931170945498279716[1] = 0;
+   out_4931170945498279716[2] = 0;
+   out_4931170945498279716[3] = 0;
+   out_4931170945498279716[4] = 0;
+   out_4931170945498279716[5] = 0;
+   out_4931170945498279716[6] = 0;
+   out_4931170945498279716[7] = 0;
+   out_4931170945498279716[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_2959898895877275796) {
-   out_2959898895877275796[0] = state[8];
+void h_31(double *state, double *unused, double *out_678068494458118898) {
+   out_678068494458118898[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_4418357360004741903) {
-   out_4418357360004741903[0] = 0;
-   out_4418357360004741903[1] = 0;
-   out_4418357360004741903[2] = 0;
-   out_4418357360004741903[3] = 0;
-   out_4418357360004741903[4] = 0;
-   out_4418357360004741903[5] = 0;
-   out_4418357360004741903[6] = 0;
-   out_4418357360004741903[7] = 0;
-   out_4418357360004741903[8] = 1;
+void H_31(double *state, double *unused, double *out_199018363722940828) {
+   out_199018363722940828[0] = 0;
+   out_199018363722940828[1] = 0;
+   out_199018363722940828[2] = 0;
+   out_199018363722940828[3] = 0;
+   out_199018363722940828[4] = 0;
+   out_199018363722940828[5] = 0;
+   out_199018363722940828[6] = 0;
+   out_199018363722940828[7] = 0;
+   out_199018363722940828[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
   update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_4175267327343220067) {
-  err_fun(nom_x, delta_x, out_4175267327343220067);
+void car_err_fun(double *nom_x, double *delta_x, double *out_3258091067705697569) {
+  err_fun(nom_x, delta_x, out_3258091067705697569);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_8001717605441520298) {
-  inv_err_fun(nom_x, true_x, out_8001717605441520298);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_2755855052909692674) {
+  inv_err_fun(nom_x, true_x, out_2755855052909692674);
 }
-void car_H_mod_fun(double *state, double *out_7971983971871194008) {
-  H_mod_fun(state, out_7971983971871194008);
+void car_H_mod_fun(double *state, double *out_7781028056435054793) {
+  H_mod_fun(state, out_7781028056435054793);
 }
-void car_f_fun(double *state, double dt, double *out_1960663178760025601) {
-  f_fun(state,  dt, out_1960663178760025601);
+void car_f_fun(double *state, double dt, double *out_2481579766534270415) {
+  f_fun(state,  dt, out_2481579766534270415);
 }
-void car_F_fun(double *state, double dt, double *out_893498683805935900) {
-  F_fun(state,  dt, out_893498683805935900);
+void car_F_fun(double *state, double dt, double *out_3144021140418811034) {
+  F_fun(state,  dt, out_3144021140418811034);
 }
-void car_h_25(double *state, double *unused, double *out_3235092958161781685) {
-  h_25(state, unused, out_3235092958161781685);
+void car_h_25(double *state, double *unused, double *out_6782490269790769427) {
+  h_25(state, unused, out_6782490269790769427);
 }
-void car_H_25(double *state, double *unused, double *out_4449003321881702331) {
-  H_25(state, unused, out_4449003321881702331);
+void car_H_25(double *state, double *unused, double *out_4168693057384466872) {
+  H_25(state, unused, out_4168693057384466872);
 }
-void car_h_24(double *state, double *unused, double *out_5426063249126145013) {
-  h_24(state, unused, out_5426063249126145013);
+void car_h_24(double *state, double *unused, double *out_8556983568758949116) {
+  h_24(state, unused, out_8556983568758949116);
 }
-void car_H_24(double *state, double *unused, double *out_4845648626235561826) {
-  H_24(state, unused, out_4845648626235561826);
+void car_H_24(double *state, double *unused, double *out_9004556733412072150) {
+  H_24(state, unused, out_9004556733412072150);
 }
-void car_h_30(double *state, double *unused, double *out_9056203919707109802) {
-  h_30(state, unused, out_9056203919707109802);
+void car_h_30(double *state, double *unused, double *out_3145040741536747077) {
+  h_30(state, unused, out_3145040741536747077);
 }
-void car_H_30(double *state, double *unused, double *out_1930670363374453704) {
-  H_30(state, unused, out_1930670363374453704);
+void car_H_30(double *state, double *unused, double *out_359003272743141326) {
+  H_30(state, unused, out_359003272743141326);
 }
-void car_h_26(double *state, double *unused, double *out_6243651816437256419) {
-  h_26(state, unused, out_6243651816437256419);
+void car_h_26(double *state, double *unused, double *out_4127124834872045189) {
+  h_26(state, unused, out_4127124834872045189);
 }
-void car_H_26(double *state, double *unused, double *out_8190506640755758555) {
-  H_26(state, unused, out_8190506640755758555);
+void car_H_26(double *state, double *unused, double *out_427189738510410648) {
+  H_26(state, unused, out_427189738510410648);
 }
-void car_h_27(double *state, double *unused, double *out_3838408842838259202) {
-  h_27(state, unused, out_3838408842838259202);
+void car_h_27(double *state, double *unused, double *out_1780817002129923782) {
+  h_27(state, unused, out_1780817002129923782);
 }
-void car_H_27(double *state, double *unused, double *out_292923707809489513) {
-  H_27(state, unused, out_292923707809489513);
+void car_H_27(double *state, double *unused, double *out_2533766584543566237) {
+  H_27(state, unused, out_2533766584543566237);
 }
-void car_h_29(double *state, double *unused, double *out_259894455441112662) {
-  h_29(state, unused, out_259894455441112662);
+void car_h_29(double *state, double *unused, double *out_8902661814758955393) {
+  h_29(state, unused, out_8902661814758955393);
 }
-void car_H_29(double *state, double *unused, double *out_1420439019060061520) {
-  H_29(state, unused, out_1420439019060061520);
+void car_H_29(double *state, double *unused, double *out_151228071571250858) {
+  H_29(state, unused, out_151228071571250858);
 }
-void car_h_28(double *state, double *unused, double *out_5143560329168581563) {
-  h_28(state, unused, out_5143560329168581563);
+void car_h_28(double *state, double *unused, double *out_7683957899572787069) {
+  h_28(state, unused, out_7683957899572787069);
 }
-void car_H_28(double *state, double *unused, double *out_6502838036129592094) {
-  H_28(state, unused, out_6502838036129592094);
+void car_H_28(double *state, double *unused, double *out_4931170945498279716) {
+  H_28(state, unused, out_4931170945498279716);
 }
-void car_h_31(double *state, double *unused, double *out_2959898895877275796) {
-  h_31(state, unused, out_2959898895877275796);
+void car_h_31(double *state, double *unused, double *out_678068494458118898) {
+  h_31(state, unused, out_678068494458118898);
 }
-void car_H_31(double *state, double *unused, double *out_4418357360004741903) {
-  H_31(state, unused, out_4418357360004741903);
+void car_H_31(double *state, double *unused, double *out_199018363722940828) {
+  H_31(state, unused, out_199018363722940828);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
   predict(in_x, in_P, in_Q, dt);

selfdrive/locationd/models/generated/car.h

@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_4175267327343220067);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_8001717605441520298);
-void car_H_mod_fun(double *state, double *out_7971983971871194008);
-void car_f_fun(double *state, double dt, double *out_1960663178760025601);
-void car_F_fun(double *state, double dt, double *out_893498683805935900);
-void car_h_25(double *state, double *unused, double *out_3235092958161781685);
-void car_H_25(double *state, double *unused, double *out_4449003321881702331);
-void car_h_24(double *state, double *unused, double *out_5426063249126145013);
-void car_H_24(double *state, double *unused, double *out_4845648626235561826);
-void car_h_30(double *state, double *unused, double *out_9056203919707109802);
-void car_H_30(double *state, double *unused, double *out_1930670363374453704);
-void car_h_26(double *state, double *unused, double *out_6243651816437256419);
-void car_H_26(double *state, double *unused, double *out_8190506640755758555);
-void car_h_27(double *state, double *unused, double *out_3838408842838259202);
-void car_H_27(double *state, double *unused, double *out_292923707809489513);
-void car_h_29(double *state, double *unused, double *out_259894455441112662);
-void car_H_29(double *state, double *unused, double *out_1420439019060061520);
-void car_h_28(double *state, double *unused, double *out_5143560329168581563);
-void car_H_28(double *state, double *unused, double *out_6502838036129592094);
-void car_h_31(double *state, double *unused, double *out_2959898895877275796);
-void car_H_31(double *state, double *unused, double *out_4418357360004741903);
+void car_err_fun(double *nom_x, double *delta_x, double *out_3258091067705697569);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_2755855052909692674);
+void car_H_mod_fun(double *state, double *out_7781028056435054793);
+void car_f_fun(double *state, double dt, double *out_2481579766534270415);
+void car_F_fun(double *state, double dt, double *out_3144021140418811034);
+void car_h_25(double *state, double *unused, double *out_6782490269790769427);
+void car_H_25(double *state, double *unused, double *out_4168693057384466872);
+void car_h_24(double *state, double *unused, double *out_8556983568758949116);
+void car_H_24(double *state, double *unused, double *out_9004556733412072150);
+void car_h_30(double *state, double *unused, double *out_3145040741536747077);
+void car_H_30(double *state, double *unused, double *out_359003272743141326);
+void car_h_26(double *state, double *unused, double *out_4127124834872045189);
+void car_H_26(double *state, double *unused, double *out_427189738510410648);
+void car_h_27(double *state, double *unused, double *out_1780817002129923782);
+void car_H_27(double *state, double *unused, double *out_2533766584543566237);
+void car_h_29(double *state, double *unused, double *out_8902661814758955393);
+void car_H_29(double *state, double *unused, double *out_151228071571250858);
+void car_h_28(double *state, double *unused, double *out_7683957899572787069);
+void car_H_28(double *state, double *unused, double *out_4931170945498279716);
+void car_h_31(double *state, double *unused, double *out_678068494458118898);
+void car_H_31(double *state, double *unused, double *out_199018363722940828);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);

selfdrive/locationd/models/generated/pose.h

@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_2182435369834896236);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2883166859334089197);
-void pose_H_mod_fun(double *state, double *out_671491409411989589);
-void pose_f_fun(double *state, double dt, double *out_7919031781502386377);
-void pose_F_fun(double *state, double dt, double *out_7380360880106775220);
-void pose_h_4(double *state, double *unused, double *out_4702057929225754412);
-void pose_H_4(double *state, double *unused, double *out_3100315800431917983);
-void pose_h_10(double *state, double *unused, double *out_1901997026290432130);
-void pose_H_10(double *state, double *unused, double *out_5200655737232907775);
-void pose_h_13(double *state, double *unused, double *out_6749072148499604094);
-void pose_H_13(double *state, double *unused, double *out_6312589625764250784);
-void pose_h_14(double *state, double *unused, double *out_7677380843386584152);
-void pose_H_14(double *state, double *unused, double *out_17527368136545687);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_7195396181340429812);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6214721926630491138);
+void pose_H_mod_fun(double *state, double *out_8844892526159179063);
+void pose_f_fun(double *state, double dt, double *out_8367001063557654494);
+void pose_F_fun(double *state, double dt, double *out_2658931498525825673);
+void pose_h_4(double *state, double *unused, double *out_84081779949576375);
+void pose_H_4(double *state, double *unused, double *out_7708340643954596463);
+void pose_h_10(double *state, double *unused, double *out_7874962955445622717);
+void pose_H_10(double *state, double *unused, double *out_8079274030706525455);
+void pose_h_13(double *state, double *unused, double *out_2170051261675760671);
+void pose_H_13(double *state, double *unused, double *out_7526129604422622352);
+void pose_h_14(double *state, double *unused, double *out_352195394061766033);
+void pose_H_14(double *state, double *unused, double *out_6775162573415470624);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }

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

@@ -674,14 +674,25 @@ class StarPilotLongitudinalLayout(_SettingsPage):
 
     # ── 4. Adaptive Speed Controls Rows (CES + CSC + CCM) ──
     self._curve_speed_controller_rows = [
+      SettingRow("CurveSpeedApproachDecel", "value", tr_noop("Curve Speed Approach Decel"),
+                 subtitle=tr_noop("How hard the slowdown into a curve is planned. Lower starts it sooner and spreads it over more distance; higher waits longer and slows more firmly."),
+                 get_value=lambda: f"{self._params.get_float('CurveSpeedApproachDecel'):.1f} m/s²",
+                 on_click=lambda: self._show_slider("CurveSpeedApproachDecel", 0.3, 1.5, step=0.1,
+                                                   unit=" m/s²", value_type="float"),
+                 visible=csc_on),
+      SettingRow("CurveSpeedMargin", "value", tr_noop("Curve Speed Margin"),
+                 subtitle=tr_noop("How much of your learned cornering comfort to use. Lower slows more for curves; 100% matches how you take them yourself."),
+                 get_value=lambda: f"{self._params.get_int('CurveSpeedMargin')}%",
+                 on_click=lambda: self._show_slider("CurveSpeedMargin", 70, 100, step=5, unit="%"),
+                 visible=csc_on),
       SettingRow("CalibratedLatAccel", "value", tr_noop("Calibrated Lateral Accel"),
                  subtitle=tr_noop("The learned lateral acceleration from collected driving data. Higher values allow faster cornering."),
-                 get_value=lambda: f"{self._params_memory.get_float('CalibratedLateralAcceleration'):.2f} m/s",
+                 get_value=lambda: f"{self._params.get_float('CalibratedLateralAcceleration'):.2f} m/s",
                  on_click=None,
                  visible=csc_on),
       SettingRow("CalibrationProgress", "value", tr_noop("Calibration Progress"),
                  subtitle=tr_noop("How much curve data has been collected. Normal for the value to stay low."),
-                 get_value=lambda: f"{self._params_memory.get_float('CalibrationProgress'):.2f}%",
+                 get_value=lambda: f"{self._params.get_float('CalibrationProgress'):.2f}%",
                  on_click=None,
                  visible=csc_on),
       SettingRow("ResetCurve", "action", tr_noop("Reset Curve Data"),

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/common/starpilot_variables.py

@@ -49,6 +49,12 @@ from openpilot.system.version import get_build_metadata
 
 CITY_SPEED_LIMIT = 25                     # 55mph is typically the minimum speed for highways
 CRUISING_SPEED = 5                        # Roughly the speed cars go when not touching the gas while in drive
+CSC_DEFAULT_MARGIN_PERCENT = 85           # Percent of learned cornering comfort the Curve Speed Controller targets
+CSC_MIN_MARGIN_PERCENT = 70               # Slows the most; 100 would exactly match the driver's own habit
+CSC_MAX_MARGIN_PERCENT = 100
+CSC_DEFAULT_APPROACH_DECEL = 0.6          # m/s^2 the approach is planned at; sets how early the slowdown starts
+CSC_MIN_APPROACH_DECEL = 0.3              # earlier than this saturates against the model's ~10s horizon
+CSC_MAX_APPROACH_DECEL = 1.5              # later and firmer; matches the old ramp cap
 DEFAULT_LATERAL_ACCELERATION = 2.0        # m/s^2, typical lateral acceleration when taking curves
 DISPLAY_MENU_TIMER = 350                  # The length of time the following distance menu appears on some GM vehicles to prevent things getting out of sync
 EARTH_RADIUS = 6378137                    # Radius of the Earth in meters
@@ -826,6 +832,15 @@ class StarPilotVariables:
     toggle.curve_speed_controller = toggle.openpilot_longitudinal and self.get_value("CurveSpeedController")
     toggle.csc_no_lead = self.get_value("CurveSpeedControllerNoLead", condition=toggle.curve_speed_controller)
     toggle.csc_status = self.get_value("ShowCSCStatus", condition=toggle.curve_speed_controller) or toggle.debug_mode
+    # percent of learned cornering comfort to actually use; lower slows more
+    toggle.csc_margin = self.get_value("CurveSpeedMargin", cast=float, condition=toggle.curve_speed_controller,
+                                       default=CSC_DEFAULT_MARGIN_PERCENT, min=CSC_MIN_MARGIN_PERCENT,
+                                       max=CSC_MAX_MARGIN_PERCENT) / 100.0
+    # lower plans the approach over a longer distance, so the slowdown starts sooner
+    toggle.csc_approach_decel = self.get_value("CurveSpeedApproachDecel", cast=float,
+                                               condition=toggle.curve_speed_controller,
+                                               default=CSC_DEFAULT_APPROACH_DECEL, min=CSC_MIN_APPROACH_DECEL,
+                                               max=CSC_MAX_APPROACH_DECEL)
 
     toggle.goat_scream_alert = self.get_value("GoatScream")
     toggle.goat_scream_critical_alerts = self.get_value("GoatScreamCriticalAlerts")

starpilot/controls/lib/curve_speed_controller.py

@@ -2,19 +2,86 @@
 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,
+  CSC_DEFAULT_APPROACH_DECEL,
+  CSC_DEFAULT_MARGIN_PERCENT,
+  DEFAULT_LATERAL_ACCELERATION,
+  PLANNER_TIME,
+)
 
 CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL
 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
-CSC_MAX_DECEL_RATE = 1.5
+
+# braking distance is (v^2 - v_curve^2) / (2 * this), so lower starts the slowdown
+# sooner and spreads it further. User-tunable via CurveSpeedApproachDecel.
+CSC_APPROACH_DECEL = CSC_DEFAULT_APPROACH_DECEL
+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_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_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
+# Learned values match the driver's own cornering, which alone would never slow them
+# below their habit. Speed scales as the square root, so 0.85 is ~8% slower.
+CSC_COMFORT_MARGIN = CSC_DEFAULT_MARGIN_PERCENT / 100.0
+
 MAX_CURVATURE = 0.1
 MIN_CURVATURE = 0.001
-PERCENTILE = 90
 ROUNDING_PRECISION = 5
 STEP = 0.001
 
+# 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):
   try:
@@ -41,19 +108,33 @@ class CurveSpeedController:
   def __init__(self, StarPilotVCruise):
     self.starpilot_planner = StarPilotVCruise.starpilot_planner
 
+    self.starpilot_toggles = None
+
     self.enable_training = False
-    self.target_set = False
+    self.nudge_applied = False
 
     self.training_timer = 0.0
     self.persistence_timer = 0.0
+    self.training_quiet_timer = 0.0
     self.data_dirty = False
 
+    self.target = 0.0
+    self.binding_distance = 0.0
+    self.release_timer = 0.0
+    self.target_filter = FirstOrderFilter(0.0, CSC_TARGET_FILTER_RC, DT_MDL, initialized=False)
+    self.seed_pending = True
+
+    self._long_active_prev = False
+
     curvature_data = self.starpilot_planner.params.get("CurvatureData")
     self.curvature_data = self._normalize_curvature_data(curvature_data)
 
     self.required_curvatures = [str(round(road_curvature, ROUNDING_PRECISION)) for road_curvature in np.arange(MIN_CURVATURE, MAX_CURVATURE + STEP, STEP)]
 
-    self.update_lateral_acceleration()
+    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
 
   @staticmethod
   def _bucket_curvature(road_curvature):
@@ -107,6 +188,7 @@ class CurveSpeedController:
       if key in self.curvature_data:
         progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0)
 
+    self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration)
     self.starpilot_planner.params.put_nonblocking("CalibrationProgress", (progress / len(self.required_curvatures)) * 100)
     self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data)
     self.data_dirty = False
@@ -116,16 +198,21 @@ class CurveSpeedController:
     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
 
@@ -134,7 +221,7 @@ class CurveSpeedController:
       self.persistence_timer += DT_MDL
 
     in_curve = (
-      self.training_timer >= PLANNER_TIME and
+      self.training_timer >= CSC_TRAINING_SETTLE_TIME and
       self.starpilot_planner.driving_in_curve and
       not (sm["carState"].leftBlinker or sm["carState"].rightBlinker)
     )
@@ -144,11 +231,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] = {
@@ -157,7 +244,7 @@ class CurveSpeedController:
         }
 
       self.data_dirty = True
-      self.update_lateral_acceleration()
+      self.rebuild_lat_accel_curve()
       self.enable_training = True
 
       if self.persistence_timer >= PLANNER_TIME:
@@ -165,29 +252,137 @@ 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
+
+    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):
+      self._apply_nudge(CSC_NUDGE)
+    elif (getattr(sm["carState"], "brakePressed", False) or long_dropped) and self.starpilot_planner.driving_in_curve:
+      self._apply_nudge(-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
+    sample = float(np.clip(self.learned_lat_accel(float(key)) + offset, 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.nudge_applied = True
+    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)
+  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
+  @property
+  def comfort_margin(self):
+    margin = getattr(self.starpilot_toggles, "csc_margin", None)
+    return float(margin) if margin else 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))
+  @property
+  def approach_decel(self):
+    decel = getattr(self.starpilot_toggles, "csc_approach_decel", None)
+    return float(decel) if decel else CSC_APPROACH_DECEL
+
+  def lat_accel_for_curvature(self, curvature):
+    lat_accel = np.interp(np.abs(curvature), self._curve_k, self._curve_a) * self.comfort_margin
+
+    weather = self.starpilot_planner.starpilot_weather
+    if weather.weather_id != 0:
+      lat_accel = lat_accel * (1.0 - weather.reduce_lateral_acceleration)
+
+    return lat_accel
+
+  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
+      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 * self.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,12 @@ 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_ACTIVE_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,
@@ -14,7 +19,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
 STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0
@@ -184,8 +188,8 @@ class StarPilotVCruise:
     self._nav_instruction_state = {}
     self._applied_slc_control_target = 0.0
     self.csc_controlling_speed = False
+    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 {}
@@ -528,7 +532,8 @@ class StarPilotVCruise:
     v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego)
     v_ego_diff = v_ego_cluster - v_ego
 
-    # FrogsGoMoo's Curve Speed Controller
+    # Curve Speed Controller
+    self.csc.starpilot_toggles = starpilot_toggles
     following_lead = bool(getattr(self.starpilot_planner.starpilot_following, "following_lead", False))
     manual_speed_control = is_manual_speed_control(sm)
     csc_available = (
@@ -538,28 +543,52 @@ 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)
+    csc_blinker_on = sm["carState"].leftBlinker or sm["carState"].rightBlinker
+    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)
+    # RES+ cancels the slowdown for the rest of this curve; cruise.py consumes the press
+    # so the set speed is untouched. Latched outside the availability branch because the
+    # press itself suspends CSC, so otherwise the slowdown returns on button 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_target = v_cruise
+      else:
+        self.csc_target = self.csc.target
+        # a target under the set speed alone means nothing -- until it falls under v_ego
+        # the car is still accelerating toward it. Release still waits for the set speed,
+        # so the glow spans the whole recovery instead of clearing at the apex.
+        if self.csc_target < min(v_cruise - CSC_ACTIVE_ON_DELTA, v_ego):
+          self.csc_controlling_speed = True
+        elif self.csc_target > v_cruise - CSC_ACTIVE_OFF_DELTA:
+          self.csc_controlling_speed = False
+    elif csc_available:
+      # Blinker: release the cap so CSC can't fight a lane change, but keep planning.
+      # Resetting here threw the braking plan away, so the restart re-planned from the
+      # set speed with the curve much closer -- which arrived as a panic stop.
+      self.csc.update_target(v_ego, v_cruise)
+      self.csc_controlling_speed = False
+      self.csc_target = v_cruise
+    else:
+      self.csc.reset(v_cruise)
+      self.csc_controlling_speed = False
+      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
@@ -217,6 +217,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)
 
@@ -325,6 +326,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 = self.starpilot_following.disable_throttle

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

@@ -426,6 +426,14 @@
   color: var(--text-muted);
 }
 
+/* Read-only learned values (e.g. Curve Speed Controller calibration) */
+.ds-readout {
+  color: var(--text-color);
+  font-size: var(--font-size-sm);
+  font-weight: var(--font-weight-semibold);
+  white-space: nowrap;
+}
+
 .ds-manual-row {
   align-items: center;
   display: flex;

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

@@ -484,6 +484,13 @@ function formatStepValue(step, precision) {
   return Number(n.toFixed(Math.max(0, precision))).toString()
 }
 
+function formatReadoutValue(param, value) {
+  const n = Number(value)
+  if (value === null || value === undefined || !Number.isFinite(n)) return "--"
+  const precision = Number.isFinite(Number(param.precision)) ? Number(param.precision) : 2
+  return `${n.toFixed(precision)}${param.unit || ""}`
+}
+
 function numericBounds(param) {
   const defaultBounds = {
     min: param.min !== undefined ? param.min : (param.data_type === "float" ? 0.0 : 0),
@@ -1620,6 +1627,10 @@ function renderSettingRow(p) {
         disabled="${() => isLocked()}"
         @change="${() => updateParam(p.key, "text")}" />
     `
+  } else if (p.ui_type === "readout") {
+    rowControl = html`
+      <span class="ds-readout" id="ds-${p.key}">${() => formatReadoutValue(p, state.values[p.key])}</span>
+    `
   } else if (p.ui_type === "color") {
     rowControl = html`
       <div style="display:flex; align-items:center; gap:0.75rem;">

starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json

@@ -751,12 +751,39 @@
       {
         "key": "CurveSpeedController",
         "label": "Curve Speed Controller",
-        "description": "Automatically slow down for upcoming curves using data learned from your driving style, adapting to curves as you would.",
+        "description": "Automatically slow down for upcoming curves using data learned from your driving style, adapting to curves as you would. Press RES+ while it is slowing to cancel the slowdown for that curve.",
         "data_type": "bool",
         "ui_type": "toggle",
         "is_parent_toggle": true,
         "settings_tier": "simple"
       },
+      {
+        "key": "CurveSpeedApproachDecel",
+        "label": "Curve Speed Approach Decel",
+        "description": "How hard the slowdown into a curve is planned. Lower starts it sooner and spreads it over more distance; higher waits longer and slows more firmly.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.3,
+        "max": 1.5,
+        "step": 0.1,
+        "precision": 1,
+        "unit": " m/s²",
+        "parent_key": "CurveSpeedController",
+        "settings_tier": "simple"
+      },
+      {
+        "key": "CurveSpeedMargin",
+        "label": "Curve Speed Margin",
+        "description": "How much of your learned cornering comfort to use for curves. Lower slows more; 100% matches how you take curves yourself.",
+        "data_type": "int",
+        "ui_type": "numeric",
+        "min": 70.0,
+        "max": 100.0,
+        "step": 5.0,
+        "unit": "%",
+        "parent_key": "CurveSpeedController",
+        "settings_tier": "simple"
+      },
       {
         "key": "ShowCSCStatus",
         "label": "Status Widget",
@@ -775,10 +802,32 @@
         "parent_key": "CurveSpeedController",
         "settings_tier": "simple"
       },
+      {
+        "key": "CalibratedLateralAcceleration",
+        "label": "Calibrated Lateral Accel",
+        "description": "The learned lateral acceleration from collected driving data. Higher values allow faster cornering.",
+        "data_type": "float",
+        "ui_type": "readout",
+        "precision": 2,
+        "unit": " m/s²",
+        "parent_key": "CurveSpeedController",
+        "settings_tier": "simple"
+      },
+      {
+        "key": "CalibrationProgress",
+        "label": "Calibration Progress",
+        "description": "How much curve data has been collected. Normal for the value to stay low.",
+        "data_type": "float",
+        "ui_type": "readout",
+        "precision": 2,
+        "unit": "%",
+        "parent_key": "CurveSpeedController",
+        "settings_tier": "simple"
+      },
       {
         "key": "ResetCurveData",
         "label": "Reset Curve Data",
-        "description": "Clear learned Curve Speed Controller data and begin training again from the default lateral acceleration.",
+        "description": "Clear learned Curve Speed Controller data and begin training again from the built-in comfort defaults.",
         "ui_type": "action",
         "action_label": "Reset",
         "action_endpoint": "/api/curve_speed_controller/reset",

starpilot/system/the_galaxy/the_galaxy.py

@@ -5323,6 +5323,13 @@ def setup(app):
     result["AlphaLongitudinalAvailable"] = _get_alpha_longitudinal_available()
     result["HasRivianAngleHarness"] = _get_has_rivian_angle_harness()
 
+    # display only; kept out of allowed_keys so the write paths still reject them
+    for readonly_key in ("CalibratedLateralAcceleration", "CalibrationProgress"):
+      try:
+        result[readonly_key] = params.get_float(readonly_key)
+      except Exception:
+        result[readonly_key] = None
+
     return jsonify(_sanitize_json_value(result)), 200
 
   @app.route("/api/params/defaults", methods=["GET"])