diff --git a/common/params_keys.h b/common/params_keys.h index 4c8e582d5..c3adcc0aa 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}}, + {"CurveSpeedApproachDecel", {PERSISTENT, FLOAT, "0.6", "0.6", 2, SETTINGS_SIMPLE}}, {"CurveSpeedMargin", {PERSISTENT, INT, "85", "85", 2, SETTINGS_SIMPLE}}, {"CustomAlerts", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, {"CustomAccelProfile", {PERSISTENT, BOOL, "0", "0", 3}}, diff --git a/selfdrive/controls/tests/test_curve_speed_controller.py b/selfdrive/controls/tests/test_curve_speed_controller.py index 49ae9d833..d0ab4ae5d 100644 --- a/selfdrive/controls/tests/test_curve_speed_controller.py +++ b/selfdrive/controls/tests/test_curve_speed_controller.py @@ -85,7 +85,12 @@ def test_straight_road_target_is_cruise_speed(): def test_distant_apex_does_not_constrain_until_braking_is_due(): - _, controller = make_controller(curve_profile=single_apex_profile(0.02, 400.0)) + # derived from the shipped decel so retuning it doesn't silently invalidate the case + _, probe = make_controller() + curve_speed = max(float(np.sqrt(probe.lat_accel_for_curvature(0.02) / 0.02)), CSC_MIN_SPEED) + beyond_braking = 1.3 * (30.0**2 - curve_speed**2) / (2 * CSC_APPROACH_DECEL) + + _, controller = make_controller(curve_profile=single_apex_profile(0.02, beyond_braking)) target = converge(controller, 30.0, 30.0) @@ -209,6 +214,25 @@ def test_lower_margin_engages_on_gentler_curves(): assert aggressive < relaxed +def test_approach_decel_slider_moves_where_braking_starts(): + # the same curve must bind further out when the approach is planned gentler + def bind_distance(decel): + planner, controller = make_controller(curve_profile=single_apex_profile(0.01, 200.0)) + controller.starpilot_toggles = SimpleNamespace(csc_approach_decel=decel) + converge(controller, 30.0, 30.0) + return controller.target + + assert bind_distance(0.3) < bind_distance(1.5) + + +def test_approach_decel_falls_back_to_default(): + _, controller = make_controller() + + assert controller.approach_decel == pytest.approx(CSC_APPROACH_DECEL) + controller.starpilot_toggles = SimpleNamespace(csc_approach_decel=0.0) + assert controller.approach_decel == pytest.approx(CSC_APPROACH_DECEL) + + 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) diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index e28c8ad8a..0bd2f04a8 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -674,6 +674,12 @@ class StarPilotLongitudinalLayout(_SettingsPage): # ── 4. Adaptive Speed Controls Rows (CES + CSC + CCM) ── self._curve_speed_controller_rows = [ + SettingRow("CurveSpeedApproachDecel", "value", tr_noop("Curve Speed Approach Decel"), + subtitle=tr_noop("How hard the slowdown into a curve is planned. Lower starts it sooner and spreads it over more distance; higher waits longer and slows more firmly."), + get_value=lambda: f"{self._params.get_float('CurveSpeedApproachDecel'):.1f} m/s²", + on_click=lambda: self._show_slider("CurveSpeedApproachDecel", 0.3, 1.5, step=0.1, + unit=" m/s²", value_type="float"), + visible=csc_on), 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')}%", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index aa4d2d3e0..be06f76f1 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -52,6 +52,9 @@ CRUISING_SPEED = 5 # Roughly the speed cars go when not t 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 +CSC_DEFAULT_APPROACH_DECEL = 0.6 # m/s^2 the approach is planned at; sets how early the slowdown starts +CSC_MIN_APPROACH_DECEL = 0.3 # earlier than this saturates against the model's ~10s horizon +CSC_MAX_APPROACH_DECEL = 1.5 # later and firmer; matches the old ramp cap 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 @@ -833,6 +836,11 @@ class StarPilotVariables: 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 + # lower plans the approach over a longer distance, so the slowdown starts sooner + toggle.csc_approach_decel = self.get_value("CurveSpeedApproachDecel", cast=float, + condition=toggle.curve_speed_controller, + default=CSC_DEFAULT_APPROACH_DECEL, min=CSC_MIN_APPROACH_DECEL, + max=CSC_MAX_APPROACH_DECEL) 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 36cbf2dc0..cec199512 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -8,6 +8,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import ( CITY_SPEED_LIMIT, CRUISING_SPEED, + CSC_DEFAULT_APPROACH_DECEL, CSC_DEFAULT_MARGIN_PERCENT, DEFAULT_LATERAL_ACCELERATION, PLANNER_TIME, @@ -16,7 +17,9 @@ from openpilot.starpilot.common.starpilot_variables import ( CALIBRATION_PROGRESS_THRESHOLD = 10 / DT_MDL CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS -CSC_APPROACH_DECEL = 1.0 +# braking distance is (v^2 - v_curve^2) / (2 * this), so lower starts the slowdown +# sooner and spreads it further. User-tunable via CurveSpeedApproachDecel. +CSC_APPROACH_DECEL = CSC_DEFAULT_APPROACH_DECEL CSC_TARGET_UP_RATE = 3.0 CSC_TARGET_DOWN_RATE = 2.5 CSC_TARGET_FILTER_RC = 0.4 @@ -312,6 +315,11 @@ class CurveSpeedController: margin = getattr(self.starpilot_toggles, "csc_margin", None) return float(margin) if margin else CSC_COMFORT_MARGIN + @property + def approach_decel(self): + decel = getattr(self.starpilot_toggles, "csc_approach_decel", None) + return float(decel) if decel else CSC_APPROACH_DECEL + def lat_accel_for_curvature(self, curvature): lat_accel = np.interp(np.abs(curvature), self._curve_k, self._curve_a) * self.comfort_margin @@ -339,7 +347,7 @@ class CurveSpeedController: 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)) + allowed_speeds = np.sqrt(point_speeds**2 + 2.0 * self.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 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 a92a57aa2..9c928e085 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 @@ -757,6 +757,20 @@ "is_parent_toggle": true, "settings_tier": "simple" }, + { + "key": "CurveSpeedApproachDecel", + "label": "Curve Speed Approach Decel", + "description": "How hard the slowdown into a curve is planned. Lower starts it sooner and spreads it over more distance; higher waits longer and slows more firmly.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.3, + "max": 1.5, + "step": 0.1, + "precision": 1, + "unit": " m/s²", + "parent_key": "CurveSpeedController", + "settings_tier": "simple" + }, { "key": "CurveSpeedMargin", "label": "Curve Speed Margin", diff --git a/tools/longitudinal/analyze_csc.py b/tools/longitudinal/analyze_csc.py index 9d5ce9c0f..c26c4ad41 100644 --- a/tools/longitudinal/analyze_csc.py +++ b/tools/longitudinal/analyze_csc.py @@ -23,6 +23,7 @@ M_TO_MILES = 1.0 / 1609.34 HIGHWAY_SPEED = 60.0 / MS_TO_MPH # above this, engagement is the over-slowing regression risk CURVE_LAT_ACCEL = 1.3 # MINIMUM_LATERAL_ACCELERATION EPISODE_GAP_S = 1.0 +V_CRUISE_UNSET = 255 @dataclass @@ -35,11 +36,13 @@ class Frame: brake: bool = False accel_pressed: bool = False long_active: bool = False + blinker: bool = False # CSC gating input: a blinker suspends it entirely csc_active: bool = False csc_overridden: bool = False csc_training: bool = False csc_speed: float = 0.0 - v_cruise: float = 0.0 + v_cruise: float = 0.0 # applied cruise speed, already reduced by CSC + set_speed: float = 0.0 # what the driver dialled in, so cuts are measurable learned_lat_accel: float = 0.0 binding_distance: float = 0.0 @@ -66,17 +69,31 @@ class Episode: return self.end - self.start +def read_events(identifier: str): + """A downloaded rlog reads directly; anything else goes through LogReader.""" + path = Path(identifier) + if path.is_file(): + from cereal import log as capnp_log + + data = path.read_bytes() + if data[:4] == b"\x28\xb5\x2f\xfd": + import zstandard + data = zstandard.ZstdDecompressor().decompress(data, max_output_size=2 << 30) + return capnp_log.Event.read_multiple_bytes(data) + + from openpilot.tools.lib.logreader import LogReader, ReadMode # needs the device stack + + return LogReader(identifier, default_mode=ReadMode.AUTO, sort_by_time=True) + + def read_frames(identifier: str) -> list[Frame]: """Join carState/controlsState/starpilotPlan onto the plan's cadence.""" - 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: + for msg in read_events(identifier): which = msg.which() if which == "carState": cs = msg.carState @@ -84,6 +101,9 @@ def read_frames(identifier: str) -> list[Frame]: 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": @@ -123,15 +143,19 @@ def build_episodes(frames: list[Frame]) -> list[Episode]: 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) + 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: - # the cancel lands on the frame CSC releases, so look just past the end + # an override releases CSC on the same frame it registers, so the rejection + # always lands just past the end of the episode it rejected current.cancelled |= f.csc_overridden or f.accel_pressed + current.gas |= f.gas + current.brake |= f.brake return episodes