diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index fb82cf477..89d905978 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/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 diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index eb31447ad..288eb86e5 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/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 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 3cc00e131..5a2d3bc87 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/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)