From f1bcffd2790629b56bbf87845f11818146d8579f Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Wed, 2 Sep 2026 09:11:33 -0500 Subject: [PATCH] CSC rewrite --- .../test_conditional_experimental_mode.py | 1 + .../tests/test_curve_speed_controller.py | 506 ++++++++++++++++++ .../controls/tests/test_starpilot_vcruise.py | 394 ++++++++++---- .../ui/onroad/starpilot/starpilot_border.py | 2 + starpilot/common/starpilot_utilities.py | 17 + .../controls/lib/curve_speed_controller.py | 374 ++++++++++--- starpilot/controls/lib/starpilot_vcruise.py | 82 ++- starpilot/controls/starpilot_planner.py | 6 +- .../components/tools/device_settings.css | 1 + .../components/tools/device_settings.js | 6 +- starpilot/system/the_galaxy/the_galaxy.py | 10 + tools/longitudinal/analyze_csc.py | 295 ++++++++++ 12 files changed, 1511 insertions(+), 183 deletions(-) create mode 100644 selfdrive/controls/tests/test_curve_speed_controller.py create mode 100644 tools/longitudinal/analyze_csc.py diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 3e72eb70f..5c0b973bf 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -1168,6 +1168,7 @@ def test_starpilot_planner_updates_cem_with_current_frame_state(monkeypatch): try: monkeypatch.setattr(starpilot_planner_module, "calculate_road_curvature", lambda model, v_ego: (0.01, 1.0)) + monkeypatch.setattr(starpilot_planner_module, "extract_curve_profile", lambda model: ([], [])) monkeypatch.setattr(planner.starpilot_acceleration, "update", lambda *args, **kwargs: None) monkeypatch.setattr(planner.starpilot_events, "update", lambda *args, **kwargs: None) monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0) diff --git a/selfdrive/controls/tests/test_curve_speed_controller.py b/selfdrive/controls/tests/test_curve_speed_controller.py new file mode 100644 index 000000000..5993c184a --- /dev/null +++ b/selfdrive/controls/tests/test_curve_speed_controller.py @@ -0,0 +1,506 @@ +import numpy as np +import pytest + +from types import SimpleNamespace + +from openpilot.common.realtime import DT_MDL +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, + CSC_COUNT_CAP, + CSC_EGO_HEADROOM, + CSC_FARFIELD_GAIN, + CSC_LAT_ACCEL_MAX, + CSC_MIN_SPEED, + MAX_CURVATURE, + PRIOR_CURVATURE_BP, + PRIOR_LAT_ACCEL_V, + CSC_NUDGE, + CSC_NUDGE_WEIGHT, + CSC_OVERRIDE_WATCH_TIME, + CSC_TARGET_UP_RATE, + CSC_TRAINING_SETTLE_TIME, + CurveSpeedController, + weighted_isotonic, +) + + +class FakeParams: + def __init__(self, values=None): + self.values = dict(values or {}) + + def get(self, *args, **kwargs): + key = args[0] if args else None + return self.values.get(key) + + def put_nonblocking(self, key, value): + self.values[key] = value + + +def make_controller(curve_profile=None, curvature_data=None, weather_id=0, reduce_lat=0.0, road_curvature=0.02, driving_in_curve=False): + if curve_profile is None: + curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33)) + + planner = SimpleNamespace( + params=FakeParams({"CurvatureData": curvature_data} if curvature_data is not None else None), + curve_profile=curve_profile, + starpilot_weather=SimpleNamespace(weather_id=weather_id, reduce_lateral_acceleration=reduce_lat), + road_curvature=road_curvature, + driving_in_curve=driving_in_curve, + tracking_lead=False, + lateral_acceleration=0.0, + ) + controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner)) + return planner, controller + + +def make_sm(*, gas=False, brake=False, long_active=True, blinker=False, accel_pressed=False): + return { + "carControl": SimpleNamespace(longActive=long_active), + "carState": SimpleNamespace(gasPressed=gas, brakePressed=brake, leftBlinker=blinker, rightBlinker=False), + "starpilotCarState": SimpleNamespace(accelPressed=accel_pressed), + "onroadEvents": [], + } + + +def single_apex_profile(curvature, distance): + distances = np.linspace(0.0, max(distance * 1.5, 1.0), 33) + curvatures = np.zeros(33) + index = int(np.argmin(np.abs(distances - distance))) + distances[index] = distance + curvatures[index] = curvature + return curvatures, distances + + +def converge(controller, v_ego, v_cruise, frames=600): + for _ in range(frames): + controller.update_target(v_ego, v_cruise) + return controller.target + + +def envelope_speed(controller, curvature, distance): + curve_speed = max(float(np.sqrt(controller.lat_accel_for_curvature(curvature) / curvature)), CSC_MIN_SPEED) + return float(np.sqrt(curve_speed**2 + 2.0 * CSC_APPROACH_DECEL * distance)) + + +def test_straight_road_target_is_cruise_speed(): + _, controller = make_controller() + + controller.update_target(30.0, 30.0) + + assert controller.target == pytest.approx(30.0) + + +def test_distant_apex_does_not_constrain_until_braking_is_due(): + # derived from the shipped decel so retuning it doesn't silently invalidate the case + _, probe = make_controller() + curve_speed = max(float(np.sqrt(probe.lat_accel_for_curvature(0.02) / 0.02)), CSC_MIN_SPEED) + beyond_braking = 1.3 * (30.0**2 - curve_speed**2) / (2 * CSC_APPROACH_DECEL) + + _, controller = make_controller(curve_profile=single_apex_profile(0.02, beyond_braking)) + + target = converge(controller, 30.0, 30.0) + + assert target == pytest.approx(30.0) + + +def test_apex_in_braking_range_constrains_to_kinematic_envelope(): + _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0)) + + target = converge(controller, 30.0, 30.0) + + assert target == pytest.approx(envelope_speed(controller, 0.02, 150.0), abs=0.1) + assert target < 30.0 + + +def test_exit_recovery_rises_immediately_without_freeze(): + planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0)) + low_target = converge(controller, 15.0, 30.0) + assert low_target < 20.0 + + planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33)) + controller.update_target(15.0, 30.0) + assert controller.target > low_target # rises on the very next frame, no freeze + assert controller.target - low_target == pytest.approx(CSC_TARGET_UP_RATE * DT_MDL) + + # and it clears the car by the headroom within the time the up-rate needs + frames = int((15.0 + CSC_EGO_HEADROOM - controller.target) / (CSC_TARGET_UP_RATE * DT_MDL)) + 1 + for _ in range(frames): + controller.update_target(15.0, 30.0) + assert controller.target >= 15.0 + CSC_EGO_HEADROOM + + recovered = converge(controller, 15.0, 30.0) + assert recovered == pytest.approx(30.0) + + +def test_upward_jitter_in_the_envelope_is_rate_limited(): + # a sweeper the envelope only grazes: raw_target flicks between a mild cap and the + # set speed. The target must not chase the jumps, or the glow strobes. + planner, controller = make_controller(curve_profile=single_apex_profile(0.002, 40.0)) + steady = converge(controller, 30.0, 32.0) + assert steady < 32.0 + + flat = (np.zeros(33), np.linspace(0.0, 300.0, 33)) + grazing = planner.curve_profile + peak = steady + for i in range(40): + planner.curve_profile = flat if i % 2 else grazing + controller.update_target(30.0, 32.0) + assert controller.target - peak <= CSC_TARGET_UP_RATE * DT_MDL + 1e-6 + peak = controller.target + + +def test_firm_distant_curvature_is_corrected_for_the_model_under_read(): + # the model reads ~0.81x actual at range, so a firm distant bend binds later than it should + distance = 90.0 + _, plain = make_controller(curve_profile=single_apex_profile(0.0045, distance)) + _, probe = make_controller() + corrected = probe._correct_far_field(*single_apex_profile(0.0045, distance)) + + assert corrected.max() == pytest.approx(0.0045 * CSC_FARFIELD_GAIN) + assert converge(plain, 30.0, 30.0) < envelope_speed(plain, 0.0045, distance) + 1e-6 + + +def test_weak_or_near_readings_are_left_alone(): + _, probe = make_controller() + + # too weak to carry usable magnitude at range + weak = probe._correct_far_field(*single_apex_profile(0.002, 90.0)) + assert weak.max() == pytest.approx(0.002) + + # firm, but close enough that the model is already accurate + near = probe._correct_far_field(*single_apex_profile(0.0045, 10.0)) + assert near.max() == pytest.approx(0.0045) + + +def test_far_field_correction_brings_the_slowdown_forward(): + profile = single_apex_profile(0.0045, 120.0) + _, controller = make_controller(curve_profile=profile) + + corrected = converge(controller, 30.0, 30.0) + raw_curvatures, distances = profile + uncorrected = float(np.sqrt( + max(np.sqrt(controller.lat_accel_for_curvature(0.0045) / 0.0045), CSC_MIN_SPEED) ** 2 + + 2.0 * CSC_APPROACH_DECEL * 120.0)) + + assert corrected < uncorrected # binds sooner than the model's own reading would + + +def test_fresh_activation_seeds_at_envelope_not_cruise(): + _, controller = make_controller(curve_profile=(np.full(33, 0.05), np.linspace(0.0, 60.0, 33))) + + controller.update_target(6.0, 30.0) + + assert controller.target < 15.0 + + +def test_target_never_trails_accelerating_car_when_unconstrained(): + planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0)) + converge(controller, 15.0, 30.0) + + planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33)) + v_ego = 15.0 + caught_up = None + for frame in range(200): + v_ego = min(v_ego + 2.0 * DT_MDL, 30.0) + controller.update_target(v_ego, 30.0) + # the target climbs faster than the car can, so once it is ahead it stays ahead + if controller.target >= v_ego: + caught_up = caught_up if caught_up is not None else frame + assert caught_up is None or controller.target >= min(30.0, v_ego) - 1e-6 + + assert caught_up is not None and caught_up * DT_MDL < 2.0 + assert controller.target == pytest.approx(30.0) + + +def test_target_does_not_ratchet_down_with_ego_speed(): + _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0)) + target = converge(controller, 30.0, 30.0) + assert target > CSC_MIN_SPEED # a real curve speed, not floored + + controller.update_target(14.0, 30.0) + + assert controller.target == pytest.approx(target, abs=0.2) + + +def test_sharp_curve_target_floors_at_min_speed(): + _, controller = make_controller(curve_profile=(np.full(33, 0.1), np.linspace(0.0, 100.0, 33))) + + target = converge(controller, 15.0, 30.0) + + assert target == pytest.approx(CSC_MIN_SPEED, abs=0.05) + + +def test_weather_reduces_curve_speed(): + _, dry = make_controller(curve_profile=single_apex_profile(0.01, 0.0)) + _, wet = make_controller(curve_profile=single_apex_profile(0.01, 0.0), weather_id=1, reduce_lat=0.2) + + dry_target = converge(dry, 20.0, 30.0) + wet_target = converge(wet, 20.0, 30.0) + + assert wet_target < dry_target + assert wet_target == pytest.approx(dry_target * np.sqrt(0.8), abs=0.1) + + +def test_prior_gives_higher_lat_accel_for_sharper_curves(): + _, controller = make_controller() + + assert controller.learned_lat_accel(0.001) == pytest.approx(1.5, abs=0.05) + assert controller.learned_lat_accel(MAX_CURVATURE) > controller.learned_lat_accel(0.001) + assert controller.learned_lat_accel(MAX_CURVATURE) == pytest.approx( + float(np.interp(MAX_CURVATURE, PRIOR_CURVATURE_BP, PRIOR_LAT_ACCEL_V)), abs=0.05) + assert controller.lateral_acceleration == pytest.approx(DEFAULT_LATERAL_ACCELERATION) + + +def test_comfort_margin_matches_the_learned_habit(): + # margin is fixed at 1.0 -- CSC targets exactly the driver's own learned comfort + _, controller = make_controller() + + assert CSC_COMFORT_MARGIN == pytest.approx(1.0) + assert controller.lat_accel_for_curvature(0.01) == pytest.approx(controller.learned_lat_accel(0.01)) + + +def test_binding_distance_reports_the_constraining_point(): + _, controller = make_controller(curve_profile=single_apex_profile(0.02, 150.0)) + converge(controller, 30.0, 30.0) + + assert controller.binding_distance == pytest.approx(150.0, abs=1.0) + + +def test_binding_distance_is_zero_when_unconstrained(): + _, controller = make_controller() + converge(controller, 30.0, 30.0) + + assert controller.binding_distance == 0.0 + + +def test_heavily_sampled_bucket_dominates_prior(): + _, controller = make_controller(curvature_data={"0.05": {"average": 3.0, "count": 100000}}) + + assert controller.learned_lat_accel(0.05) == pytest.approx(3.0, abs=0.05) + assert controller.learned_lat_accel(0.08) >= controller.learned_lat_accel(0.05) + + +def test_learned_curve_stays_monotonic_despite_low_outlier_bucket(): + _, controller = make_controller(curvature_data={"0.05": {"average": 0.5, "count": 100000}}) + + assert controller.learned_lat_accel(0.05) >= controller.learned_lat_accel(0.03) + + +def test_dense_bucket_is_not_overridden_by_sparse_neighbour(): + # real device data: a running maximum ratcheted the 80-sample bucket up to the 20-sample neighbour + _, dense_low = make_controller(curvature_data={ + "0.003": {"average": 1.95, "count": 20}, + "0.005": {"average": 1.38, "count": 80}, + }) + _, dense_high = make_controller(curvature_data={ + "0.003": {"average": 1.95, "count": 80}, + "0.005": {"average": 1.38, "count": 20}, + }) + + assert dense_low.learned_lat_accel(0.005) < 1.95 # not ratcheted to the sparse neighbour + assert dense_low.learned_lat_accel(0.005) >= dense_low.learned_lat_accel(0.003) + # whichever side is better sampled should pull the fit: swapping the counts must raise it + assert dense_high.learned_lat_accel(0.005) > dense_low.learned_lat_accel(0.005) + + +def test_weighted_isotonic_pools_violators_by_weight(): + fitted = weighted_isotonic(np.array([1.0, 3.0, 1.2]), np.array([1.0, 1.0, 1000.0])) + + assert np.all(np.diff(fitted) >= -1e-9) + assert fitted[-1] == pytest.approx(1.2, abs=0.02) + + +def test_weighted_isotonic_leaves_sorted_input_untouched(): + values = np.array([1.0, 1.5, 2.0, 2.5]) + fitted = weighted_isotonic(values, np.ones(4)) + + assert fitted == pytest.approx(values) + + +def test_legacy_off_grid_curvature_data_merges_into_buckets(): + _, controller = make_controller(curvature_data={ + "0.0203": {"average": 2.5, "count": 10}, + "0.02": {"average": 2.0, "count": 10}, + }) + + assert controller.curvature_data["0.02"]["count"] == 20 + assert controller.curvature_data["0.02"]["average"] == pytest.approx(2.25) + + +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 = CSC_TRAINING_SETTLE_TIME + + controller.log_data(10.0, make_sm(long_active=False)) + + data = controller.curvature_data["0.02"] + assert data["count"] == 10001 + assert data["average"] == pytest.approx((2.0 * CSC_COUNT_CAP + 3.0) / (CSC_COUNT_CAP + 1)) + + +def test_no_passive_training_right_after_csc_limited_speed(): + planner, controller = make_controller(curve_profile=single_apex_profile(0.03, 20.0), driving_in_curve=True) + planner.lateral_acceleration = 3.0 + converge(controller, 15.0, 30.0) + assert controller.training_quiet_timer > 0.0 + + 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 = 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 settle_override(controller, sm=None, frames=None): + """Run the post-override watch out so the pseudo-sample is committed.""" + sm = sm if sm is not None else make_sm() + for _ in range(frames if frames is not None else int(CSC_OVERRIDE_WATCH_TIME / DT_MDL) + 1): + controller.handle_override(20.0, False, sm) + + +def test_gas_override_nudges_bucket_up_once_per_episode(): + _, controller = make_controller() + prior = controller.learned_lat_accel(0.02) + controller.target = 10.0 + + controller.handle_override(20.0, True, make_sm(gas=True)) + controller.handle_override(20.0, True, make_sm(gas=True)) + assert "0.02" not in controller.curvature_data # still watching what the driver holds + + settle_override(controller) + assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT + assert controller.curvature_data["0.02"]["average"] > prior + + controller.handle_override(20.0, False, make_sm()) + controller.target = 10.0 + controller.handle_override(20.0, True, make_sm(gas=True)) + settle_override(controller) + assert controller.curvature_data["0.02"]["count"] == 2 * CSC_NUDGE_WEIGHT + + +def test_override_learns_the_cornering_the_driver_actually_held(): + # the whole point: a fixed step needs several rejections to close a real disagreement, + # so record what they demonstrated instead + planner, observed = make_controller(driving_in_curve=True) + observed.target = 10.0 + observed.handle_override(20.0, True, make_sm(gas=True)) + planner.lateral_acceleration = 2.9 # they hold the curve much harder than CSC wanted + settle_override(observed, make_sm(gas=True)) + + _, stepped = make_controller(driving_in_curve=True) + stepped._apply_nudge(CSC_NUDGE) # what the old fixed-step path would have recorded + + assert observed.curvature_data["0.02"]["average"] == pytest.approx(2.9) + assert observed.curvature_data["0.02"]["average"] > stepped.curvature_data["0.02"]["average"] + assert observed.learned_lat_accel(0.02) > stepped.learned_lat_accel(0.02) + + +def test_override_on_a_straight_still_registers_the_fixed_step(): + planner, controller = make_controller() + prior = controller.learned_lat_accel(0.02) + controller.target = 10.0 + + controller.handle_override(20.0, True, make_sm(gas=True)) + planner.lateral_acceleration = 0.0 # never reached a corner + settle_override(controller) + + assert controller.curvature_data["0.02"]["average"] == pytest.approx(prior + CSC_NUDGE) + + +def test_res_button_nudges_bucket_up_even_at_target_speed(): + _, controller = make_controller() + prior = controller.learned_lat_accel(0.02) + controller.target = 20.0 # car tracking the target, so the gas-press condition would not fire + + controller.handle_override(20.0, True, make_sm(), accel_button=True) + settle_override(controller) + + assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT + assert controller.curvature_data["0.02"]["average"] > prior + + +def test_brake_override_nudges_bucket_down(): + _, controller = make_controller(driving_in_curve=True) + prior = controller.learned_lat_accel(0.02) + + controller.handle_override(20.0, True, make_sm(brake=True)) + + assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT + assert controller.curvature_data["0.02"]["average"] < prior + + +def test_calibrated_lateral_acceleration_param_is_written_on_flush(): + planner, controller = make_controller(curvature_data={"0.02": {"average": 2.8, "count": 5000}}) + + assert "CalibratedLateralAcceleration" not in planner.params.values + controller.flush_data() + + assert planner.params.values["CalibratedLateralAcceleration"] > DEFAULT_LATERAL_ACCELERATION + assert controller.lateral_acceleration == planner.params.values["CalibratedLateralAcceleration"] + + +def test_stale_param_from_a_previous_build_is_republished_without_training(): + # a stale value must not survive a restart just because this drive never trained + planner, controller = make_controller(curvature_data={"0.02": {"average": 2.8, "count": 5000}}) + planner.params.values["CalibratedLateralAcceleration"] = 3.71 + + controller.log_data(0.0, make_sm()) # standstill: ineligible -> flush path + + assert planner.params.values["CalibratedLateralAcceleration"] <= CSC_LAT_ACCEL_MAX diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 1006f0fd1..c2eadafa9 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -4,7 +4,8 @@ import pytest from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController, MIN_TRAINING_TIME +from openpilot.starpilot.common.starpilot_variables import PLANNER_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_CAP_SLACK_M, FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, @@ -53,6 +54,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False raw_model_stopped=raw_model_stopped, road_curvature=road_curvature, road_curvature_detected=False, + lateral_acceleration=0.0, ) vcruise = StarPilotVCruise(planner) vcruise.forcing_stop = forcing_stop @@ -82,12 +84,12 @@ def make_sm(*, standstill=True, min_steer_speed=0.0, car_fingerprint=""): } -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, @@ -145,30 +147,56 @@ def test_santa_fe_force_stop_tune_only_applies_to_that_car(): assert get_force_stop_low_speed_hold(other) is None -def test_curve_speed_controller_holds_target_through_brief_detector_dropout(): +def test_curve_speed_controller_blinker_releases_the_cap_but_keeps_the_plan(): planner, vcruise = make_vcruise() sm = make_sm(standstill=False) toggles = make_toggles() toggles.curve_speed_controller = True - def set_curve_target(_v_ego): - vcruise.csc.target_set = True + calls = [] + + def set_curve_target(_v_ego, _v_cruise): + calls.append(_v_ego) vcruise.csc.target = 14.0 vcruise.csc.update_target = set_curve_target - planner.road_curvature_detected = True result = update_vcruise(vcruise, sm, toggles, now=10.0, v_ego=20.0) assert result == pytest.approx(14.0) assert vcruise.csc_controlling_speed - planner.road_curvature_detected = False + # the cap lifts so CSC can't fight the lane change, but the envelope keeps planning + # so the curve doesn't have to be re-discovered from the set speed afterwards + sm["carState"].leftBlinker = True result = update_vcruise(vcruise, sm, toggles, now=10.25, v_ego=20.0) + assert result == pytest.approx(20.0) + assert not vcruise.csc_controlling_speed + assert len(calls) == 2 # still planning, so nothing has to be rediscovered + + # blinker off: the plan is already current, so the cap comes straight back + sm["carState"].leftBlinker = False + result = update_vcruise(vcruise, sm, toggles, now=10.5, v_ego=20.0) assert result == pytest.approx(14.0) assert vcruise.csc_controlling_speed - result = update_vcruise(vcruise, sm, toggles, now=10.8, v_ego=20.0) - assert result == pytest.approx(20.0) + +def test_curve_speed_controller_reseeds_after_a_real_dropout(): + 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 + update_vcruise(vcruise, sm, toggles, now=11.0, v_ego=20.0) + assert vcruise.csc_controlling_speed + + # disengaging is a real dropout, not a momentary veto -- that still resets + sm["carControl"].longActive = False + update_vcruise(vcruise, sm, toggles, now=11.05, v_ego=20.0) assert not vcruise.csc_controlling_speed + assert vcruise.csc.seed_pending def test_curve_speed_controller_releases_immediately_when_disabled(): @@ -177,53 +205,19 @@ def test_curve_speed_controller_releases_immediately_when_disabled(): toggles = make_toggles() toggles.curve_speed_controller = True - def set_curve_target(_v_ego): - vcruise.csc.target_set = True + def set_curve_target(_v_ego, _v_cruise): vcruise.csc.target = 14.0 vcruise.csc.update_target = set_curve_target - planner.road_curvature_detected = True update_vcruise(vcruise, sm, toggles, now=20.0, v_ego=20.0) assert vcruise.csc_controlling_speed - planner.road_curvature_detected = False toggles.curve_speed_controller = False result = update_vcruise(vcruise, sm, toggles, now=20.1, v_ego=20.0) assert result == pytest.approx(20.0) assert not vcruise.csc_controlling_speed -def test_curve_speed_controller_does_not_compete_with_force_stop(): - planner, vcruise = make_vcruise(red_light=True, road_curvature=0.001) - sm = make_sm(standstill=False) - toggles = make_toggles() - toggles.curve_speed_controller = True - planner.road_curvature_detected = True - vcruise.csc.target_set = True - vcruise.csc.target = 12.0 - - update_vcruise(vcruise, sm, toggles, now=25.0, v_ego=20.0) - - assert not vcruise.csc_controlling_speed - assert not vcruise.csc.target_set - - -def test_curve_speed_controller_learns_through_a_signaled_curve(): - planner, vcruise = make_vcruise(road_curvature=0.02) - sm = make_sm(standstill=False) - sm["carControl"].longActive = False - sm["carState"].leftBlinker = True - planner.driving_in_curve = True - planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = MIN_TRAINING_TIME - - vcruise.csc.log_data(20.0, sm) - - assert vcruise.csc.enable_training - assert vcruise.csc.curvature_data["0.02"]["count"] == 1 - - - def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead(): planner, vcruise = make_vcruise() sm = make_sm(standstill=False) @@ -231,12 +225,10 @@ def test_curve_speed_controller_can_be_limited_to_driving_without_a_lead(): toggles.curve_speed_controller = True toggles.csc_no_lead = True - def set_curve_target(_v_ego): - vcruise.csc.target_set = True + def set_curve_target(_v_ego, _v_cruise): vcruise.csc.target = 14.0 vcruise.csc.update_target = set_curve_target - planner.road_curvature_detected = True result = update_vcruise(vcruise, sm, toggles, now=30.0, v_ego=20.0) assert result == pytest.approx(14.0) @@ -254,10 +246,8 @@ def test_curve_speed_controller_stays_enabled_with_a_lead_by_default(): toggles = make_toggles() toggles.curve_speed_controller = True planner.starpilot_following.following_lead = True - planner.road_curvature_detected = True - def set_curve_target(_v_ego): - vcruise.csc.target_set = True + def set_curve_target(_v_ego, _v_cruise): vcruise.csc.target = 14.0 vcruise.csc.update_target = set_curve_target @@ -281,7 +271,7 @@ def test_curve_speed_controller_learns_when_speed_is_manually_controlled(long_ac planner.driving_in_curve = True planner.road_curvature_detected = True planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = MIN_TRAINING_TIME + vcruise.csc.training_timer = PLANNER_TIME update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0) @@ -299,7 +289,7 @@ def test_curve_speed_controller_learns_when_longitudinal_override_event_is_activ planner.driving_in_curve = True planner.road_curvature_detected = True planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = MIN_TRAINING_TIME + vcruise.csc.training_timer = PLANNER_TIME update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0) @@ -313,7 +303,7 @@ def test_curve_speed_controller_persists_data_after_leaving_curve(): sm["carControl"].longActive = False planner.driving_in_curve = True planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = MIN_TRAINING_TIME + vcruise.csc.training_timer = PLANNER_TIME vcruise.csc.log_data(20.0, sm) assert not any(key == "CurvatureData" for key, _ in planner.params.writes) @@ -324,54 +314,276 @@ def test_curve_speed_controller_persists_data_after_leaving_curve(): assert any(key == "CurvatureData" for key, _ in planner.params.writes) -def test_curve_speed_controller_publishes_live_values_to_memory_params(): - planner, vcruise = make_vcruise(road_curvature=0.02) +def test_csc_res_press_cancels_for_episode_and_rearms(): + planner, vcruise = make_vcruise() sm = make_sm(standstill=False) - sm["carControl"].longActive = 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 + result = update_vcruise(vcruise, sm, toggles, now=60.0, v_ego=20.0) + assert result == pytest.approx(14.0) + assert vcruise.csc_controlling_speed + + sm["starpilotCarState"].accelPressed = True + result = update_vcruise(vcruise, sm, toggles, now=60.05, v_ego=20.0) + assert result == pytest.approx(20.0) + assert not vcruise.csc_controlling_speed + assert vcruise.csc_override + + # latches for the rest of the episode, not just while pressed + sm["starpilotCarState"].accelPressed = False + result = update_vcruise(vcruise, sm, toggles, now=60.1, v_ego=20.0) + assert result == pytest.approx(20.0) + assert vcruise.csc_override + + # curve ends -> re-arms + curve_target["v"] = 20.0 + update_vcruise(vcruise, sm, toggles, now=60.15, v_ego=20.0) + assert not vcruise.csc_override + + curve_target["v"] = 14.0 + result = update_vcruise(vcruise, sm, toggles, now=60.2, v_ego=20.0) + assert result == pytest.approx(14.0) + assert vcruise.csc_controlling_speed + + +def test_csc_res_press_does_not_latch_when_csc_was_not_active(): + 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 + # press before CSC ever limited: suspends it while held, but must not latch a cancel + sm["starpilotCarState"].accelPressed = True + result = update_vcruise(vcruise, sm, toggles, now=70.0, v_ego=20.0) + assert result == pytest.approx(20.0) + assert not vcruise.csc_override + + sm["starpilotCarState"].accelPressed = False + result = update_vcruise(vcruise, sm, toggles, now=70.05, v_ego=20.0) + assert result == pytest.approx(14.0) + assert vcruise.csc_controlling_speed + + +def test_csc_res_press_defers_to_slc_confirmation(): + 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 + update_vcruise(vcruise, sm, toggles, now=80.0, v_ego=20.0) + assert vcruise.csc_controlling_speed + + # confirming a speed limit must not also cancel the curve slowdown + vcruise.slc.speed_limit_changed_timer = 1.0 + vcruise.slc.unconfirmed_speed_limit = 25.0 + sm["starpilotCarState"].accelPressed = True + update_vcruise(vcruise, sm, toggles, now=80.05, v_ego=20.0) + assert not vcruise.csc_override + + sm["starpilotCarState"].accelPressed = False + result = update_vcruise(vcruise, sm, toggles, now=80.1, v_ego=20.0) + assert result == pytest.approx(14.0) + 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) + 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 - planner.lateral_acceleration = 2.4 - vcruise.csc.training_timer = MIN_TRAINING_TIME + 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 - vcruise.csc.log_data(20.0, sm) - - assert any(key == "CalibratedLateralAcceleration" for key, _ in planner.params_memory.writes) - assert any(key == "CalibrationProgress" for key, _ in planner.params_memory.writes) - assert planner.params_memory.values["CalibrationProgress"] > 0.0 + # 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_ramps_toward_curve_speed_at_bounded_rate(): - planner = SimpleNamespace( - params=FakeParams(), - road_curvature=0.004, - time_to_curve=2.0, - starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0), - ) - controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner)) - controller.lateral_acceleration = 2.0 - controller.target_set = True - controller.target = 30.0 +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 - controller.update_target(30.0) + # 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 - assert controller.target == pytest.approx(30.0 - CSC_MAX_DECEL_RATE * DT_MDL) - assert controller.target > (controller.lateral_acceleration / planner.road_curvature) ** 0.5 + 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_does_not_slow_for_curve_speed_above_ego(): - planner = SimpleNamespace( - params=FakeParams(), - road_curvature=0.001, - time_to_curve=2.0, - starpilot_weather=SimpleNamespace(weather_id=0, reduce_lateral_acceleration=0.0), - ) - controller = CurveSpeedController(SimpleNamespace(starpilot_planner=planner)) - controller.lateral_acceleration = 2.0 - controller.target_set = True - controller.target = 28.0 +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 - controller.update_target(30.0) + # 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 + + # fully released, but the glow only clears once the release has stuck + curve_target["v"] = 20.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 + + +def test_curve_speed_controller_hysteresis_keeps_glow_off_for_marginal_targets(): + 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 = 19.7 + + vcruise.csc.update_target = set_curve_target + result = update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0) + + assert result == pytest.approx(19.7) + assert not vcruise.csc_controlling_speed - assert controller.target == pytest.approx(30.0) def test_active_slc_control_target_applies_offset_and_cluster_diff(): diff --git a/selfdrive/ui/onroad/starpilot/starpilot_border.py b/selfdrive/ui/onroad/starpilot/starpilot_border.py index c1d49f2fc..f5019a930 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_border.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_border.py @@ -59,6 +59,8 @@ def _csc_state(): plan = sm["starpilotPlan"] params = ui_state.ui_params + # A pending speed limit flashes the speed limit sign, not the border -- it has no reason + # to blank this, and doing so hid real curve slowdowns for the whole confirmation window. if not params.get_bool("ShowCSCStatus"): return None diff --git a/starpilot/common/starpilot_utilities.py b/starpilot/common/starpilot_utilities.py index efd3f3ce3..804a0302e 100644 --- a/starpilot/common/starpilot_utilities.py +++ b/starpilot/common/starpilot_utilities.py @@ -138,6 +138,23 @@ def calculate_road_curvature(modelData, v_ego): return float(predicted_lateral_acc / max(v_ego, 1)**2), max(time_to_curve, 1) +PROFILE_MIN_SPEED = 3.0 # m/s — model points planned near standstill have unusable curvature +PROFILE_MAX_CURVATURE = 0.1 + + +def extract_curve_profile(modelData): + orientation_rate = np.abs(np.array(modelData.orientationRate.z)) + velocity = np.array(modelData.velocity.x) + distances = np.array(modelData.position.x) + + # k = psi_dot / v per point, against the model's own planned speed so its + # slowdowns don't inflate the curvature + curvatures = orientation_rate / np.clip(velocity, PROFILE_MIN_SPEED, None) + curvatures = np.where(velocity < PROFILE_MIN_SPEED, 0.0, np.minimum(curvatures, PROFILE_MAX_CURVATURE)) + + return curvatures, distances + + def clean_model_name(name): return name.replace("(Default)", "").strip() diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index e1bce48ae..a232eef29 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -2,19 +2,97 @@ import numpy as np from openpilot.common.constants import CV +from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED, DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME +from openpilot.starpilot.common.starpilot_variables import ( + CITY_SPEED_LIMIT, + CRUISING_SPEED, + DEFAULT_LATERAL_ACCELERATION, + PLANNER_TIME, +) CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL -MIN_TRAINING_TIME = 5.0 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS -CSC_MAX_DECEL_RATE = 1.5 -MAX_CURVATURE = 0.1 -MIN_CURVATURE = 0.001 -PERCENTILE = 90 -ROUNDING_PRECISION = 5 -STEP = 0.001 + +# braking distance is (v^2 - v_curve^2) / (2 * this), so lower starts the slowdown +# sooner and spreads it further. +CSC_APPROACH_DECEL = 0.3 +CSC_TARGET_UP_RATE = 3.0 +CSC_TARGET_DOWN_RATE = 2.5 +CSC_TARGET_FILTER_RC = 0.4 +CSC_EGO_HEADROOM = 2.0 # target never trails below v_ego, so CSC can't drag re-acceleration +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 (training) so a trivial graze doesn't light the glow +CSC_GLOW_HOLD_TIME = 3.0 # s the cap must stay released before the glow clears, so it doesn't flicker on/off across 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 +CSC_LAT_ACCEL_MIN = 1.2 +CSC_LAT_ACCEL_MAX = 3.2 +CSC_NUDGE = 0.15 +CSC_NUDGE_WEIGHT = 20 # counts a single override pseudo-sample is worth +CSC_OVERRIDE_WATCH_TIME = 6.0 # s to keep watching what the driver holds after they reject a cut +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 +CSC_COMFORT_MARGIN = 1.0 # 1.0 = matches the driver's own learned cornering, no extra cushion + +# The model under-reads curvature at range: measured 0.81x actual beyond ~75 m. That holds +# only where the reading is already firm -- weak distant readings carry no usable magnitude +# (0.40x median with a 14:1 spread), so scaling those would amplify noise, not signal. +CSC_FARFIELD_MIN_CURVATURE = 0.004 # ~R 250 m; at this strength range readings were 85%+ reliable +CSC_FARFIELD_MIN_DISTANCE = 30.0 # inside this the model is already accurate +CSC_FARFIELD_GAIN = 1.23 # 1 / 0.81 + +# Buckets are spaced geometrically, not linearly: comfort is a speed and v = sqrt(a/k), so equal +# steps in k give wildly uneven speed resolution. Regridding is safe -- _normalize_curvature_data +# re-buckets stored keys on load. +MIN_CURVATURE = 0.0005 # R 2000 m — gentler than this never constrains anything +MAX_CURVATURE = 0.02 # R 50 m — already well below the CSC_MIN_SPEED floor +CURVATURE_BUCKETS = 24 # keeps every bucket under ~7 mph wide without over-thinning the data +ROUNDING_PRECISION = 6 +CURVATURE_GRID = MIN_CURVATURE * np.power(MAX_CURVATURE / MIN_CURVATURE, + np.arange(CURVATURE_BUCKETS) / (CURVATURE_BUCKETS - 1)) +LOG_CURVATURE_GRID = np.log(CURVATURE_GRID) + +# Drivers accept more lateral acceleration in sharp slow corners than in highway sweepers. +PRIOR_CURVATURE_BP = [0.001, 0.003, 0.01, 0.03, 0.1] +PRIOR_LAT_ACCEL_V = [1.5, 1.8, 2.2, 2.6, 2.9] + + +def weighted_isotonic(values, weights): + """Weighted non-decreasing fit (pool adjacent violators). + + Keeps comfort from falling as curves tighten, without letting a sparse bucket + overrule a well-sampled neighbour the way a running maximum would. + """ + block_values: list[float] = [] + block_weights: list[float] = [] + block_sizes: list[int] = [] + + for value, weight in zip(values, weights, strict=True): + block_values.append(float(value)) + block_weights.append(float(weight)) + block_sizes.append(1) + + while len(block_values) > 1 and block_values[-2] > block_values[-1]: + merged_weight = block_weights[-2] + block_weights[-1] + merged_value = ((block_values[-2] * block_weights[-2]) + (block_values[-1] * block_weights[-1])) / merged_weight + block_values.pop() + block_weights.pop() + merged_size = block_sizes.pop() + block_values[-1] = merged_value + block_weights[-1] = merged_weight + block_sizes[-1] += merged_size + + fitted = np.empty(len(values)) + index = 0 + for value, size in zip(block_values, block_sizes, strict=True): + fitted[index:index + size] = value + index += size + return fitted def is_user_overriding_longitudinal(sm): @@ -43,26 +121,45 @@ class CurveSpeedController: self.starpilot_planner = StarPilotVCruise.starpilot_planner self.enable_training = False - self.target_set = False + self.nudge_applied = False + + self.override_watch_key = None + self.override_watch_peak = 0.0 + self.override_watch_timer = 0.0 self.training_timer = 0.0 self.persistence_timer = 0.0 + self.training_quiet_timer = 0.0 self.data_dirty = False + self.target = 0.0 + self.binding_distance = 0.0 + self.release_timer = 0.0 + self.target_filter = FirstOrderFilter(0.0, CSC_TARGET_FILTER_RC, DT_MDL, initialized=False) + self.seed_pending = True + + self._long_active_prev = False + curvature_data = self.starpilot_planner.params.get("CurvatureData") self.curvature_data = self._normalize_curvature_data(curvature_data) - self.required_curvatures = [str(round(road_curvature, ROUNDING_PRECISION)) for road_curvature in np.arange(MIN_CURVATURE, MAX_CURVATURE + STEP, STEP)] + # built through the bucketer so the keys are byte-identical to what training writes + self.required_curvatures = [self._bucket_curvature(curvature) for curvature in CURVATURE_GRID] - self.update_lateral_acceleration() - self._publish_calibration_progress(persist=True) + self.rebuild_lat_accel_curve() + # publish on the first flush even if this drive never trains, or the readout + # keeps showing whatever a previous build left behind + self.data_dirty = True + # the Settings screen reads the memory param for a live readout -- seed it now, or it + # shows nothing until the first disk flush + self._publish_live_values() @staticmethod def _bucket_curvature(road_curvature): - clipped_curvature = float(np.clip(road_curvature, MIN_CURVATURE, MAX_CURVATURE)) - bucket_index = round((clipped_curvature - MIN_CURVATURE) / STEP) - bucketed_curvature = MIN_CURVATURE + (bucket_index * STEP) - return str(round(bucketed_curvature, ROUNDING_PRECISION)) + clipped_curvature = float(np.clip(abs(road_curvature), MIN_CURVATURE, MAX_CURVATURE)) + # nearest in log space, so a bucket is a constant speed step rather than a constant radius one + bucket_index = int(np.argmin(np.abs(LOG_CURVATURE_GRID - np.log(clipped_curvature)))) + return str(round(float(CURVATURE_GRID[bucket_index]), ROUNDING_PRECISION)) @classmethod def _normalize_curvature_data(cls, curvature_data): @@ -100,17 +197,6 @@ class CurveSpeedController: return normalized - def _persist_data(self): - if not self.data_dirty: - return - - progress = self._calibration_progress() - self.starpilot_planner.params.put_nonblocking("CalibrationProgress", progress) - self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data) - self._put_memory_param("CalibrationProgress", progress) - self.data_dirty = False - self.persistence_timer = 0.0 - def _calibration_progress(self): progress = 0.0 for key in self.required_curvatures: @@ -118,31 +204,48 @@ class CurveSpeedController: progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0) return (progress / len(self.required_curvatures)) * 100 - def _publish_calibration_progress(self, persist=False): - progress = self._calibration_progress() - if persist: - self.starpilot_planner.params.put_nonblocking("CalibrationProgress", progress) - self._put_memory_param("CalibrationProgress", progress) - - def _put_memory_param(self, key, value): + def _publish_live_values(self, progress=None): + # memory-only and cheap, so this can run every frame training touches the data -- + # it's what the on-device Settings screen reads for a live readout between disk flushes params_memory = getattr(self.starpilot_planner, "params_memory", None) - if params_memory is not None: - params_memory.put_nonblocking(key, value) + if params_memory is None: + return + if progress is None: + progress = self._calibration_progress() + params_memory.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration) + params_memory.put_nonblocking("CalibrationProgress", progress) + + def _persist_data(self): + if not self.data_dirty: + return + + progress = self._calibration_progress() + self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration) + self.starpilot_planner.params.put_nonblocking("CalibrationProgress", progress) + self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data) + self._publish_live_values(progress) + self.data_dirty = False + self.persistence_timer = 0.0 def flush_data(self): self._persist_data() def log_data(self, v_ego, sm): + self.training_quiet_timer = max(self.training_quiet_timer - DT_MDL, 0.0) + eligible = ( v_ego > CRUISING_SPEED and not self.starpilot_planner.tracking_lead and - is_manual_speed_control(sm) + is_manual_speed_control(sm) and + self.training_quiet_timer <= 0.0 ) self.enable_training = False 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 @@ -151,8 +254,9 @@ class CurveSpeedController: self.persistence_timer += DT_MDL in_curve = ( - self.training_timer >= MIN_TRAINING_TIME and - self.starpilot_planner.driving_in_curve + self.training_timer >= CSC_TRAINING_SETTLE_TIME and + self.starpilot_planner.driving_in_curve and + not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) ) if in_curve: lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration) @@ -160,11 +264,11 @@ class CurveSpeedController: if road_curvature in self.curvature_data: data = self.curvature_data[road_curvature] - average = data["average"] - count = data["count"] + # capped so an established bucket still tracks a change in driving style + effective_count = min(data["count"], CSC_COUNT_CAP) self.curvature_data[road_curvature] = { - "average": ((average * count) + lateral_acceleration) / (count + 1), - "count": count + 1 + "average": ((data["average"] * effective_count) + lateral_acceleration) / (effective_count + 1), + "count": data["count"] + 1 } else: self.curvature_data[road_curvature] = { @@ -173,8 +277,8 @@ class CurveSpeedController: } self.data_dirty = True - self.update_lateral_acceleration() - self._publish_calibration_progress() + self.rebuild_lat_accel_curve() + self._publish_live_values() self.enable_training = True if self.persistence_timer >= PLANNER_TIME: @@ -182,30 +286,166 @@ class CurveSpeedController: elif self.data_dirty: self.flush_data() - def update_lateral_acceleration(self): - if self.curvature_data: - all_samples = [data["average"] for data in self.curvature_data.values()] - self.lateral_acceleration = float(np.percentile(all_samples, PERCENTILE)) + def handle_override(self, v_ego, was_controlling, sm, accel_button=False): + long_active = bool(sm["carControl"].longActive) + long_dropped = self._long_active_prev and not long_active + self._long_active_prev = long_active + + self._update_override_watch(sm) + + if not was_controlling: + self.nudge_applied = False + return + + if self.nudge_applied: + return + + if accel_button or (sm["carState"].gasPressed and self.target < v_ego - 0.5): + # Watch what the driver actually holds instead of stepping by a fixed amount -- CSC is + # suspended while overridden, so their cornering now measures their real comfort. + self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) + self.override_watch_peak = abs(self.starpilot_planner.lateral_acceleration) + self.override_watch_timer = CSC_OVERRIDE_WATCH_TIME + self.nudge_applied = True + elif (getattr(sm["carState"], "brakePressed", False) or long_dropped) and self.starpilot_planner.driving_in_curve: + self._apply_nudge(-CSC_NUDGE) + + def _update_override_watch(self, sm): + if self.override_watch_key is None: + return + + lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration) + if lateral_acceleration > self.override_watch_peak: + # credit the bucket the peak actually happened in, not the one at the button press + self.override_watch_peak = lateral_acceleration + self.override_watch_key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) + + self.override_watch_timer -= DT_MDL + if self.override_watch_timer > 0.0 and (is_user_overriding_longitudinal(sm) or + self.starpilot_planner.driving_in_curve): + return + + key = self.override_watch_key + self.override_watch_key = None + # floored at the old fixed step, so a rejection that never reaches a corner still counts + # and this path can only ever raise the bucket + self._record_pseudo_sample(key, max(self.override_watch_peak, + self.learned_lat_accel(float(key)) + CSC_NUDGE)) + + def _apply_nudge(self, offset): + key = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) + # relative to the learned value, not the margined one, or repeated overrides walk the bucket down + self._record_pseudo_sample(key, self.learned_lat_accel(float(key)) + offset) + self.nudge_applied = True + + def _record_pseudo_sample(self, key, sample): + sample = float(np.clip(sample, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX)) + + data = self.curvature_data.get(key, {"average": sample, "count": 0}) + effective_count = min(data["count"], CSC_COUNT_CAP) + total = effective_count + CSC_NUDGE_WEIGHT + self.curvature_data[key] = { + "average": ((data["average"] * effective_count) + (sample * CSC_NUDGE_WEIGHT)) / total, + "count": data["count"] + CSC_NUDGE_WEIGHT, + } + + self.rebuild_lat_accel_curve() + self.data_dirty = True + self.flush_data() + + def rebuild_lat_accel_curve(self): + grid_k = np.array([float(key) for key in self.required_curvatures]) + prior = np.interp(grid_k, PRIOR_CURVATURE_BP, PRIOR_LAT_ACCEL_V) + + blended = prior.copy() + counts = np.zeros(len(grid_k)) + for i, key in enumerate(self.required_curvatures): + data = self.curvature_data.get(key) + if data: + confidence = data["count"] / (data["count"] + CSC_PRIOR_COUNT) + blended[i] = confidence * data["average"] + (1.0 - confidence) * prior[i] + counts[i] = data["count"] + + blended = np.clip(blended, CSC_LAT_ACCEL_MIN, CSC_LAT_ACCEL_MAX) + blended = weighted_isotonic(blended, counts + CSC_PRIOR_COUNT) + + self._curve_k = grid_k + self._curve_a = blended + + if counts.sum() > 0: + self.lateral_acceleration = float(np.average(blended, weights=counts)) else: self.lateral_acceleration = DEFAULT_LATERAL_ACCELERATION - self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration) - self._put_memory_param("CalibratedLateralAcceleration", self.lateral_acceleration) + def learned_lat_accel(self, curvature): + """Comfort level learned for this curvature, before any control margin.""" + return float(np.interp(abs(curvature), self._curve_k, self._curve_a)) - def update_target(self, v_ego): - lateral_acceleration = self.lateral_acceleration - if self.starpilot_planner.starpilot_weather.weather_id != 0: - lateral_acceleration -= self.lateral_acceleration * self.starpilot_planner.starpilot_weather.reduce_lateral_acceleration + def lat_accel_for_curvature(self, curvature): + lat_accel = np.interp(np.abs(curvature), self._curve_k, self._curve_a) * CSC_COMFORT_MARGIN - if self.target_set: - csc_speed = (lateral_acceleration / abs(self.starpilot_planner.road_curvature))**0.5 - csc_speed = max(float(csc_speed), CSC_MIN_SPEED) - if csc_speed >= v_ego: - self.target = v_ego - else: - time_to_curve = max(float(self.starpilot_planner.time_to_curve), DT_MDL) - decel_rate = float(np.clip((v_ego - csc_speed) / time_to_curve, 0.0, CSC_MAX_DECEL_RATE)) - self.target = float(np.clip(self.target - decel_rate * DT_MDL, csc_speed, v_ego)) + weather = self.starpilot_planner.starpilot_weather + if weather.weather_id != 0: + lat_accel = lat_accel * (1.0 - weather.reduce_lateral_acceleration) + + return lat_accel + + @staticmethod + def _correct_far_field(curvatures, distances): + """Undo the model's known under-read of distant curvature, where the reading is firm.""" + firm = (curvatures >= CSC_FARFIELD_MIN_CURVATURE) & (distances >= CSC_FARFIELD_MIN_DISTANCE) + return np.minimum(np.where(firm, curvatures * CSC_FARFIELD_GAIN, curvatures), MAX_CURVATURE) + + def reset(self, v_cruise): + self.target = float(v_cruise) + self.release_timer = 0.0 + self.target_filter.x = float(v_cruise) + self.target_filter.initialized = True + self.seed_pending = True + + def update_target(self, v_ego, v_cruise): + if not self.target_filter.initialized: + self.reset(v_cruise) + + curvatures, distances = self.starpilot_planner.curve_profile + if len(curvatures) == 0: + raw_target = float(v_cruise) + self.binding_distance = 0.0 else: - self.target_set = True - self.target = v_ego + curvatures = self._correct_far_field(curvatures, distances) + lat_accel = self.lat_accel_for_curvature(curvatures) + point_speeds = np.sqrt(lat_accel / np.maximum(curvatures, 1e-4)) + point_speeds = np.maximum(point_speeds, CSC_MIN_SPEED) + allowed_speeds = np.sqrt(point_speeds**2 + 2.0 * CSC_APPROACH_DECEL * np.maximum(distances, 0.0)) + binding_index = int(np.argmin(allowed_speeds)) + raw_target = min(float(allowed_speeds[binding_index]), float(v_cruise)) + self.binding_distance = float(distances[binding_index]) if raw_target < v_cruise else 0.0 + + # a fresh activation starts at the envelope, or it spends seconds ramping down + # toward a curve it already sees (engaging or launching into a turn) + if self.seed_pending: + seed = min(float(v_cruise), max(raw_target, v_ego + CSC_EGO_HEADROOM)) + self.target = seed + self.target_filter.x = seed + self.seed_pending = False + + if raw_target >= v_ego: + self.release_timer += DT_MDL + else: + self.release_timer = 0.0 + + # The headroom aim goes through the rate limiter with everything else; applying it + # after the clamp let every upward jitter in raw_target reach the target unsmoothed. + filtered = self.target_filter.update(raw_target) + self.target = float(np.clip(max(filtered, min(raw_target, v_ego + CSC_EGO_HEADROOM)), + self.target - CSC_TARGET_DOWN_RATE * DT_MDL, + self.target + CSC_TARGET_UP_RATE * DT_MDL)) + + # Once the envelope really has released, the target must not sit under the car or it + # drags re-acceleration. Debounced, because a single jittery frame doing this yanks a + # legitimate cut back up to v_ego and strobes the glow on sweepers. + if self.release_timer >= CSC_RELEASE_DEBOUNCE: + self.target = max(self.target, min(raw_target, v_ego)) + + if self.target < v_cruise - CSC_ACTIVE_ON_DELTA: + self.training_quiet_timer = CSC_TRAINING_QUIET_TIME diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index d77180a1c..3c5325c05 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -6,7 +6,13 @@ from openpilot.common.constants import CV 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 CurveSpeedController, is_manual_speed_control +from openpilot.starpilot.controls.lib.curve_speed_controller import ( + CSC_ACTIVE_OFF_DELTA, + CSC_GLOW_HOLD_TIME, + CSC_GLOW_ON_DELTA, + CurveSpeedController, + is_manual_speed_control, +) from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_force_stop_distance_bias, @@ -16,7 +22,6 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( ) CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS -CSC_CURVE_RELEASE_HOLD_TIME = 0.75 OVERRIDE_FORCE_STOP_TIMER = 10 STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75 # Open-loop — green is undetectable at standstill, so this only needs to cover the @@ -202,8 +207,9 @@ 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 - self.csc_curve_last_seen_at = None def _update_nav_instruction_state(self): raw = self.starpilot_planner.params_memory.get("NavInstructionState") or {} @@ -571,28 +577,62 @@ class StarPilotVCruise: starpilot_toggles.curve_speed_controller and (not getattr(starpilot_toggles, "csc_no_lead", False) or not following_lead) ) - csc_curve_detected = csc_available and self.starpilot_planner.road_curvature_detected - if csc_curve_detected: - self.csc.update_target(v_ego) + # The blinker veto is for lane changes/turns, not for an already-real curve -- releasing it + # there let the car accelerate into the bend, then claw the speed back once the blinker cleared. + 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 + csc_accel_button = bool(sm["starpilotCarState"].accelPressed) and not slc_confirmation_pending - self.csc_controlling_speed = True - self.csc_target = self.csc.target - self.csc_curve_last_seen_at = now - else: - csc_release_hold = bool( - csc_available and - self.csc_controlling_speed and - self.csc_curve_last_seen_at is not None and - self._elapsed_seconds(now, self.csc_curve_last_seen_at) < CSC_CURVE_RELEASE_HOLD_TIME - ) - if not csc_release_hold: - self.csc.log_data(v_ego, sm) + # Latched outside the availability branch: the press itself suspends CSC this frame, so + # latching inside it would never see the press, and the slowdown would return on release. + if csc_was_controlling and csc_accel_button: + self.csc_override = True + if not (long_control_active and starpilot_toggles.curve_speed_controller): + self.csc_override = False + if csc_available and not csc_blinker_on: + self.csc.update_target(v_ego, v_cruise) + + if self.csc_override and self.csc.target > v_cruise - CSC_ACTIVE_OFF_DELTA: + self.csc_override = False + + if self.csc_override: self.csc_controlling_speed = False - self.csc.target_set = False - self.csc_curve_last_seen_at = None - + self.csc_glow_release_timer = 0.0 self.csc_target = v_cruise + else: + self.csc_target = self.csc.target + # A low target alone means nothing until the car has actually reached it (slowed down + # to it, or accelerated up into it). Release still waits for the set speed, so the glow + # spans the hold and the recovery, not just the braking. + 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: + # 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: + # Release the cap so CSC can't fight the lane change, but keep planning -- resetting here + # threw the braking plan away and re-planned from the set speed with the curve closer. + 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) + self.csc.log_data(v_ego, sm) # Pfeiferj's Speed Limit Controller self.slc.starpilot_toggles = starpilot_toggles diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 908394483..bf40fa3ff 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -20,7 +20,7 @@ from openpilot.selfdrive.controls.lib.lead_behavior import ( from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST, STOP_DISTANCE from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_lead_follow_jerk_scale -from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature +from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature, extract_curve_profile from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME, THRESHOLD from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode @@ -214,6 +214,7 @@ class StarPilotPlanner: self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop self.road_curvature, self.time_to_curve = calculate_road_curvature(sm["modelV2"], v_ego) + self.curve_profile = extract_curve_profile(sm["modelV2"]) self.road_curvature_detected = (1 / abs(self.road_curvature))**0.5 < v_ego > CRUISING_SPEED and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) @@ -328,6 +329,9 @@ class StarPilotPlanner: starpilotPlan.cscControllingSpeed = self.starpilot_vcruise.csc_controlling_speed starpilotPlan.cscSpeed = float(self.starpilot_vcruise.csc_target) starpilotPlan.cscTraining = self.starpilot_vcruise.csc.enable_training + starpilotPlan.cscOverridden = self.starpilot_vcruise.csc_override + starpilotPlan.cscLearnedLatAccel = float(self.starpilot_vcruise.csc.learned_lat_accel(self.road_curvature)) + starpilotPlan.cscBindingDistance = float(self.starpilot_vcruise.csc.binding_distance) starpilotPlan.desiredFollowDistance = int(self.starpilot_following.desired_follow_distance) starpilotPlan.disableThrottle = ( diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings.css b/starpilot/system/the_galaxy/assets/components/tools/device_settings.css index 074600d98..06ce82688 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings.css +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings.css @@ -384,6 +384,7 @@ padding: 0.2rem 0.6rem; } +/* read-only: no border, since there is nothing here to click or edit */ .ds-row-readout { background-color: transparent; border: none; diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js index f7ca579d1..53fa0c6ae 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js @@ -484,11 +484,11 @@ function formatSliderValue(val, stepStr, precisionInt, key) { function formatReadoutValue(p) { const raw = state.values[p.key] - const value = parseFloat(raw) - if (raw === undefined || raw === null || Number.isNaN(value)) return "--" + const v = parseFloat(raw) + if (raw === undefined || raw === null || Number.isNaN(v)) return "--" const precision = p.precision !== undefined && p.precision !== null ? Number(p.precision) : 2 - const formatted = Number(value.toFixed(Math.max(0, precision))).toString() + const formatted = Number(v.toFixed(Math.max(0, precision))).toString() return p.unit ? `${formatted}${p.unit}` : formatted } diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index dfef5e33e..37c42c599 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -6136,6 +6136,16 @@ def setup(app): result["VehicleParked"] = _get_vehicle_parked() result["AlphaLongitudinalAvailable"] = _get_alpha_longitudinal_available() result["HasRivianAngleHarness"] = _get_has_rivian_angle_harness() + # read-only: excluded from allowed_keys (and so from the write paths) but still + # worth surfacing as a display-only readout + try: + result["CalibratedLateralAcceleration"] = _get_current_param_value("CalibratedLateralAcceleration", float, defaults_lookup) + except Exception: + result["CalibratedLateralAcceleration"] = None + try: + result["CalibrationProgress"] = _get_current_param_value("CalibrationProgress", float, defaults_lookup) + except Exception: + result["CalibrationProgress"] = None for key in ("CalibratedLateralAcceleration", "CalibrationProgress"): try: diff --git a/tools/longitudinal/analyze_csc.py b/tools/longitudinal/analyze_csc.py new file mode 100644 index 000000000..c26c4ad41 --- /dev/null +++ b/tools/longitudinal/analyze_csc.py @@ -0,0 +1,295 @@ +#!/usr/bin/env python3 +"""Curve Speed Controller field report: does it cut the lateral-accel tail, how +often does it engage, and how often do drivers reject it. + +Usage: + ./analyze_csc.py # e.g. a1b2c3d4e5f6g7h8|2026-08-14--10-30-00 + ./analyze_csc.py [ ...] + ./analyze_csc.py --json report.json +""" +from __future__ import annotations + +import argparse +import json +import math +from dataclasses import dataclass, field +from pathlib import Path + +import numpy as np + +DT = 0.05 # modelV2/starpilotPlan cadence +MS_TO_MPH = 2.23694 +M_TO_MILES = 1.0 / 1609.34 +HIGHWAY_SPEED = 60.0 / MS_TO_MPH # above this, engagement is the over-slowing regression risk +CURVE_LAT_ACCEL = 1.3 # MINIMUM_LATERAL_ACCELERATION +EPISODE_GAP_S = 1.0 +V_CRUISE_UNSET = 255 + + +@dataclass +class Frame: + t: float = 0.0 + v_ego: float = 0.0 + a_ego: float = 0.0 + curvature: float = 0.0 + gas: bool = False + brake: bool = False + accel_pressed: bool = False + long_active: bool = False + blinker: bool = False # CSC gating input: a blinker suspends it entirely + csc_active: bool = False + csc_overridden: bool = False + csc_training: bool = False + csc_speed: float = 0.0 + v_cruise: float = 0.0 # applied cruise speed, already reduced by CSC + set_speed: float = 0.0 # what the driver dialled in, so cuts are measurable + learned_lat_accel: float = 0.0 + binding_distance: float = 0.0 + + @property + def lat_accel(self) -> float: + return self.v_ego ** 2 * abs(self.curvature) + + +@dataclass +class Episode: + start: float + end: float + peak_cut: float = 0.0 + peak_lat_accel: float = 0.0 + min_a_ego: float = 0.0 + entry_speed: float = 0.0 + binding_distance: float = 0.0 + cancelled: bool = False + gas: bool = False + brake: bool = False + + @property + def duration(self) -> float: + return self.end - self.start + + +def read_events(identifier: str): + """A downloaded rlog reads directly; anything else goes through LogReader.""" + path = Path(identifier) + if path.is_file(): + from cereal import log as capnp_log + + data = path.read_bytes() + if data[:4] == b"\x28\xb5\x2f\xfd": + import zstandard + data = zstandard.ZstdDecompressor().decompress(data, max_output_size=2 << 30) + return capnp_log.Event.read_multiple_bytes(data) + + from openpilot.tools.lib.logreader import LogReader, ReadMode # needs the device stack + + return LogReader(identifier, default_mode=ReadMode.AUTO, sort_by_time=True) + + +def read_frames(identifier: str) -> list[Frame]: + """Join carState/controlsState/starpilotPlan onto the plan's cadence.""" + frames: list[Frame] = [] + latest = Frame() + t0 = None + have_plan = False + + for msg in read_events(identifier): + which = msg.which() + if which == "carState": + cs = msg.carState + latest.v_ego = float(cs.vEgo) + latest.a_ego = float(cs.aEgo) + latest.gas = bool(cs.gasPressed) + latest.brake = bool(cs.brakePressed) + latest.blinker = bool(cs.leftBlinker or cs.rightBlinker) + set_kph = float(cs.vCruise) + latest.set_speed = set_kph / 3.6 if 0 < set_kph < V_CRUISE_UNSET else 0.0 + elif which == "carControl": + latest.long_active = bool(msg.carControl.longActive) + elif which == "controlsState": + latest.curvature = float(msg.controlsState.curvature) + elif which == "starpilotCarState": + latest.accel_pressed = bool(getattr(msg.starpilotCarState, "accelPressed", False)) + elif which == "starpilotPlan": + plan = msg.starpilotPlan + have_plan = True + if t0 is None: + t0 = msg.logMonoTime / 1e9 + latest.t = msg.logMonoTime / 1e9 - t0 + latest.csc_active = bool(plan.cscControllingSpeed) + latest.csc_training = bool(plan.cscTraining) + latest.csc_speed = float(plan.cscSpeed) + latest.v_cruise = float(plan.vCruise) + # absent in older logs + latest.csc_overridden = bool(getattr(plan, "cscOverridden", False)) + latest.learned_lat_accel = float(getattr(plan, "cscLearnedLatAccel", 0.0)) + latest.binding_distance = float(getattr(plan, "cscBindingDistance", 0.0)) + frames.append(Frame(**vars(latest))) + + if not have_plan: + raise SystemExit(f"no starpilotPlan messages in {identifier} — is this a StarPilot route?") + return frames + + +def build_episodes(frames: list[Frame]) -> list[Episode]: + episodes: list[Episode] = [] + current: Episode | None = None + last_active_t = -math.inf + + for f in frames: + if f.csc_active: + if current is None or (f.t - last_active_t) > EPISODE_GAP_S: + current = Episode(start=f.t, end=f.t, entry_speed=f.v_ego, + binding_distance=f.binding_distance, min_a_ego=f.a_ego) + episodes.append(current) + current.end = f.t + if f.set_speed > 0: + current.peak_cut = max(current.peak_cut, f.set_speed - f.csc_speed) + current.peak_lat_accel = max(current.peak_lat_accel, f.lat_accel) + current.min_a_ego = min(current.min_a_ego, f.a_ego) + current.gas |= f.gas + current.brake |= f.brake + last_active_t = f.t + elif current is not None and (f.t - last_active_t) <= EPISODE_GAP_S: + # an override releases CSC on the same frame it registers, so the rejection + # always lands just past the end of the episode it rejected + current.cancelled |= f.csc_overridden or f.accel_pressed + current.gas |= f.gas + current.brake |= f.brake + + return episodes + + +def curve_lat_accel_peaks(frames: list[Frame]) -> list[float]: + """Peak lateral acceleration of each distinct curve, engaged driving only.""" + peaks: list[float] = [] + peak = 0.0 + in_curve = False + for f in frames: + if not f.long_active: + continue + if f.lat_accel >= CURVE_LAT_ACCEL: + in_curve = True + peak = max(peak, f.lat_accel) + elif in_curve: + peaks.append(peak) + peak = 0.0 + in_curve = False + if in_curve: + peaks.append(peak) + return peaks + + +def summarize(frames: list[Frame], episodes: list[Episode]) -> dict: + driving = [f for f in frames if f.v_ego > 5.0] + engaged = [f for f in driving if f.long_active] + active = [f for f in engaged if f.csc_active] + distance_mi = sum(f.v_ego * DT for f in driving) * M_TO_MILES + peaks = curve_lat_accel_peaks(frames) + highway = [e for e in episodes if e.entry_speed >= HIGHWAY_SPEED] + + def pct(n, d): + return 100.0 * n / d if d else 0.0 + + return { + "route": { + "duration_min": len(frames) * DT / 60.0, + "distance_mi": distance_mi, + "engaged_pct": pct(len(engaged), len(driving)), + "mean_speed_mph": float(np.mean([f.v_ego for f in driving]) * MS_TO_MPH) if driving else 0.0, + }, + "engagement": { + "active_pct_of_engaged": pct(len(active), len(engaged)), + "episodes": len(episodes), + "episodes_per_mile": len(episodes) / distance_mi if distance_mi > 0.1 else 0.0, + "median_duration_s": float(np.median([e.duration for e in episodes])) if episodes else 0.0, + "max_duration_s": max((e.duration for e in episodes), default=0.0), + "median_cut_mph": float(np.median([e.peak_cut for e in episodes]) * MS_TO_MPH) if episodes else 0.0, + "max_cut_mph": max((e.peak_cut for e in episodes), default=0.0) * MS_TO_MPH, + "median_anticipation_m": float(np.median([e.binding_distance for e in episodes])) if episodes else 0.0, + }, + "outcome_lat_accel": { + "curves_seen": len(peaks), + "median": float(np.median(peaks)) if peaks else 0.0, + "p90": float(np.percentile(peaks, 90)) if peaks else 0.0, + "p99": float(np.percentile(peaks, 99)) if peaks else 0.0, + "max": max(peaks, default=0.0), + "over_3_0_pct": pct(sum(1 for p in peaks if p > 3.0), len(peaks)), + }, + "acceptance": { + "cancelled_episodes": sum(1 for e in episodes if e.cancelled), + "cancel_rate_pct": pct(sum(1 for e in episodes if e.cancelled), len(episodes)), + "gas_during_episode_pct": pct(sum(1 for e in episodes if e.gas), len(episodes)), + "brake_during_episode_pct": pct(sum(1 for e in episodes if e.brake), len(episodes)), + }, + "comfort": { + "median_min_a_ego": float(np.median([e.min_a_ego for e in episodes])) if episodes else 0.0, + "hardest_decel": min((e.min_a_ego for e in episodes), default=0.0), + }, + "highway_watch": { + "episodes_above_60mph": len(highway), + "max_cut_mph": max((e.peak_cut for e in highway), default=0.0) * MS_TO_MPH, + }, + "learning": { + "training_pct_of_driving": pct(sum(1 for f in driving if f.csc_training), len(driving)), + "learned_lat_accel_min": min((f.learned_lat_accel for f in active), default=0.0), + "learned_lat_accel_max": max((f.learned_lat_accel for f in active), default=0.0), + }, + } + + +def print_report(name: str, s: dict) -> None: + r, e, o, a, c, h, l = (s["route"], s["engagement"], s["outcome_lat_accel"], + s["acceptance"], s["comfort"], s["highway_watch"], s["learning"]) + print(f"\n=== {name}") + print(f" {r['duration_min']:.1f} min, {r['distance_mi']:.1f} mi, " + f"{r['mean_speed_mph']:.0f} mph avg, engaged {r['engaged_pct']:.0f}% of driving") + + print("\n DOES IT WORK -- peak lateral accel per curve (engaged)") + print(f" {o['curves_seen']} curves median {o['median']:.2f} p90 {o['p90']:.2f} " + f"p99 {o['p99']:.2f} max {o['max']:.2f} m/s^2") + print(f" curves over 3.0 m/s^2: {o['over_3_0_pct']:.1f}% <-- this tail should shrink vs a CSC-off route") + + print("\n DO USERS ACCEPT IT") + print(f" cancel rate (RES+) {a['cancel_rate_pct']:.0f}% gas {a['gas_during_episode_pct']:.0f}% " + f"brake {a['brake_during_episode_pct']:.0f}% of {e['episodes']} episodes") + print(" cancels/gas high => too slow; brake high => too fast") + + print("\n ENGAGEMENT") + print(f" {e['active_pct_of_engaged']:.1f}% of engaged time, {e['episodes_per_mile']:.2f} episodes/mi, " + f"median {e['median_duration_s']:.1f}s (max {e['max_duration_s']:.1f}s)") + print(f" speed cut median {e['median_cut_mph']:.1f} mph, max {e['max_cut_mph']:.1f} mph") + print(f" braking begins {e['median_anticipation_m']:.0f} m ahead (median)") + + print("\n COMFORT / REGRESSION WATCH") + print(f" decel median {c['median_min_a_ego']:.2f}, hardest {c['hardest_decel']:.2f} m/s^2") + print(f" highway (>60 mph) episodes: {h['episodes_above_60mph']}, max cut {h['max_cut_mph']:.1f} mph" + f" <-- over-slowing complaints start here") + + print("\n LEARNING") + print(f" training {l['training_pct_of_driving']:.1f}% of driving; " + f"learned comfort in use {l['learned_lat_accel_min']:.2f}-{l['learned_lat_accel_max']:.2f} m/s^2") + + +def main() -> None: + parser = argparse.ArgumentParser(description="Curve Speed Controller field report.") + parser.add_argument("routes", nargs="+", help="route/segment identifier(s) or rlog path(s)") + parser.add_argument("--json", type=Path, help="also write the raw numbers here") + args = parser.parse_args() + + reports = {} + for identifier in args.routes: + name = Path(identifier).name if Path(identifier).exists() else identifier + frames = read_frames(identifier) + episodes = build_episodes(frames) + summary = summarize(frames, episodes) + reports[name] = summary + print_report(name, summary) + + if args.json: + args.json.write_text(json.dumps(reports, indent=2)) + print(f"\nwrote {args.json}") + + +if __name__ == "__main__": + main()