mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-06-08 08:34:49 +08:00
Force Stop 2.0
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.
This commit is contained in:
whoisdomi
9
.claude/settings.local.json
Normal file
9
.claude/settings.local.json
Normal file
@@ -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\")"
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -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 {
|
||||
|
||||
@@ -276,6 +276,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -157,6 +157,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
|
||||
{"CustomCruise", tr("Cruise Interval"), tr("<b>How much the set speed increases or decreases</b> for each + or – cruise control button press."), ""},
|
||||
{"CustomCruiseLong", tr("Cruise Interval (Hold)"), tr("<b>How much the set speed increases or decreases while holding the + or – cruise control buttons.</b>"), ""},
|
||||
{"ForceStops", tr("Force Stop at \"Detected\" Stop Lights/Signs"), tr("<b>Force openpilot to stop whenever the driving model \"detects\" a red light or stop sign.</b><br><br><i><b>Disclaimer</b>: 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!</i>"), ""},
|
||||
{"ForceStopDistanceOffset", tr("Force Stop Distance Offset"), tr("<b>Tune where Force Stops bring the car to rest.</b> 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("<b>Keep openpilot in the standstill state until you press the gas pedal or the Resume/+ cruise button.</b><br><br>This applies to any engaged stop, not just red lights or stop signs."), ""},
|
||||
{"IncreasedStoppedDistance", tr("Increase Stopped Distance by:"), tr("<b>Add extra space when stopped behind vehicles.</b> Increase for more room; decrease for shorter gaps."), ""},
|
||||
{"MapGears", tr("Map Accel/Decel to Gears"), tr("<b>Map the Acceleration or Deceleration profiles to the vehicle's \"Eco\" and \"Sport\" gear modes.</b>"), ""},
|
||||
@@ -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<QString> mapGearsToggles{"MapAcceleration", "MapDeceleration"};
|
||||
std::vector<QString> mapGearsToggleNames{tr("Acceleration"), tr("Deceleration")};
|
||||
|
||||
@@ -34,7 +34,7 @@ private:
|
||||
QSet<QString> curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"};
|
||||
QSet<QString> customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"};
|
||||
QSet<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "HumanFollowing", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune"};
|
||||
QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"};
|
||||
QSet<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"};
|
||||
QSet<QString> relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"};
|
||||
QSet<QString> speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"};
|
||||
QSet<QString> speedLimitControllerOffsetsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7"};
|
||||
|
||||
Reference in New Issue
Block a user