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..11c6ac397 --- /dev/null +++ b/selfdrive/controls/tests/test_curve_speed_controller.py @@ -0,0 +1,547 @@ +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.common.starpilot_utilities import extract_curve_profile +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_MAX_LATERAL_ACCEL, + 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 = min( + max(float(np.sqrt(controller.lat_accel_for_curvature(curvature) / curvature)), CSC_MIN_SPEED), + float(np.sqrt(CSC_MAX_LATERAL_ACCEL / curvature)), + ) + 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(): + + _, 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 + assert controller.target - low_target == pytest.approx(CSC_TARGET_UP_RATE * DT_MDL) + + + 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(): + + + 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(): + + 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() + + + weak = probe._correct_far_field(*single_apex_profile(0.002, 90.0)) + assert weak.max() == pytest.approx(0.002) + + + 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 + + +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) + + 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 + + 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(np.sqrt(CSC_MAX_LATERAL_ACCEL / 0.1), abs=0.05) + + +def test_sharp_curve_target_respects_lateral_acceleration_cap(): + _, 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**2 * 0.1 <= CSC_MAX_LATERAL_ACCEL + 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(): + + _, 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(): + + _, 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 + assert dense_low.learned_lat_accel(0.005) >= dense_low.learned_lat_accel(0.003) + + 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_non_finite_persisted_curvature_data_is_ignored(): + _, controller = make_controller(curvature_data={ + "0.01": {"average": float("nan"), "count": 10}, + "0.02": {"average": float("inf"), "count": 10}, + }) + + assert controller.curvature_data == {} + assert np.all(np.isfinite(controller._curve_a)) + + +def test_invalid_curve_profile_is_ignored(): + model = SimpleNamespace( + orientationRate=SimpleNamespace(z=[0.01, 0.02]), + velocity=SimpleNamespace(x=[10.0]), + position=SimpleNamespace(x=[20.0, 40.0]), + ) + + curvatures, distances = extract_curve_profile(model) + + assert curvatures.size == 0 + assert distances.size == 0 + + curvatures, distances = extract_curve_profile(SimpleNamespace()) + + assert curvatures.size == 0 + assert distances.size == 0 + + +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(): + + + 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"] + + + 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 + + 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(): + + + 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 + settle_override(observed, make_sm(gas=True)) + + _, stepped = make_controller(driving_in_curve=True) + stepped._apply_nudge(CSC_NUDGE) + + 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 + 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 + + 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(): + + 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()) + + 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..07daa98be 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,55 @@ 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 + + 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 + + + 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 + + + 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 +204,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 +224,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 +245,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 +270,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 +288,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 +302,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 +313,272 @@ 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 + + + 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_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 + + 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 + + + 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 + + + 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 + assert not vcruise.csc_controlling_speed + + +def test_curve_speed_controller_glow_holds_through_a_brief_release(): + planner, vcruise = make_vcruise() + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.curve_speed_controller = True + + curve_target = {"v": 14.0} + + def set_curve_target(_v_ego, _v_cruise): + vcruise.csc.target = curve_target["v"] + + vcruise.csc.update_target = set_curve_target + update_vcruise(vcruise, sm, toggles, now=130.0, v_ego=20.0) + assert vcruise.csc_controlling_speed + + + 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) + + 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 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 + 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 -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 - - controller.update_target(30.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 + 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_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_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) - assert controller.target == pytest.approx(30.0) + def set_curve_target(_v_ego, _v_cruise): + vcruise.csc.target = 22.0 + + 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 + + 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 + + +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 + + + 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 + + + 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 + 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 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..81d996e64 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 + + if not params.get_bool("ShowCSCStatus"): return None diff --git a/starpilot/common/starpilot_utilities.py b/starpilot/common/starpilot_utilities.py index efd3f3ce3..dd040e2be 100644 --- a/starpilot/common/starpilot_utilities.py +++ b/starpilot/common/starpilot_utilities.py @@ -138,6 +138,31 @@ 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 +PROFILE_MAX_CURVATURE = 0.1 + + +def extract_curve_profile(modelData): + try: + orientation_rate = np.abs(np.array(modelData.orientationRate.z)) + velocity = np.array(modelData.velocity.x) + distances = np.array(modelData.position.x) + except (AttributeError, TypeError, ValueError): + return np.array([]), np.array([]) + + if not (len(orientation_rate) == len(velocity) == len(distances)): + return np.array([]), np.array([]) + if not (np.all(np.isfinite(orientation_rate)) and + np.all(np.isfinite(velocity)) and + np.all(np.isfinite(distances))): + return np.array([]), np.array([]) + + 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..006854009 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -1,20 +1,93 @@ #!/usr/bin/env python3 +import math + 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 + +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 +CSC_RELEASE_DEBOUNCE = 0.25 +CSC_ACTIVE_ON_DELTA = 0.5 +CSC_ACTIVE_OFF_DELTA = 0.25 +CSC_GLOW_ON_DELTA = 1.0 +CSC_GLOW_HOLD_TIME = 3.0 + +CSC_COUNT_CAP = 600 +CSC_PRIOR_COUNT = 100 +CSC_LAT_ACCEL_MIN = 1.2 +CSC_LAT_ACCEL_MAX = 3.2 +CSC_MAX_LATERAL_ACCEL = 4.0 +CSC_NUDGE = 0.15 +CSC_NUDGE_WEIGHT = 20 +CSC_OVERRIDE_WATCH_TIME = 6.0 +CSC_TRAINING_QUIET_TIME = 5.0 +CSC_TRAINING_SETTLE_TIME = 2.0 +CSC_COMFORT_MARGIN = 1.0 + +CSC_FARFIELD_MIN_CURVATURE = 0.004 +CSC_FARFIELD_MIN_DISTANCE = 30.0 +CSC_FARFIELD_GAIN = 1.23 + +MIN_CURVATURE = 0.0005 +MAX_CURVATURE = 0.02 +CURVATURE_BUCKETS = 24 +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) + + +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 +116,42 @@ 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)] + 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() + + self.data_dirty = True + + 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)) + + 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): @@ -81,7 +170,7 @@ class CurveSpeedController: except (KeyError, TypeError, ValueError): continue - if count <= 0: + if count <= 0 or not math.isfinite(raw_curvature) or not math.isfinite(average): continue bucket = cls._bucket_curvature(raw_curvature) @@ -100,17 +189,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 +196,45 @@ 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): 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 + + self.training_timer = max(self.training_timer - DT_MDL, 0.0) self.persistence_timer = 0.0 return @@ -151,8 +243,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 +253,11 @@ class CurveSpeedController: if road_curvature in self.curvature_data: data = self.curvature_data[road_curvature] - average = data["average"] - count = data["count"] + + 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 +266,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 +275,160 @@ 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): + 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: + + 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 + + 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)) + + 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) + curvature_floor = np.maximum(curvatures, 1e-4) + point_speeds = np.sqrt(lat_accel / curvature_floor) + point_speeds = np.minimum( + np.maximum(point_speeds, CSC_MIN_SPEED), + np.sqrt(CSC_MAX_LATERAL_ACCEL / curvature_floor), + ) + 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 + + 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 + + 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)) + + 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..b41a47b52 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) - 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) + 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 + + 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 + + + + 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 + + + + 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: + + 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: + + + 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.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/tools/longitudinal/analyze_csc.py b/tools/longitudinal/analyze_csc.py new file mode 100644 index 000000000..c10b5e5df --- /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 + ./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 +MS_TO_MPH = 2.23694 +M_TO_MILES = 1.0 / 1609.34 +HIGHWAY_SPEED = 60.0 / MS_TO_MPH +CURVE_LAT_ACCEL = 1.3 +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_active: bool = False + csc_overridden: bool = False + csc_training: bool = False + csc_speed: float = 0.0 + v_cruise: float = 0.0 + set_speed: float = 0.0 + 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 + + 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) + + 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: + + + 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()