test6

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -3,7 +3,9 @@ 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_GLOW_HOLD_TIME
 from openpilot.starpilot.controls.lib.starpilot_vcruise import (
   FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME,
   StarPilotVCruise,
@@ -365,6 +367,85 @@ def test_csc_res_press_defers_to_slc_confirmation():
   assert vcruise.csc_controlling_speed
 
 
+def test_curve_speed_controller_glow_holds_through_a_brief_release():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  curve_target = {"v": 14.0}
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = curve_target["v"]
+
+  vcruise.csc.update_target = set_curve_target
+  update_vcruise(vcruise, sm, toggles, now=130.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  # one curve routinely lets go and re-engages; the glow must ride through it
+  curve_target["v"] = 20.0
+  now = 130.0
+  for _ in range(int((CSC_GLOW_HOLD_TIME - 0.2) / DT_MDL)):
+    now += DT_MDL
+    update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
+    assert vcruise.csc_controlling_speed
+
+  curve_target["v"] = 14.0
+  now += DT_MDL
+  update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_glow_clears_once_the_release_sticks():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  curve_target = {"v": 14.0}
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = curve_target["v"]
+
+  vcruise.csc.update_target = set_curve_target
+  update_vcruise(vcruise, sm, toggles, now=140.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  curve_target["v"] = 20.0
+  now = 140.0
+  for _ in range(int(CSC_GLOW_HOLD_TIME / DT_MDL) + 1):
+    now += DT_MDL
+    update_vcruise(vcruise, sm, toggles, now=now, v_ego=20.0)
+  assert not vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_keeps_the_cap_when_signalling_mid_curve():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  def set_curve_target(_v_ego, _v_cruise):
+    vcruise.csc.target = 14.0
+
+  vcruise.csc.update_target = set_curve_target
+  result = update_vcruise(vcruise, sm, toggles, now=150.0, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+
+  # a lane change taken inside a curve must not hand the speed back
+  planner.driving_in_curve = True
+  sm["carState"].leftBlinker = True
+  result = update_vcruise(vcruise, sm, toggles, now=150.05, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+  # on a straight it still yields, so CSC can't fight the manoeuvre
+  planner.driving_in_curve = False
+  result = update_vcruise(vcruise, sm, toggles, now=150.1, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_controlling_speed
+
+
 def test_curve_speed_controller_glow_lights_when_the_car_arrives_at_the_cap_from_below():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)
@@ -424,8 +505,15 @@ def test_curve_speed_controller_glow_holds_through_the_recovery_ramp():
   update_vcruise(vcruise, sm, toggles, now=100.05, v_ego=15.0)
   assert vcruise.csc_controlling_speed
 
+  # fully released, but the glow only clears once the release has stuck
   curve_target["v"] = 20.0
-  update_vcruise(vcruise, sm, toggles, now=100.1, v_ego=17.0)
+  now = 100.1
+  update_vcruise(vcruise, sm, toggles, now=now, v_ego=17.0)
+  assert vcruise.csc_controlling_speed
+
+  for _ in range(int(CSC_GLOW_HOLD_TIME / DT_MDL) + 1):
+    now += DT_MDL
+    update_vcruise(vcruise, sm, toggles, now=now, v_ego=17.0)
   assert not vcruise.csc_controlling_speed
 
 

starpilot/controls/lib/curve_speed_controller.py

@@ -27,6 +27,9 @@ CSC_EGO_HEADROOM = 2.0            # target never trails below v_ego, so CSC can'
 CSC_RELEASE_DEBOUNCE = 0.25       # s the envelope must stay clear before that floor applies
 CSC_ACTIVE_ON_DELTA = 0.5
 CSC_ACTIVE_OFF_DELTA = 0.25
+CSC_GLOW_HOLD_TIME = 3.0          # s the cap must stay released before the glow clears; the
+                                  # on/off band is only ~0.5 mph wide, so a target hovering
+                                  # near the set speed crosses it repeatedly on one curve
 
 CSC_COUNT_CAP = 600               # EMA floor: samples beyond this stop shrinking the update step
 CSC_PRIOR_COUNT = 100             # bucket count at which learned data and the prior have equal weight

starpilot/controls/lib/starpilot_vcruise.py

@@ -9,6 +9,7 @@ from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRU
 from openpilot.starpilot.controls.lib.curve_speed_controller import (
   CSC_ACTIVE_OFF_DELTA,
   CSC_ACTIVE_ON_DELTA,
+  CSC_GLOW_HOLD_TIME,
   CurveSpeedController,
   is_manual_speed_control,
 )
@@ -188,6 +189,7 @@ class StarPilotVCruise:
     self._nav_instruction_state = {}
     self._applied_slc_control_target = 0.0
     self.csc_controlling_speed = False
+    self.csc_glow_release_timer = 0.0
     self.csc_override = False
     self.csc_target = 0.0
 
@@ -543,7 +545,12 @@ class StarPilotVCruise:
       starpilot_toggles.curve_speed_controller and
       (not getattr(starpilot_toggles, "csc_no_lead", False) or not following_lead)
     )
-    csc_blinker_on = sm["carState"].leftBlinker or sm["carState"].rightBlinker
+    # The blinker veto is for lane changes and turns, where the model's path curvature is the
+    # manoeuvre rather than the road. Already cornering, the cut is real: releasing it there let
+    # the car accelerate into the curve and then claw the speed back in one step when the
+    # blinker cleared, with the glow dark for the whole lane change.
+    csc_blinker_on = ((sm["carState"].leftBlinker or sm["carState"].rightBlinker) and
+                      not self.starpilot_planner.driving_in_curve)
     csc_was_controlling = self.csc_controlling_speed
     # a pending SLC confirmation owns the accel button
     slc_confirmation_pending = self.slc.speed_limit_changed_timer > DT_MDL and self.slc.unconfirmed_speed_limit >= 1
@@ -565,6 +572,7 @@ class StarPilotVCruise:
 
       if self.csc_override:
         self.csc_controlling_speed = False
+        self.csc_glow_release_timer = 0.0
         self.csc_target = v_cruise
       else:
         self.csc_target = self.csc.target
@@ -574,18 +582,26 @@ class StarPilotVCruise:
         # 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
+          self.csc_glow_release_timer = 0.0
         elif self.csc_target > v_cruise - CSC_ACTIVE_OFF_DELTA:
-          self.csc_controlling_speed = False
+          # hold through a brief release: one curve routinely lets go and re-engages
+          self.csc_glow_release_timer += DT_MDL
+          if self.csc_glow_release_timer >= CSC_GLOW_HOLD_TIME:
+            self.csc_controlling_speed = False
+        else:
+          self.csc_glow_release_timer = 0.0
     elif csc_available:
       # 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_glow_release_timer = 0.0
       self.csc_target = v_cruise
     else:
       self.csc.reset(v_cruise)
       self.csc_controlling_speed = False
+      self.csc_glow_release_timer = 0.0
       self.csc_target = v_cruise
 
     self.csc.handle_override(v_ego, csc_was_controlling, sm, accel_button=csc_accel_button)