diff --git a/selfdrive/controls/tests/test_curve_speed_controller.py b/selfdrive/controls/tests/test_curve_speed_controller.py index d0ab4ae5d..39ee0f9f9 100644 --- a/selfdrive/controls/tests/test_curve_speed_controller.py +++ b/selfdrive/controls/tests/test_curve_speed_controller.py @@ -4,7 +4,7 @@ import pytest from types import SimpleNamespace from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.common.starpilot_variables import DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME +from openpilot.starpilot.common.starpilot_variables import DEFAULT_LATERAL_ACCELERATION from openpilot.starpilot.controls.lib.curve_speed_controller import ( CSC_APPROACH_DECEL, CSC_COMFORT_MARGIN, @@ -13,6 +13,7 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import ( CSC_LAT_ACCEL_MAX, CSC_MIN_SPEED, CSC_NUDGE_WEIGHT, + CSC_TRAINING_SETTLE_TIME, CurveSpeedController, weighted_isotonic, ) @@ -298,7 +299,7 @@ def test_legacy_off_grid_curvature_data_merges_into_buckets(): def test_training_update_step_is_capped_by_ema_count(): planner, controller = make_controller(curvature_data={"0.02": {"average": 2.0, "count": 10000}}, driving_in_curve=True) planner.lateral_acceleration = 3.0 - controller.training_timer = PLANNER_TIME + controller.training_timer = CSC_TRAINING_SETTLE_TIME controller.log_data(10.0, make_sm(long_active=False)) @@ -313,17 +314,67 @@ def test_no_passive_training_right_after_csc_limited_speed(): converge(controller, 15.0, 30.0) assert controller.training_quiet_timer > 0.0 - controller.training_timer = PLANNER_TIME + controller.training_timer = CSC_TRAINING_SETTLE_TIME controller.log_data(10.0, make_sm(long_active=False)) assert "0.02" not in controller.curvature_data assert not controller.enable_training controller.training_quiet_timer = 0.0 - controller.training_timer = PLANNER_TIME + controller.training_timer = CSC_TRAINING_SETTLE_TIME controller.log_data(10.0, make_sm(long_active=False)) assert controller.curvature_data["0.02"]["count"] == 1 +def test_training_settles_within_a_couple_of_seconds(): + # a real drive rarely holds every eligibility condition for a whole model horizon, + # so the settle time has to be short enough that ordinary curves still teach it + planner, controller = make_controller(driving_in_curve=True) + planner.lateral_acceleration = 2.4 + sm = make_sm(long_active=False) + + for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) - 2): + controller.log_data(10.0, sm) + assert "0.02" not in controller.curvature_data + + for _ in range(3): + controller.log_data(10.0, sm) + assert controller.curvature_data["0.02"]["count"] >= 1 + + +def test_brief_ineligibility_does_not_restart_the_settle_timer(): + planner, controller = make_controller(driving_in_curve=True) + planner.lateral_acceleration = 2.4 + sm = make_sm(long_active=False) + for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) + 1): + controller.log_data(10.0, sm) + trained = controller.curvature_data["0.02"]["count"] + + # a lead flickers into the tracker for two frames, then leaves + planner.tracking_lead = True + controller.log_data(10.0, sm) + controller.log_data(10.0, sm) + planner.tracking_lead = False + + controller.log_data(10.0, sm) + assert controller.curvature_data["0.02"]["count"] == trained + 1 + + +def test_sustained_ineligibility_still_drains_the_settle_timer(): + planner, controller = make_controller(driving_in_curve=True) + planner.lateral_acceleration = 2.4 + engaged = make_sm(long_active=True) + manual = make_sm(long_active=False) + for _ in range(int(CSC_TRAINING_SETTLE_TIME / DT_MDL) + 1): + controller.log_data(10.0, manual) + + for _ in range(int(2 * CSC_TRAINING_SETTLE_TIME / DT_MDL)): + controller.log_data(10.0, engaged) + assert controller.training_timer == pytest.approx(0.0) + + controller.log_data(10.0, manual) + assert not controller.enable_training + + def test_gas_override_nudges_bucket_up_once_per_episode(): _, controller = make_controller() prior = controller.learned_lat_accel(0.02) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index ec9a7f715..99f82a719 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -75,12 +75,12 @@ def make_sm(*, standstill=True, min_steer_speed=0.0): } -def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, controls_enabled=True): +def update_vcruise(vcruise, sm, toggles, *, now, v_ego=0.0, v_cruise=20.0, controls_enabled=True): return vcruise.update( controls_enabled=controls_enabled, now=now, time_validated=True, - v_cruise=20.0, + v_cruise=v_cruise, v_ego=v_ego, sm=sm, starpilot_toggles=toggles, @@ -333,6 +333,49 @@ def test_csc_res_press_defers_to_slc_confirmation(): assert vcruise.csc_controlling_speed +def test_curve_speed_controller_glow_stays_off_while_the_target_is_above_v_ego(): + planner, vcruise = make_vcruise() + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.curve_speed_controller = True + + # a highway sweeper trims the target well under the set speed but never under v_ego, + # so the car keeps accelerating and the driver feels nothing + def set_curve_target(_v_ego, _v_cruise): + vcruise.csc.target = 26.0 + + vcruise.csc.update_target = set_curve_target + result = update_vcruise(vcruise, sm, toggles, now=90.0, v_ego=20.0, v_cruise=30.0) + + assert result == pytest.approx(26.0) + assert not vcruise.csc_controlling_speed + + +def test_curve_speed_controller_glow_holds_through_the_recovery_ramp(): + 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=100.0, v_ego=20.0) + assert vcruise.csc_controlling_speed + + # past the apex the target climbs back above v_ego while the car is still cornering + curve_target["v"] = 18.0 + update_vcruise(vcruise, sm, toggles, now=100.05, v_ego=15.0) + assert vcruise.csc_controlling_speed + + curve_target["v"] = 20.0 + update_vcruise(vcruise, sm, toggles, now=100.1, v_ego=17.0) + assert not vcruise.csc_controlling_speed + + def test_curve_speed_controller_hysteresis_keeps_glow_off_for_marginal_targets(): planner, vcruise = make_vcruise() sm = make_sm(standstill=False) diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index cec199512..734e92933 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -34,6 +34,7 @@ CSC_LAT_ACCEL_MAX = 3.2 CSC_NUDGE = 0.15 CSC_NUDGE_WEIGHT = 20 # counts a single override pseudo-sample is worth 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 # below their habit. Speed scales as the square root, so 0.85 is ~8% slower. CSC_COMFORT_MARGIN = CSC_DEFAULT_MARGIN_PERCENT / 100.0 @@ -207,7 +208,9 @@ class CurveSpeedController: if not eligible: self.flush_data() - self.training_timer = 0.0 + # decay instead of resetting: a lead flickering in and out of the tracker used to + # cost the full re-arm, which left almost nothing to learn from on a real drive + self.training_timer = max(self.training_timer - DT_MDL, 0.0) self.persistence_timer = 0.0 return @@ -216,7 +219,7 @@ class CurveSpeedController: self.persistence_timer += DT_MDL in_curve = ( - self.training_timer >= PLANNER_TIME and + self.training_timer >= CSC_TRAINING_SETTLE_TIME and self.starpilot_planner.driving_in_curve and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) ) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index be0757bf3..67fae8c59 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -568,7 +568,10 @@ class StarPilotVCruise: self.csc_target = v_cruise else: self.csc_target = self.csc.target - if self.csc_target < v_cruise - CSC_ACTIVE_ON_DELTA: + # 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): self.csc_controlling_speed = True elif self.csc_target > v_cruise - CSC_ACTIVE_OFF_DELTA: self.csc_controlling_speed = False