diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 13c61f80d..5e0fcc87b 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -5,7 +5,7 @@ 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.curve_speed_controller import CSC_GLOW_HOLD_TIME, CSC_GLOW_ON_DELTA from openpilot.starpilot.controls.lib.starpilot_vcruise import ( FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, StarPilotVCruise, @@ -367,6 +367,24 @@ def test_csc_res_press_defers_to_slc_confirmation(): assert vcruise.csc_controlling_speed +def test_curve_speed_controller_glow_ignores_a_trivial_graze(): + planner, vcruise = make_vcruise() + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.curve_speed_controller = True + + # a long gentle bend where the envelope only shaves a little: the target hovers either + # side of the threshold for the whole curve, so a low bar strobes the glow + def set_curve_target(_v_ego, _v_cruise): + vcruise.csc.target = 20.0 - (CSC_GLOW_ON_DELTA / 2.0) + + vcruise.csc.update_target = set_curve_target + result = update_vcruise(vcruise, sm, toggles, now=160.0, v_ego=20.0) + + assert result < 20.0 # the cap is still applied + assert not vcruise.csc_controlling_speed # it just isn't worth announcing + + def test_curve_speed_controller_glow_holds_through_a_brief_release(): planner, vcruise = make_vcruise() sm = make_sm(standstill=False) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_border.py b/selfdrive/ui/onroad/starpilot/starpilot_border.py index b2a6f4dee..2345ecf83 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_border.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_border.py @@ -57,7 +57,10 @@ def _csc_state(): plan = sm["starpilotPlan"] params = ui_state.ui_params - if plan.speedLimitChanged or not params.get_bool("ShowCSCStatus"): + # A pending speed limit flashes the speed limit sign, not the border, so it has no + # reason to blank this. Suppressing it hid real curve slowdowns for the whole + # confirmation window, which can run the full 30 s before the prompt times out. + if not params.get_bool("ShowCSCStatus"): return None car_state = sm["carState"] diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index 288eb86e5..e3867e40c 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -27,6 +27,10 @@ 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_ON_DELTA = 1.0 # ~2.2 mph. Separate from CSC_ACTIVE_ON_DELTA, which arms the + # training quiet timer and must stay sensitive. A graze this + # small isn't worth showing, and on a long gentle curve the + # target hovers across a lower threshold for the whole bend. 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 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 7df5921e6..27ea68ceb 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -8,8 +8,8 @@ from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED from openpilot.starpilot.controls.lib.curve_speed_controller import ( CSC_ACTIVE_OFF_DELTA, - CSC_ACTIVE_ON_DELTA, CSC_GLOW_HOLD_TIME, + CSC_GLOW_ON_DELTA, CurveSpeedController, is_manual_speed_control, ) @@ -581,7 +581,7 @@ class StarPilotVCruise: # 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: + if self.csc_target < v_cruise - CSC_GLOW_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: