test5

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -365,6 +365,27 @@ def test_csc_res_press_defers_to_slc_confirmation():
   assert vcruise.csc_controlling_speed
 
 
+def test_curve_speed_controller_glow_lights_when_the_car_arrives_at_the_cap_from_below():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  # accelerating out of a slow zone into a curve: the target is never under v_ego, but it
+  # is still the only thing stopping the car from reaching the set speed
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 22.0
+
+  vcruise.csc.update_target = set_curve_target
+
+  update_vcruise(vcruise, sm, toggles, now=120.0, v_ego=15.0, v_cruise=32.0)
+  assert not vcruise.csc_controlling_speed          # still climbing, CSC isn't holding it yet
+
+  result = update_vcruise(vcruise, sm, toggles, now=120.05, v_ego=22.0, v_cruise=32.0)
+  assert result == pytest.approx(22.0)
+  assert vcruise.csc_controlling_speed              # arrived at the cap, and it binds
+
+
 def test_curve_speed_controller_glow_stays_off_while_the_target_is_above_v_ego():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)

starpilot/controls/lib/starpilot_vcruise.py

@@ -568,10 +568,11 @@ class StarPilotVCruise:
         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):
+        # A target under the set speed alone means nothing -- it only bites once the car has
+        # reached it, whether by being slowed down to it or by accelerating up into it. The
+        # second reading of CSC_ACTIVE_OFF_DELTA is that margin. Release still waits for the
+        # set speed, so the glow spans the hold and the whole recovery, not just the braking.
+        if self.csc_target < v_cruise - CSC_ACTIVE_ON_DELTA and v_ego >= self.csc_target - CSC_ACTIVE_OFF_DELTA:
           self.csc_controlling_speed = True
         elif self.csc_target > v_cruise - CSC_ACTIVE_OFF_DELTA:
           self.csc_controlling_speed = False