From 6fc7b9ceb8fd31ed7cbe4246909c3b0dea55e9fb Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Sun, 16 Aug 2026 19:28:34 -0500 Subject: [PATCH] Test1 --- cereal/custom.capnp | 4 + common/params_keys.h | 1 + selfdrive/car/card.py | 4 +- selfdrive/car/cruise.py | 15 +- selfdrive/car/tests/test_csc_cruise_button.py | 74 ++++ .../test_conditional_experimental_mode.py | 3 + .../tests/test_curve_speed_controller.py | 359 ++++++++++++++++++ .../controls/tests/test_starpilot_vcruise.py | 149 +++++--- .../settings/starpilot/longitudinal.py | 9 +- starpilot/common/starpilot_utilities.py | 17 + starpilot/common/starpilot_variables.py | 7 + .../controls/lib/curve_speed_controller.py | 231 +++++++++-- starpilot/controls/lib/starpilot_vcruise.py | 63 +-- starpilot/controls/starpilot_planner.py | 6 +- .../components/tools/device_settings.css | 8 + .../components/tools/device_settings.js | 11 + .../tools/device_settings_layout.json | 39 +- starpilot/system/the_galaxy/the_galaxy.py | 7 + tools/longitudinal/analyze_csc.py | 271 +++++++++++++ 19 files changed, 1167 insertions(+), 111 deletions(-) create mode 100644 selfdrive/car/tests/test_csc_cruise_button.py create mode 100644 selfdrive/controls/tests/test_curve_speed_controller.py create mode 100644 tools/longitudinal/analyze_csc.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index c577ad906..01c8f4f24 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -220,6 +220,10 @@ struct StarPilotPlan @0xf98d843bfd7004a3 { disableThrottle @35 :Bool; trackingLead @36 :Bool; stopSignConfirmed @37 :Bool; + # Curve Speed Controller diagnostics, for tuning and rollout validation + cscOverridden @38 :Bool; # driver cancelled this curve with RES+ + cscLearnedLatAccel @39 :Float32; # learned comfort at the current curvature, before margin + cscBindingDistance @40 :Float32; # distance to the horizon point setting the target, m } struct StarPilotRadarState @0xb86e6369214c01c8 { diff --git a/common/params_keys.h b/common/params_keys.h index 49ba5309d..4c8e582d5 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -241,6 +241,7 @@ inline static std::unordered_map keys = { {"CurvatureData", {PERSISTENT | DONT_LOG, JSON, "{}", "{}"}}, {"CurveSpeedController", {PERSISTENT, BOOL, "1", "0", 1, SETTINGS_SIMPLE}}, {"CurveSpeedControllerNoLead", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, + {"CurveSpeedMargin", {PERSISTENT, INT, "85", "85", 2, SETTINGS_SIMPLE}}, {"CustomAlerts", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, {"CustomAccelProfile", {PERSISTENT, BOOL, "0", "0", 3}}, {"CustomAccelProfileInitialized", {PERSISTENT, BOOL, "0", "0", 3}}, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 9e43863f6..11bf764a5 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -24,7 +24,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.common.constants import CV from openpilot.selfdrive.car.cruise import ( VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN, - is_speed_limit_confirmation_pending, + is_csc_override_pending, is_speed_limit_confirmation_pending, ) from openpilot.selfdrive.car.redneck_cruise import RedneckCruise, select_redneck_target_speed from openpilot.selfdrive.car.car_specific import MockCarState @@ -277,6 +277,7 @@ class Car: ) if not preap_software_cruise: speed_limit_confirmation_pending = is_speed_limit_confirmation_pending(self.sm['starpilotPlan']) + csc_override_pending = is_csc_override_pending(self.sm['starpilotPlan']) self.v_cruise_helper.update_v_cruise( CS, self.sm['carControl'].enabled, @@ -284,6 +285,7 @@ class Car: speed_limit_confirmation_pending, self.starpilot_toggles, FPCS, + csc_active=csc_override_pending, ) else: preap_v_cruise_kph = float(CS.cruiseState.speed * CV.MS_TO_KPH) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index e2f770bb6..739961922 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -37,6 +37,11 @@ def is_speed_limit_confirmation_pending(starpilot_plan) -> bool: return bool(starpilot_plan.speedLimitChanged and starpilot_plan.unconfirmedSlcSpeedLimit >= 1) +def is_csc_override_pending(starpilot_plan) -> bool: + """Accel presses cancel an active curve slowdown instead of raising the set speed.""" + return bool(starpilot_plan.cscControllingSpeed) and not is_speed_limit_confirmation_pending(starpilot_plan) + + class VCruiseHelper: def __init__(self, CP, FPCP=None): self.CP = CP @@ -89,13 +94,13 @@ class VCruiseHelper: return bool(getattr(starpilot_car_state, "decelHardCruise", False)) return False - def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None): + def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None, csc_active=False): self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: if self.gm_cc_only or self.redneck_non_pcm or not self.CP.pcmCruise: # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state) + self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state, csc_active) self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled, starpilot_car_state) else: @@ -111,7 +116,7 @@ class VCruiseHelper: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET - def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None): + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None, csc_active=False): # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition if not enabled: @@ -126,7 +131,9 @@ class VCruiseHelper: for b in CS.buttonEvents: event_button_type = b.type.raw if event_button_type in self.button_timers: - if speed_limit_changed and b.pressed: + # decel presses keep their normal meaning + consume_press = speed_limit_changed or (csc_active and event_button_type in ACCEL_CRUISE_BUTTONS) + if consume_press and b.pressed: self.confirmation_button_suppressed.add(event_button_type) elif not b.pressed and event_button_type in self.confirmation_button_suppressed: self.confirmation_button_suppressed.remove(event_button_type) diff --git a/selfdrive/car/tests/test_csc_cruise_button.py b/selfdrive/car/tests/test_csc_cruise_button.py new file mode 100644 index 000000000..c4b75a451 --- /dev/null +++ b/selfdrive/car/tests/test_csc_cruise_button.py @@ -0,0 +1,74 @@ +import pytest + +from types import SimpleNamespace + +from openpilot.selfdrive.car.cruise import ButtonType, VCruiseHelper, is_csc_override_pending + + +def make_helper(): + CP = SimpleNamespace(carFingerprint="MOCK", flags=0, pcmCruise=False, brand="mock") + helper = VCruiseHelper(CP) + helper.v_cruise_kph = 40.0 + return helper + + +def make_toggles(): + return SimpleNamespace(cruise_increase=1.0, cruise_increase_long=5.0, reverse_cruise_increase=False) + + +def make_cs(button_events): + return SimpleNamespace( + buttonEvents=button_events, + cruiseState=SimpleNamespace(available=True, standstill=False, speed=0, speedCluster=0), + gasPressed=False, + vEgo=20.0, + ) + + +def press(button): + return SimpleNamespace(type=SimpleNamespace(raw=button), pressed=True) + + +def release(button): + return SimpleNamespace(type=SimpleNamespace(raw=button), pressed=False) + + +def press_and_release(helper, button, csc_active): + toggles = make_toggles() + helper.update_v_cruise(make_cs([press(button)]), True, True, False, toggles, None, csc_active=csc_active) + helper.update_v_cruise(make_cs([release(button)]), True, True, False, toggles, None, csc_active=csc_active) + + +def test_accel_press_consumed_while_csc_active(): + helper = make_helper() + + press_and_release(helper, ButtonType.accelCruise, csc_active=True) + + assert helper.v_cruise_kph == pytest.approx(40.0) + + +def test_accel_press_adjusts_set_speed_when_csc_inactive(): + helper = make_helper() + + press_and_release(helper, ButtonType.accelCruise, csc_active=False) + + assert helper.v_cruise_kph > 40.0 + + +def test_decel_press_still_works_while_csc_active(): + helper = make_helper() + + press_and_release(helper, ButtonType.decelCruise, csc_active=True) + + assert helper.v_cruise_kph < 40.0 + + +def test_csc_override_pending_defers_to_slc_confirmation(): + active_plan = SimpleNamespace(cscControllingSpeed=True, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0) + assert is_csc_override_pending(active_plan) + + idle_plan = SimpleNamespace(cscControllingSpeed=False, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0) + assert not is_csc_override_pending(idle_plan) + + slc_pending_plan = SimpleNamespace(cscControllingSpeed=True, speedLimitChanged=True, unconfirmedSlcSpeedLimit=25) + assert not is_csc_override_pending(slc_pending_plan) diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 3e72eb70f..ab934873c 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -1,6 +1,8 @@ from pathlib import Path from types import SimpleNamespace +import numpy as np + from openpilot.common.constants import CV from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module @@ -1168,6 +1170,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: (np.zeros(33), np.zeros(33))) 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..49ae9d833 --- /dev/null +++ b/selfdrive/controls/tests/test_curve_speed_controller.py @@ -0,0 +1,359 @@ +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, PLANNER_TIME +from openpilot.starpilot.controls.lib.curve_speed_controller import ( + CSC_APPROACH_DECEL, + CSC_COMFORT_MARGIN, + CSC_COUNT_CAP, + CSC_EGO_HEADROOM, + CSC_LAT_ACCEL_MAX, + CSC_MIN_SPEED, + CSC_NUDGE_WEIGHT, + 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(): + _, controller = make_controller(curve_profile=single_apex_profile(0.02, 400.0)) + + 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 == pytest.approx(15.0 + CSC_EGO_HEADROOM) + + recovered = converge(controller, 15.0, 30.0) + assert recovered == pytest.approx(30.0) + + +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 + for _ in range(100): + v_ego = min(v_ego + 2.0 * DT_MDL, 30.0) + controller.update_target(v_ego, 30.0) + assert controller.target >= min(30.0, v_ego + CSC_EGO_HEADROOM) - 1e-6 + + +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 > 15.0 + + 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(0.1) == pytest.approx(2.9, abs=0.05) + assert controller.lateral_acceleration == pytest.approx(DEFAULT_LATERAL_ACCELERATION) + + +def test_comfort_margin_aims_below_the_learned_habit(): + _, controller = make_controller() + + assert controller.lat_accel_for_curvature(0.01) == pytest.approx( + controller.learned_lat_accel(0.01) * CSC_COMFORT_MARGIN) + assert controller.lat_accel_for_curvature(0.01) < controller.learned_lat_accel(0.01) + + +def test_margin_slider_overrides_the_default(): + _, controller = make_controller() + controller.starpilot_toggles = SimpleNamespace(csc_margin=0.7) + + assert controller.comfort_margin == pytest.approx(0.7) + assert controller.lat_accel_for_curvature(0.01) == pytest.approx( + controller.learned_lat_accel(0.01) * 0.7) + + +def test_margin_falls_back_to_default_without_toggles(): + _, controller = make_controller() + + # toggles are only attached once vcruise runs + assert controller.comfort_margin == pytest.approx(CSC_COMFORT_MARGIN) + controller.starpilot_toggles = SimpleNamespace(csc_margin=0.0) + assert controller.comfort_margin == pytest.approx(CSC_COMFORT_MARGIN) + + +def test_lower_margin_engages_on_gentler_curves(): + planner, controller = make_controller(curve_profile=single_apex_profile(0.004, 0.0)) + controller.starpilot_toggles = SimpleNamespace(csc_margin=1.0) + relaxed = converge(controller, 30.0, 30.0) + + planner2, aggressive_controller = make_controller(curve_profile=single_apex_profile(0.004, 0.0)) + aggressive_controller.starpilot_toggles = SimpleNamespace(csc_margin=0.7) + aggressive = converge(aggressive_controller, 30.0, 30.0) + + assert aggressive < relaxed + + +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 + _, controller = make_controller(curvature_data={ + "0.003": {"average": 1.95, "count": 20}, + "0.005": {"average": 1.38, "count": 80}, + }) + + assert controller.learned_lat_accel(0.005) < 1.82 + assert controller.learned_lat_accel(0.005) >= controller.learned_lat_accel(0.003) + + +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 = PLANNER_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 = PLANNER_TIME + controller.log_data(10.0, make_sm(long_active=False)) + assert "0.02" not in controller.curvature_data + assert not controller.enable_training + + controller.training_quiet_timer = 0.0 + controller.training_timer = PLANNER_TIME + controller.log_data(10.0, make_sm(long_active=False)) + assert controller.curvature_data["0.02"]["count"] == 1 + + +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)) + assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT + assert controller.curvature_data["0.02"]["average"] > prior + + controller.handle_override(20.0, True, make_sm(gas=True)) + assert controller.curvature_data["0.02"]["count"] == CSC_NUDGE_WEIGHT + + controller.handle_override(20.0, False, make_sm()) + controller.target = 10.0 + controller.handle_override(20.0, True, make_sm(gas=True)) + assert controller.curvature_data["0.02"]["count"] == 2 * CSC_NUDGE_WEIGHT + + +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) + + 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 fc48e72c8..ec9a7f715 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -3,9 +3,7 @@ import datetime import pytest from openpilot.common.constants import CV -from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME -from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController from openpilot.starpilot.controls.lib.starpilot_vcruise import ( FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, StarPilotVCruise, @@ -130,28 +128,22 @@ def test_camry_tss2_gets_forward_force_stop_bias_only(): assert get_force_stop_distance_bias("TOYOTA_RAV4_TSS2") == pytest.approx(0.0) -def test_curve_speed_controller_holds_target_through_brief_detector_dropout(): +def test_curve_speed_controller_blinker_resets_target(): 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 + 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=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(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) assert not vcruise.csc_controlling_speed @@ -162,16 +154,13 @@ 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) @@ -185,12 +174,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) @@ -208,10 +195,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 @@ -261,39 +246,107 @@ 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_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_csc_res_press_cancels_for_episode_and_rearms(): + planner, vcruise = make_vcruise() + sm = make_sm(standstill=False) + toggles = make_toggles() + toggles.curve_speed_controller = True - controller.update_target(30.0) + curve_target = {"v": 14.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 + 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_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_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 - controller.update_target(30.0) + def set_curve_target(_v_ego, _v_cruise): + vcruise.csc.target = 14.0 - assert controller.target == pytest.approx(30.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_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/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index b273ee7f2..e2f59e873 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -684,14 +684,19 @@ class StarPilotLongitudinalLayout(_SettingsPage): # ── 5. Adaptive Speed Controls Rows (CES + CSC + CCM) ── self._curve_speed_controller_rows = [ + SettingRow("CurveSpeedMargin", "value", tr_noop("Curve Speed Margin"), + subtitle=tr_noop("How much of your learned cornering comfort to use. Lower slows more for curves; 100% matches how you take them yourself."), + get_value=lambda: f"{self._params.get_int('CurveSpeedMargin')}%", + on_click=lambda: self._show_slider("CurveSpeedMargin", 70, 100, step=5, unit="%"), + visible=csc_on), SettingRow("CalibratedLatAccel", "value", tr_noop("Calibrated Lateral Accel"), subtitle=tr_noop("The learned lateral acceleration from collected driving data. Higher values allow faster cornering."), - get_value=lambda: f"{self._params_memory.get_float('CalibratedLateralAcceleration'):.2f} m/s", + get_value=lambda: f"{self._params.get_float('CalibratedLateralAcceleration'):.2f} m/s", on_click=None, visible=csc_on), SettingRow("CalibrationProgress", "value", tr_noop("Calibration Progress"), subtitle=tr_noop("How much curve data has been collected. Normal for the value to stay low."), - get_value=lambda: f"{self._params_memory.get_float('CalibrationProgress'):.2f}%", + get_value=lambda: f"{self._params.get_float('CalibrationProgress'):.2f}%", on_click=None, visible=csc_on), SettingRow("ResetCurve", "action", tr_noop("Reset Curve Data"), diff --git a/starpilot/common/starpilot_utilities.py b/starpilot/common/starpilot_utilities.py index b207a347f..efafe8587 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/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 564ad6f15..aa4d2d3e0 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -49,6 +49,9 @@ from openpilot.system.version import get_build_metadata CITY_SPEED_LIMIT = 25 # 55mph is typically the minimum speed for highways CRUISING_SPEED = 5 # Roughly the speed cars go when not touching the gas while in drive +CSC_DEFAULT_MARGIN_PERCENT = 85 # Percent of learned cornering comfort the Curve Speed Controller targets +CSC_MIN_MARGIN_PERCENT = 70 # Slows the most; 100 would exactly match the driver's own habit +CSC_MAX_MARGIN_PERCENT = 100 DEFAULT_LATERAL_ACCELERATION = 2.0 # m/s^2, typical lateral acceleration when taking curves DISPLAY_MENU_TIMER = 350 # The length of time the following distance menu appears on some GM vehicles to prevent things getting out of sync EARTH_RADIUS = 6378137 # Radius of the Earth in meters @@ -826,6 +829,10 @@ class StarPilotVariables: toggle.curve_speed_controller = toggle.openpilot_longitudinal and self.get_value("CurveSpeedController") toggle.csc_no_lead = self.get_value("CurveSpeedControllerNoLead", condition=toggle.curve_speed_controller) toggle.csc_status = self.get_value("ShowCSCStatus", condition=toggle.curve_speed_controller) or toggle.debug_mode + # percent of learned cornering comfort to actually use; lower slows more + toggle.csc_margin = self.get_value("CurveSpeedMargin", cast=float, condition=toggle.curve_speed_controller, + default=CSC_DEFAULT_MARGIN_PERCENT, min=CSC_MIN_MARGIN_PERCENT, + max=CSC_MAX_MARGIN_PERCENT) / 100.0 toggle.goat_scream_alert = self.get_value("GoatScream") toggle.goat_scream_critical_alerts = self.get_value("GoatScreamCriticalAlerts") diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index 7d21780be..36cbf2dc0 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -2,19 +2,81 @@ 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, + CSC_DEFAULT_MARGIN_PERCENT, + DEFAULT_LATERAL_ACCELERATION, + PLANNER_TIME, +) CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS -CSC_MAX_DECEL_RATE = 1.5 + +CSC_APPROACH_DECEL = 1.0 +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_ACTIVE_ON_DELTA = 0.5 +CSC_ACTIVE_OFF_DELTA = 0.25 + +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_TRAINING_QUIET_TIME = 5.0 # blocks passive samples after CSC limited speed, so it can't learn its own cap +# Learned values match the driver's own cornering, which alone would never slow them +# below their habit. Speed scales as the square root, so 0.85 is ~8% slower. +CSC_COMFORT_MARGIN = CSC_DEFAULT_MARGIN_PERCENT / 100.0 + MAX_CURVATURE = 0.1 MIN_CURVATURE = 0.001 -PERCENTILE = 90 ROUNDING_PRECISION = 5 STEP = 0.001 +# 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): try: @@ -41,19 +103,32 @@ class CurveSpeedController: def __init__(self, StarPilotVCruise): self.starpilot_planner = StarPilotVCruise.starpilot_planner + self.starpilot_toggles = None + self.enable_training = False - self.target_set = False + self.nudge_applied = False 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.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.update_lateral_acceleration() + 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 @staticmethod def _bucket_curvature(road_curvature): @@ -107,6 +182,7 @@ class CurveSpeedController: if key in self.curvature_data: progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0) + self.starpilot_planner.params.put_nonblocking("CalibratedLateralAcceleration", self.lateral_acceleration) self.starpilot_planner.params.put_nonblocking("CalibrationProgress", (progress / len(self.required_curvatures)) * 100) self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data) self.data_dirty = False @@ -116,10 +192,13 @@ class CurveSpeedController: 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 @@ -144,11 +223,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] = { @@ -157,7 +236,7 @@ class CurveSpeedController: } self.data_dirty = True - self.update_lateral_acceleration() + self.rebuild_lat_accel_curve() self.enable_training = True if self.persistence_timer >= PLANNER_TIME: @@ -165,29 +244,119 @@ 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 + + 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._apply_nudge(CSC_NUDGE) + elif (getattr(sm["carState"], "brakePressed", False) or long_dropped) and self.starpilot_planner.driving_in_curve: + self._apply_nudge(-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 + sample = float(np.clip(self.learned_lat_accel(float(key)) + offset, 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.nudge_applied = True + 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) + 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 + @property + def comfort_margin(self): + margin = getattr(self.starpilot_toggles, "csc_margin", None) + return float(margin) if margin else 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)) + def lat_accel_for_curvature(self, curvature): + lat_accel = np.interp(np.abs(curvature), self._curve_k, self._curve_a) * self.comfort_margin + + weather = self.starpilot_planner.starpilot_weather + if weather.weather_id != 0: + lat_accel = lat_accel * (1.0 - weather.reduce_lateral_acceleration) + + return lat_accel + + def reset(self, v_cruise): + self.target = float(v_cruise) + 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 + 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 + + filtered = self.target_filter.update(raw_target) + self.target = float(np.clip(filtered, + self.target - CSC_TARGET_DOWN_RATE * DT_MDL, + self.target + CSC_TARGET_UP_RATE * DT_MDL)) + self.target = max(self.target, min(raw_target, v_ego + CSC_EGO_HEADROOM)) + + 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 bcc205818..be0757bf3 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -6,7 +6,12 @@ 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_ACTIVE_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, @@ -14,7 +19,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 STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0 @@ -184,8 +188,8 @@ class StarPilotVCruise: self._nav_instruction_state = {} self._applied_slc_control_target = 0.0 self.csc_controlling_speed = False + 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 {} @@ -528,7 +532,8 @@ class StarPilotVCruise: v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego) v_ego_diff = v_ego_cluster - v_ego - # FrogsGoMoo's Curve Speed Controller + # Curve Speed Controller + self.csc.starpilot_toggles = starpilot_toggles following_lead = bool(getattr(self.starpilot_planner.starpilot_following, "following_lead", False)) manual_speed_control = is_manual_speed_control(sm) csc_available = ( @@ -538,28 +543,42 @@ 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) + csc_blinker_on = sm["carState"].leftBlinker or sm["carState"].rightBlinker + 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) + # RES+ cancels the slowdown for the rest of this curve; cruise.py consumes the press + # so the set speed is untouched. Latched outside the availability branch because the + # press itself suspends CSC, so otherwise the slowdown returns on button 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_target = v_cruise + else: + self.csc_target = self.csc.target + if self.csc_target < v_cruise - CSC_ACTIVE_ON_DELTA: + self.csc_controlling_speed = True + elif self.csc_target > v_cruise - CSC_ACTIVE_OFF_DELTA: + self.csc_controlling_speed = False + else: + self.csc.reset(v_cruise) + self.csc_controlling_speed = False + 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 78edbd1a0..af009abc2 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 @@ -217,6 +217,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) @@ -325,6 +326,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 = self.starpilot_following.disable_throttle 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 e6a951b2f..834162044 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings.css +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings.css @@ -426,6 +426,14 @@ color: var(--text-muted); } +/* Read-only learned values (e.g. Curve Speed Controller calibration) */ +.ds-readout { + color: var(--text-color); + font-size: var(--font-size-sm); + font-weight: var(--font-weight-semibold); + white-space: nowrap; +} + .ds-manual-row { align-items: center; display: flex; 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 b16823652..eacd9a4de 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js @@ -484,6 +484,13 @@ function formatStepValue(step, precision) { return Number(n.toFixed(Math.max(0, precision))).toString() } +function formatReadoutValue(param, value) { + const n = Number(value) + if (value === null || value === undefined || !Number.isFinite(n)) return "--" + const precision = Number.isFinite(Number(param.precision)) ? Number(param.precision) : 2 + return `${n.toFixed(precision)}${param.unit || ""}` +} + function numericBounds(param) { const defaultBounds = { min: param.min !== undefined ? param.min : (param.data_type === "float" ? 0.0 : 0), @@ -1620,6 +1627,10 @@ function renderSettingRow(p) { disabled="${() => isLocked()}" @change="${() => updateParam(p.key, "text")}" /> ` + } else if (p.ui_type === "readout") { + rowControl = html` + ${() => formatReadoutValue(p, state.values[p.key])} + ` } else if (p.ui_type === "color") { rowControl = html`
diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index c03b7740d..f2d0b2027 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -751,12 +751,25 @@ { "key": "CurveSpeedController", "label": "Curve Speed Controller", - "description": "Automatically slow down for upcoming curves using data learned from your driving style, adapting to curves as you would.", + "description": "Automatically slow down for upcoming curves using data learned from your driving style, adapting to curves as you would. Press RES+ while it is slowing to cancel the slowdown for that curve.", "data_type": "bool", "ui_type": "toggle", "is_parent_toggle": true, "settings_tier": "simple" }, + { + "key": "CurveSpeedMargin", + "label": "Curve Speed Margin", + "description": "How much of your learned cornering comfort to use for curves. Lower slows more; 100% matches how you take curves yourself.", + "data_type": "int", + "ui_type": "numeric", + "min": 70.0, + "max": 100.0, + "step": 5.0, + "unit": "%", + "parent_key": "CurveSpeedController", + "settings_tier": "simple" + }, { "key": "ShowCSCStatus", "label": "Status Widget", @@ -775,10 +788,32 @@ "parent_key": "CurveSpeedController", "settings_tier": "simple" }, + { + "key": "CalibratedLateralAcceleration", + "label": "Calibrated Lateral Accel", + "description": "The learned lateral acceleration from collected driving data. Higher values allow faster cornering.", + "data_type": "float", + "ui_type": "readout", + "precision": 2, + "unit": " m/s²", + "parent_key": "CurveSpeedController", + "settings_tier": "simple" + }, + { + "key": "CalibrationProgress", + "label": "Calibration Progress", + "description": "How much curve data has been collected. Normal for the value to stay low.", + "data_type": "float", + "ui_type": "readout", + "precision": 2, + "unit": "%", + "parent_key": "CurveSpeedController", + "settings_tier": "simple" + }, { "key": "ResetCurveData", "label": "Reset Curve Data", - "description": "Clear learned Curve Speed Controller data and begin training again from the default lateral acceleration.", + "description": "Clear learned Curve Speed Controller data and begin training again from the built-in comfort defaults.", "ui_type": "action", "action_label": "Reset", "action_endpoint": "/api/curve_speed_controller/reset", diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 8a885f2f3..21448b523 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -5324,6 +5324,13 @@ def setup(app): result["AlphaLongitudinalAvailable"] = _get_alpha_longitudinal_available() result["HasRivianAngleHarness"] = _get_has_rivian_angle_harness() + # display only; kept out of allowed_keys so the write paths still reject them + for readonly_key in ("CalibratedLateralAcceleration", "CalibrationProgress"): + try: + result[readonly_key] = params.get_float(readonly_key) + except Exception: + result[readonly_key] = None + return jsonify(_sanitize_json_value(result)), 200 @app.route("/api/params/defaults", methods=["GET"]) diff --git a/tools/longitudinal/analyze_csc.py b/tools/longitudinal/analyze_csc.py new file mode 100644 index 000000000..9d5ce9c0f --- /dev/null +++ b/tools/longitudinal/analyze_csc.py @@ -0,0 +1,271 @@ +#!/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 + + +@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 + csc_active: bool = False + csc_overridden: bool = False + csc_training: bool = False + csc_speed: float = 0.0 + v_cruise: 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_frames(identifier: str) -> list[Frame]: + """Join carState/controlsState/starpilotPlan onto the plan's cadence.""" + from openpilot.tools.lib.logreader import LogReader, ReadMode # heavy; keeps the metrics importable off-device + + frames: list[Frame] = [] + latest = Frame() + t0 = None + have_plan = False + + reader = LogReader(identifier, default_mode=ReadMode.AUTO, sort_by_time=True) + for msg in reader: + 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) + 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 + current.peak_cut = max(current.peak_cut, f.v_cruise - 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: + # the cancel lands on the frame CSC releases, so look just past the end + current.cancelled |= f.csc_overridden or f.accel_pressed + + 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()