test3

selfdrive/controls/tests/test_curve_speed_controller.py

@@ -4,7 +4,7 @@ import pytest
 from types import SimpleNamespace
 
 from openpilot.common.realtime import DT_MDL
-from openpilot.starpilot.common.starpilot_variables import DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME
+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,
@@ -13,6 +13,7 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import (
   CSC_LAT_ACCEL_MAX,
   CSC_MIN_SPEED,
   CSC_NUDGE_WEIGHT,
+  CSC_TRAINING_SETTLE_TIME,
   CurveSpeedController,
   weighted_isotonic,
 )
@@ -298,7 +299,7 @@ def test_legacy_off_grid_curvature_data_merges_into_buckets():
 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 = PLANNER_TIME
+  controller.training_timer = CSC_TRAINING_SETTLE_TIME
 
   controller.log_data(10.0, make_sm(long_active=False))
 
@@ -313,17 +314,67 @@ def test_no_passive_training_right_after_csc_limited_speed():
   converge(controller, 15.0, 30.0)
   assert controller.training_quiet_timer > 0.0
 
-  controller.training_timer = PLANNER_TIME
+  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 = PLANNER_TIME
+  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)

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -75,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,
@@ -333,6 +333,49 @@ def test_csc_res_press_defers_to_slc_confirmation():
   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)

starpilot/controls/lib/curve_speed_controller.py

@@ -34,6 +34,7 @@ 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
@@ -207,7 +208,9 @@ class CurveSpeedController:
 
     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
 
@@ -216,7 +219,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)
     )

starpilot/controls/lib/starpilot_vcruise.py

@@ -568,7 +568,10 @@ class StarPilotVCruise:
         self.csc_target = v_cruise
       else:
         self.csc_target = self.csc.target
-        if self.csc_target < v_cruise - CSC_ACTIVE_ON_DELTA:
+        # 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