From 40372c63eec021f5159262617e3baa213c193031 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 27 Apr 2026 09:16:34 -0500 Subject: [PATCH] Force Stop 2.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Force Stop 2.0 — Smoother deceleration curve. On cars with the ADAS stop-sign signal, stops are even more confident; cars without dashboard stop sign signal will also benefit from a new deceleration curve and MPC handoff. Toggle off = stock behavior. --- .claude/settings.local.json | 9 ++ cereal/custom.capnp | 1 + common/params_keys.h | 1 + opendbc_repo/opendbc/car/hyundai/carstate.py | 5 ++ .../settings/starpilot/longitudinal.py | 8 ++ starpilot/common/starpilot_variables.py | 1 + .../lib/conditional_experimental_mode.py | 9 ++ starpilot/controls/lib/starpilot_vcruise.py | 90 +++++++++++++++++-- .../tools/device_settings_layout.json | 10 +++ .../ui/qt/offroad/longitudinal_settings.cc | 3 + .../ui/qt/offroad/longitudinal_settings.h | 2 +- 11 files changed, 131 insertions(+), 8 deletions(-) create mode 100644 .claude/settings.local.json diff --git a/.claude/settings.local.json b/.claude/settings.local.json new file mode 100644 index 000000000..a33783d0d --- /dev/null +++ b/.claude/settings.local.json @@ -0,0 +1,9 @@ +{ + "permissions": { + "allow": [ + "Bash(python3 -c \"import json; json.load\\(open\\(r'c:\\\\Users\\\\user\\\\Documents\\\\GitHub\\\\openpilot\\\\starpilot\\\\system\\\\the_pond\\\\assets\\\\components\\\\tools\\\\device_settings_layout.json'\\)\\)\")", + "Bash(python3 -m py_compile \"c:\\\\Users\\\\user\\\\Documents\\\\GitHub\\\\openpilot\\\\starpilot\\\\controls\\\\lib\\\\starpilot_vcruise.py\")", + "Bash(python3 -m py_compile \"c:\\\\Users\\\\user\\\\Documents\\\\GitHub\\\\openpilot\\\\selfdrive\\\\ui\\\\layouts\\\\settings\\\\starpilot\\\\longitudinal.py\")" + ] + } +} diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 957b840f0..6ac3a9954 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -85,6 +85,7 @@ struct StarPilotCarState @0xf35cc4560bbf6ec2 { modePressed @16 :Bool; customPressed @17 :Bool; alwaysOnLateralAllowed @18 :Bool; + dashboardStopSign @19 :UInt8; # 0 = no signal / platform doesn't publish } struct StarPilotDeviceState @0xda96579883444c35 { diff --git a/common/params_keys.h b/common/params_keys.h index e205428fd..241cb8953 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -276,6 +276,7 @@ inline static std::unordered_map keys = { {"ForceOffroad", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"ForceOnroad", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"ForceStops", {PERSISTENT, BOOL, "0", "0", 2}}, + {"ForceStopDistanceOffset", {PERSISTENT, INT, "0", "0", 2}}, {"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2}}, {"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}}, {"FPSCounter", {PERSISTENT, BOOL, "1", "0", 3}}, diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 5f4ffde32..3dd2b355b 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -386,6 +386,10 @@ class CarState(CarStateBase): fp_ret.modePressed = bool(self.mode_button) fp_ret.customPressed = bool(self.custom_button) + # ADAS camera dashboard stop-sign signal (CANFD HKG only). Registered as optional + # (freq=0); cars without it never publish, so the read returns 0 and the field stays 0. + fp_ret.dashboardStopSign = 1 if bool(cp_cam.vl["ADAS_0x380"]["STOP_SIGN"]) else 0 + return ret, fp_ret def get_can_parsers_canfd(self, CP): @@ -408,6 +412,7 @@ class CarState(CarStateBase): if CP.flags & HyundaiFlags.EV: msgs.append(("DRIVE_MODE_EV", 0)) # optional: not all CAN-FD EV variants publish drive mode msgs.append(("STEERING_WHEEL_MEDIA_BUTTONS", 0)) # optional: absent or slower on some CAN-FD variants + cam_msgs.append(("ADAS_0x380", 0)) # optional: dashboard stop-sign signal, only on ADAS-equipped HKG CANFD return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CanBus(CP).ECAN), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_msgs, CanBus(CP).CAM), diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 45fb18c1b..d4f1a7336 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -724,6 +724,14 @@ class StarPilotLongitudinalQOLLayout(StarPilotPanel): "color": "#597497", "visible": lambda: self._params.get_bool("QOLLongitudinal"), }, + { + "title": tr_noop("Force Stop Distance Offset"), + "type": "value", + "get_value": lambda: f"{self._params.get_int('ForceStopDistanceOffset'):+d} ft", + "on_click": lambda: self._show_int_selector("ForceStopDistanceOffset", -20, 20, " ft"), + "color": "#597497", + "visible": lambda: self._params.get_bool("QOLLongitudinal") and self._params.get_bool("ForceStops"), + }, { "title": tr_noop("Force Standstill State"), "type": "toggle", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index d52d47a99..8c2c87be8 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -997,6 +997,7 @@ class StarPilotVariables: toggle.cruise_increase = self.get_value("CustomCruise", cast=float, condition=(quality_of_life_longitudinal and not pcm_cruise)) toggle.cruise_increase_long = self.get_value("CustomCruiseLong", cast=float, condition=(quality_of_life_longitudinal and not pcm_cruise)) toggle.force_stops = self.get_value("ForceStops", condition=quality_of_life_longitudinal) + toggle.force_stop_distance_offset = self.get_value("ForceStopDistanceOffset", cast=int, condition=(quality_of_life_longitudinal and toggle.force_stops)) toggle.force_standstill = self.get_value("ForceStandstill", condition=quality_of_life_longitudinal) toggle.increase_stopped_distance = self.get_value("IncreasedStoppedDistance", cast=float, condition=quality_of_life_longitudinal, conversion=distance_conversion) map_gears = self.get_value("MapGears", condition=quality_of_life_longitudinal) diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index e6a265ee4..ec21db78f 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -170,6 +170,15 @@ class ConditionalExperimentalMode: self.slow_lead_detected = False def stop_sign_and_light(self, v_ego, sm, model_time): + # While the dashboard has confirmed a stop sign on this approach, pin CEM in EXP. + # Approaches routinely exceed the mode_hold_until/mode_false_since hysteresis (0.5s/0.25s), + # so without this the model briefly losing the sign drops CEM to CHILL and stalls the + # force-stop activation path. Latch is owned by starpilot_vcruise. + if getattr(self.starpilot_planner.starpilot_vcruise, 'stop_sign_confirmed', False): + self.stop_light_filter.x = 1.0 + self.stop_light_detected = True + return + if not sm["starpilotCarState"].trafficModeEnabled: speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 0584fe860..7f4746c92 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -1,4 +1,6 @@ #!/usr/bin/env python3 +import math + from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL @@ -9,6 +11,21 @@ from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitCo CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS OVERRIDE_FORCE_STOP_TIMER = 10 +# Force-stop kinematic profile. The user tunes one signed knob (ForceStopDistanceOffset, +# in feet); positive = stop later/longer, negative = stop sooner/shorter. All other +# shape parameters are fixed constants converged from FP-Testing Sessions A-O. +COMFORT_DECEL = 1.0 # m/s^2 — kinematic decel ceiling +ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this +MPC_HANDOFF_M = 6.0 # m — below this, command 0 and let MPC finish the stop +ADAS_MAX_MS = 17.88 # 40 mph — cross-street ADAS guard +DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap + # tracked length closer when dashboard confirms a sign +FT_TO_M = 0.3048 + +# Knob bounds (mirror of UI slider; defense in depth) +OFFSET_FT_MIN = -20 +OFFSET_FT_MAX = 20 + class StarPilotVCruise: def __init__(self, StarPilotPlanner): @@ -23,19 +40,54 @@ class StarPilotVCruise: self.override_force_stop_timer = 0 self.force_stop_timer = 0.0 + # Kinematic distance estimator. Same attribute also published as + # starpilotPlan.forcingStopLength, so the existing reader keeps working. self.tracked_model_length = 0.0 + self.stop_sign_confirmed = False + + # ===== Main update ===== + def update(self, controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles): long_control_active = sm["carControl"].longActive - force_stop = self.starpilot_planner.starpilot_cem.stop_light_detected and controls_enabled and starpilot_toggles.force_stops - force_stop &= self.starpilot_planner.model_stopped - force_stop &= self.override_force_stop_timer <= 0 + # ----- Activation paths ----- + # CEM/model path: model predicted stop within ACTIVATION_M + cem_path = (self.starpilot_planner.starpilot_cem.stop_light_detected + and controls_enabled and starpilot_toggles.force_stops + and self.starpilot_planner.model_length < ACTIVATION_M + and self.override_force_stop_timer <= 0 + and not self.starpilot_planner.driving_in_curve) - self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0 + # Dashboard path: ADAS camera confirms a stop sign on our road. Field is 0 on + # platforms that don't publish ADAS_0x380, so dash_path is naturally inert there. + dash_value = sm["starpilotCarState"].dashboardStopSign + dash_active = dash_value > 0 + dash_path = (dash_active and controls_enabled and starpilot_toggles.force_stops + and v_ego < ADAS_MAX_MS + and self.override_force_stop_timer <= 0 + and not self.starpilot_planner.driving_in_curve + and not self.starpilot_planner.tracking_lead) - force_stop_enabled = self.force_stop_timer >= 1 + force_stop_active = cem_path or dash_path + # Latch on first dash frame so the CEM pin can fire and we don't release on + # transient dashboard dropouts. Cleared in the no-force-stop branch below. + if dash_path: + self.stop_sign_confirmed = True + + # Timer ramp. Faster commitment when the dashboard confirms. + if force_stop_active and not sm["carState"].standstill: + rate = DT_MDL * 2 if dash_active else DT_MDL + self.force_stop_timer = min(self.force_stop_timer + rate, 2.0) + else: + self.force_stop_timer = max(self.force_stop_timer - DT_MDL * 0.25, 0.0) + + force_stop_enabled = self.force_stop_timer >= 0.5 + # Stay committed across model dropouts until standstill + force_stop_enabled |= self.forcing_stop and not sm["carState"].standstill + + # Override: gas/accel pedal during an active force stop self.override_force_stop |= sm["carState"].gasPressed self.override_force_stop |= sm["starpilotCarState"].accelPressed self.override_force_stop &= force_stop_enabled @@ -45,6 +97,7 @@ class StarPilotVCruise: elif self.override_force_stop_timer > 0: self.override_force_stop_timer -= DT_MDL + # ----- Force standstill (independent sibling toggle) ----- force_standstill_enabled = controls_enabled and starpilot_toggles.force_standstill and sm["carState"].standstill if force_standstill_enabled: self.override_force_standstill |= sm["carState"].gasPressed @@ -91,6 +144,11 @@ class StarPilotVCruise: self.slc_offset = 0 self.slc_target = 0 + # Single tuning knob (signed feet -> meters). Defense clamp on top of UI bounds. + offset_ft_raw = int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0) + offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw)) + offset_m = offset_ft * FT_TO_M + if force_standstill_enabled and not self.override_force_standstill: self.forcing_stop = True self.tracked_model_length = 0.0 @@ -99,11 +157,29 @@ class StarPilotVCruise: elif force_stop_enabled and not self.override_force_stop: self.forcing_stop |= not sm["carState"].standstill - self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0) - v_cruise = min((self.tracked_model_length // PLANNER_TIME), v_cruise) + # Kinematic distance estimator (also published as forcingStopLength). + # Decay one-to-one with motion, clamp by current model_length so we adopt + # the model's view when it regains sight, and snap closer to DASH_SEED_M + # whenever the dashboard signal is active. + self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0) + self.tracked_model_length = min(self.tracked_model_length, self.starpilot_planner.model_length) + if dash_active: + self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) + + # Kinematic profile with user offset. Positive offset shifts the perceived + # line further down the road -> car rolls further before commanding 0. + effective_d = self.tracked_model_length + offset_m + if effective_d <= MPC_HANDOFF_M: + v_target = 0.0 + else: + v_target = math.sqrt(2.0 * COMFORT_DECEL * (effective_d - MPC_HANDOFF_M)) + + v_cruise = min(v_target, v_cruise) else: self.forcing_stop = False + # Latch is only meaningful during an active force-stop cycle + self.stop_sign_confirmed = False self.tracked_model_length = self.starpilot_planner.model_length diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index b3ab5263b..b52790930 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -1048,6 +1048,16 @@ "ui_type": "toggle", "parent_key": "QOLLongitudinal" }, + { + "key": "ForceStopDistanceOffset", + "label": "Force Stop Distance Offset (ft)", + "description": "Tune where Force Stops bring the car to rest. Positive values let the car roll further before stopping (longer stop, closer to the line). Negative values stop the car sooner (more buffer before the line).", + "data_type": "int", + "ui_type": "numeric", + "min": -20.0, + "max": 20.0, + "parent_key": "QOLLongitudinal" + }, { "key": "ForceStandstill", "label": "Force Standstill State", diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.cc b/starpilot/ui/qt/offroad/longitudinal_settings.cc index 558d848c3..472535bd2 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/starpilot/ui/qt/offroad/longitudinal_settings.cc @@ -157,6 +157,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * {"CustomCruise", tr("Cruise Interval"), tr("How much the set speed increases or decreases for each + or – cruise control button press."), ""}, {"CustomCruiseLong", tr("Cruise Interval (Hold)"), tr("How much the set speed increases or decreases while holding the + or – cruise control buttons."), ""}, {"ForceStops", tr("Force Stop at \"Detected\" Stop Lights/Signs"), tr("Force openpilot to stop whenever the driving model \"detects\" a red light or stop sign.

Disclaimer: openpilot does not explicitly detect traffic lights or stop signs. In \"Experimental Mode\", openpilot makes end-to-end driving decisions from camera input, which means it may stop even when there's no clear reason!"), ""}, + {"ForceStopDistanceOffset", tr("Force Stop Distance Offset"), tr("Tune where Force Stops bring the car to rest. Positive values let the car roll further before stopping (longer stop, closer to the line). Negative values stop the car sooner (more buffer before the line)."), ""}, {"ForceStandstill", tr("Force Standstill State"), tr("Keep openpilot in the standstill state until you press the gas pedal or the Resume/+ cruise button.

This applies to any engaged stop, not just red lights or stop signs."), ""}, {"IncreasedStoppedDistance", tr("Increase Stopped Distance by:"), tr("Add extra space when stopped behind vehicles. Increase for more room; decrease for shorter gaps."), ""}, {"MapGears", tr("Map Accel/Decel to Gears"), tr("Map the Acceleration or Deceleration profiles to the vehicle's \"Eco\" and \"Sport\" gear modes."), ""}, @@ -393,6 +394,8 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * longitudinalToggle = new StarPilotParamValueControl(param, title, desc, icon, 1, 99, tr(" mph")); } else if (param == "IncreasedStoppedDistance") { longitudinalToggle = new StarPilotParamValueControl(param, title, desc, icon, 0, 10, tr(" feet")); + } else if (param == "ForceStopDistanceOffset") { + longitudinalToggle = new StarPilotParamValueControl(param, title, desc, icon, -20, 20, tr(" feet")); } else if (param == "MapGears") { std::vector mapGearsToggles{"MapAcceleration", "MapDeceleration"}; std::vector mapGearsToggleNames{tr("Acceleration"), tr("Deceleration")}; diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.h b/starpilot/ui/qt/offroad/longitudinal_settings.h index 0ae714501..eea510c15 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.h +++ b/starpilot/ui/qt/offroad/longitudinal_settings.h @@ -34,7 +34,7 @@ private: QSet curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"}; QSet customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"}; QSet longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "HumanFollowing", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune"}; - QSet qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"}; + QSet qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"}; QSet relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"}; QSet speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"}; QSet speedLimitControllerOffsetsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7"};