test8

selfdrive/controls/tests/test_curve_speed_controller.py

@@ -12,7 +12,9 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import (
   CSC_EGO_HEADROOM,
   CSC_LAT_ACCEL_MAX,
   CSC_MIN_SPEED,
+  CSC_NUDGE,
   CSC_NUDGE_WEIGHT,
+  CSC_OVERRIDE_WATCH_TIME,
   CSC_TARGET_UP_RATE,
   CSC_TRAINING_SETTLE_TIME,
   CurveSpeedController,
@@ -407,30 +409,69 @@ def test_sustained_ineligibility_still_drains_the_settle_timer():
   assert not controller.enable_training
 
 
+def settle_override(controller, sm=None, frames=None):
+  """Run the post-override watch out so the pseudo-sample is committed."""
+  sm = sm if sm is not None else make_sm()
+  for _ in range(frames if frames is not None else int(CSC_OVERRIDE_WATCH_TIME / DT_MDL) + 1):
+    controller.handle_override(20.0, False, sm)
+
+
 def test_gas_override_nudges_bucket_up_once_per_episode():
   _, controller = make_controller()
   prior = controller.learned_lat_accel(0.02)
   controller.target = 10.0
 
   controller.handle_override(20.0, True, make_sm(gas=True))
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  assert "0.02" not in controller.curvature_data  # still watching what the driver holds
+
+  settle_override(controller)
   assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
   assert controller.curvature_data["0.02"]["average"] > prior
 
-  controller.handle_override(20.0, 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))
+  settle_override(controller)
   assert controller.curvature_data["0.02"]["count"] == 2 * CSC_NUDGE_WEIGHT
 
 
+def test_override_learns_the_cornering_the_driver_actually_held():
+  # the whole point: a fixed step needs several rejections to close a real disagreement,
+  # so record what they demonstrated instead
+  planner, observed = make_controller(driving_in_curve=True)
+  observed.target = 10.0
+  observed.handle_override(20.0, True, make_sm(gas=True))
+  planner.lateral_acceleration = 2.9          # they hold the curve much harder than CSC wanted
+  settle_override(observed, make_sm(gas=True))
+
+  _, stepped = make_controller(driving_in_curve=True)
+  stepped._apply_nudge(CSC_NUDGE)             # what the old fixed-step path would have recorded
+
+  assert observed.curvature_data["0.02"]["average"] == pytest.approx(2.9)
+  assert observed.curvature_data["0.02"]["average"] > stepped.curvature_data["0.02"]["average"]
+  assert observed.learned_lat_accel(0.02) > stepped.learned_lat_accel(0.02)
+
+
+def test_override_on_a_straight_still_registers_the_fixed_step():
+  planner, controller = make_controller()
+  prior = controller.learned_lat_accel(0.02)
+  controller.target = 10.0
+
+  controller.handle_override(20.0, True, make_sm(gas=True))
+  planner.lateral_acceleration = 0.0          # never reached a corner
+  settle_override(controller)
+
+  assert controller.curvature_data["0.02"]["average"] == pytest.approx(prior + CSC_NUDGE)
+
+
 def test_res_button_nudges_bucket_up_even_at_target_speed():
   _, controller = make_controller()
   prior = controller.learned_lat_accel(0.02)
   controller.target = 20.0  # car tracking the target, so the gas-press condition would not fire
 
   controller.handle_override(20.0, True, make_sm(), accel_button=True)
+  settle_override(controller)
 
   assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT
   assert controller.curvature_data["0.02"]["average"] > prior

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -46,6 +46,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
     starpilot_following=SimpleNamespace(following_lead=False),
     tracking_lead=False,
     driving_in_curve=False,
+    lateral_acceleration=0.0,
     model_length=60.0,
     raw_model_stopped=raw_model_stopped,
     road_curvature=road_curvature,

starpilot/controls/lib/curve_speed_controller.py

@@ -41,6 +41,7 @@ CSC_LAT_ACCEL_MIN = 1.2
 CSC_LAT_ACCEL_MAX = 3.2
 CSC_NUDGE = 0.15
 CSC_NUDGE_WEIGHT = 20             # counts a single override pseudo-sample is worth
+CSC_OVERRIDE_WATCH_TIME = 6.0     # s to keep watching what the driver holds after they reject a cut
 CSC_TRAINING_QUIET_TIME = 5.0     # blocks passive samples after CSC limited speed, so it can't learn its own cap
 CSC_TRAINING_SETTLE_TIME = 2.0    # driver-owned seconds before a sample counts, so it isn't openpilot's leftover speed
 # Learned values match the driver's own cornering, which alone would never slow them
@@ -120,6 +121,10 @@ class CurveSpeedController:
     self.enable_training = False
     self.nudge_applied = False
 
+    self.override_watch_key = None
+    self.override_watch_peak = 0.0
+    self.override_watch_timer = 0.0
+
     self.training_timer = 0.0
     self.persistence_timer = 0.0
     self.training_quiet_timer = 0.0
@@ -264,6 +269,8 @@ class CurveSpeedController:
     long_dropped = self._long_active_prev and not long_active
     self._long_active_prev = long_active
 
+    self._update_override_watch(sm)
+
     if not was_controlling:
       self.nudge_applied = False
       return
@@ -272,14 +279,46 @@ class CurveSpeedController:
       return
 
     if accel_button or (sm["carState"].gasPressed and self.target < v_ego - 0.5):
-      self._apply_nudge(CSC_NUDGE)
+      # Watch what the driver actually holds instead of stepping by a fixed amount. CSC is
+      # suspended while they override, so their cornering measures their comfort rather than
+      # this controller's own cap; a fixed step needs several rejections to close a real gap.
+      self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
+      self.override_watch_peak = abs(self.starpilot_planner.lateral_acceleration)
+      self.override_watch_timer = CSC_OVERRIDE_WATCH_TIME
+      self.nudge_applied = True
     elif (getattr(sm["carState"], "brakePressed", False) or long_dropped) and self.starpilot_planner.driving_in_curve:
       self._apply_nudge(-CSC_NUDGE)
 
+  def _update_override_watch(self, sm):
+    if self.override_watch_key is None:
+      return
+
+    lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration)
+    if lateral_acceleration > self.override_watch_peak:
+      # credit the bucket the peak actually happened in, not the one at the button press
+      self.override_watch_peak = lateral_acceleration
+      self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
+
+    self.override_watch_timer -= DT_MDL
+    if self.override_watch_timer > 0.0 and (is_user_overriding_longitudinal(sm) or
+                                            self.starpilot_planner.driving_in_curve):
+      return
+
+    key = self.override_watch_key
+    self.override_watch_key = None
+    # floored at the old fixed step, so a rejection that never reaches a corner still counts
+    # and this path can only ever raise the bucket
+    self._record_pseudo_sample(key, max(self.override_watch_peak,
+                                        self.learned_lat_accel(float(key)) + CSC_NUDGE))
+
   def _apply_nudge(self, offset):
     key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature))
     # relative to the learned value, not the margined one, or repeated overrides walk the bucket down
-    sample = float(np.clip(self.learned_lat_accel(float(key)) + offset, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX))
+    self._record_pseudo_sample(key, self.learned_lat_accel(float(key)) + offset)
+    self.nudge_applied = True
+
+  def _record_pseudo_sample(self, key, sample):
+    sample = float(np.clip(sample, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX))
 
     data = self.curvature_data.get(key, {"average": sample, "count": 0})
     effective_count = min(data["count"], CSC_COUNT_CAP)
@@ -289,7 +328,6 @@ class CurveSpeedController:
       "count": data["count"] + CSC_NUDGE_WEIGHT,
     }
 
-    self.nudge_applied = True
     self.rebuild_lat_accel_curve()
     self.data_dirty = True
     self.flush_data()