mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-26 19:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

The Crimson Night

Browse Source
This commit is contained in:
firestar5683
2026-07-24 02:01:57 -05:00
parent 66af0216b9
commit 225e98ed58
46 changed files with 1386 additions and 13273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/libcommon.a
BIN
View File
Binary file not shown.
common/params_keys.h
+2
View File
@@ -44,6 +44,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ExperimentalLongitudinalEnabled", {PERSISTENT, BOOL}},
{"ExperimentalMode", {PERSISTENT, BOOL}},
{"ExperimentalModeConfirmed", {PERSISTENT, BOOL}},
{"LongitudinalModelPreference", {PERSISTENT, INT, "0", "0", 1, SETTINGS_SIMPLE}},
{"LongitudinalModelPreferenceOverride", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "-1", "-1"}},
{"PersistChillState", {PERSISTENT, BOOL, "0", "0", 1}},
{"PersistExperimentalState", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
{"PersistedCCStatus", {PERSISTENT, INT, "0", "0"}},
common/params_pyx.so
BIN
View File
Binary file not shown.
selfdrive/controls/controlsd.py
-1
View File
@@ -427,7 +427,6 @@ class Controls:
# accel PID loop
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode)
actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
self.starpilot_toggles, has_lead=long_plan.hasLead,
traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled,
selfdrive/controls/lib/lead_behavior.py
-176
View File
@@ -1,176 +0,0 @@
#!/usr/bin/env python3
from openpilot.common.constants import CV
HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED = 45. * CV.MPH_TO_MS
TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED = 52. * CV.MPH_TO_MS
TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL = 1.5
VISION_LEAD_TRACK_MIN_DISTANCE = 25.0
VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75
VISION_LEAD_TRACK_CLOSING_GAIN = 0.20
VISION_LEAD_TRACK_CLOSING_CAP = 2.50
VISION_LEAD_TRACK_EXIT_TIME_GAP = 2.30
VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET = 1.6
VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB = 0.70
VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB = 0.95
VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET = 1.1
VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN = 0.55
VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED = 20.0
VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED = 25.0
TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.40
TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.70
TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75
TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN = 1.05
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START = 2.75
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END = 3.10
TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET = 0.90
TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET = 1.60
TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.45
TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.55
RADARLESS_MATCHED_FOLLOW_MIN_SPEED = 22.0
RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED = 2.0
RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY = 0.95
RADARLESS_MATCHED_FOLLOW_HEADWAY_BELOW_TARGET = 0.35
RADARLESS_MATCHED_FOLLOW_HEADWAY_ABOVE_TARGET = 0.90
RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE = 0.35
RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB = 0.70
def _smoothstep(value: float, start: float, end: float) -> float:
factor = min(1.0, max(0.0, (float(value) - float(start)) / max(float(end) - float(start), 1e-3)))
return factor * factor * (3.0 - 2.0 * factor)
def should_track_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float,
v_ego: float, *, v_lead: float | None = None, radar: bool = False) -> bool:
if not lead_status:
return False
tracking_buffer = max(float(stop_distance), 4.0)
model_limit = float(model_length) + tracking_buffer
if radar:
return float(lead_distance) < model_limit
closing_speed = max(0.0, float(v_ego) - float(v_lead if v_lead is not None else v_ego))
vision_time_gap = VISION_LEAD_TRACK_BASE_TIME_GAP + min(closing_speed * VISION_LEAD_TRACK_CLOSING_GAIN,
VISION_LEAD_TRACK_CLOSING_CAP)
vision_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE, float(v_ego) * vision_time_gap + tracking_buffer)
return float(lead_distance) < min(model_limit, vision_limit)
def should_hold_tracked_vision_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float,
v_ego: float, *, model_prob: float,
y_rel: float, path_y: float = 0.0, radar: bool = False) -> bool:
if not lead_status or radar or float(model_prob) < VISION_LEAD_TRACK_EXIT_MIN_MODEL_PROB:
return False
if abs(float(y_rel) + float(path_y)) > VISION_LEAD_TRACK_EXIT_MAX_LATERAL_OFFSET:
return False
tracking_buffer = max(float(stop_distance), 4.0)
model_limit = float(model_length) + tracking_buffer
vision_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE,
float(v_ego) * VISION_LEAD_TRACK_EXIT_TIME_GAP + tracking_buffer)
if float(lead_distance) < min(model_limit, vision_exit_limit):
return True
path_relative_offset = abs(float(y_rel) + float(path_y))
if (float(model_prob) < VISION_LEAD_TRACK_CONTINUITY_MIN_MODEL_PROB or
path_relative_offset > VISION_LEAD_TRACK_CONTINUITY_MAX_LATERAL_OFFSET):
return False
speed_factor = 1.0 - _smoothstep(
v_ego,
VISION_LEAD_TRACK_CONTINUITY_FULL_SPEED,
VISION_LEAD_TRACK_CONTINUITY_FADE_SPEED,
)
continuity_time_gap = VISION_LEAD_TRACK_EXIT_TIME_GAP + VISION_LEAD_TRACK_CONTINUITY_TIME_GAP_GAIN * speed_factor
continuity_exit_limit = max(VISION_LEAD_TRACK_MIN_DISTANCE,
float(v_ego) * continuity_time_gap + tracking_buffer)
return float(lead_distance) < continuity_exit_limit
def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lead: float, t_follow: float, *,
radar: bool = False, lead_brake: float = 0.0,
lead_prob: float = 0.0,
min_speed: float = RADARLESS_MATCHED_FOLLOW_MIN_SPEED) -> bool:
if radar or float(t_follow) <= 0.0 or float(v_ego) < float(min_speed):
return False
if float(lead_prob) < RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB:
return False
if float(lead_brake) > RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE:
return False
relative_speed = float(v_ego) - float(v_lead)
if abs(relative_speed) > RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED:
return False
actual_headway = float(lead_distance) / max(float(v_ego), 1e-3)
min_headway = max(RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY,
float(t_follow) - RADARLESS_MATCHED_FOLLOW_HEADWAY_BELOW_TARGET)
max_headway = float(t_follow) + RADARLESS_MATCHED_FOLLOW_HEADWAY_ABOVE_TARGET
return min_headway <= actual_headway <= max_headway
def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float,
v_cruise: float | None = None, y_rel: float | None = None) -> float:
gap_error = lead_distance - desired_gap
actual_hw = lead_distance / max(v_ego, 1e-3)
desired_hw = desired_gap / max(v_ego, 1e-3)
headway_margin = actual_hw - desired_hw
if gap_error <= 0.0:
return 0.0
speed_factor = _smoothstep(v_ego, HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED, TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED)
cruise_factor = 1.0
if v_cruise is not None:
cruise_factor = _smoothstep(v_cruise - v_ego, 0.0, TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL)
if speed_factor == 0.0 or cruise_factor == 0.0:
return 0.0
# Encourage ACC to treat a tracked lead as the active constraint when we're
# hanging far above the requested time gap, but don't override cruise for a
# truly distant lead or one we're already closing on decisively.
fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN,
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw)
fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN,
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw)
entry_factor = _smoothstep(headway_margin,
TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN,
TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN)
exit_factor = 1.0 - _smoothstep(headway_margin, fade_start_margin, fade_end_margin)
closing_fade_end = max(2.5, 0.12 * v_ego)
closing_fade_start = max(1.75, 0.08 * v_ego)
closing_factor = 1.0 - _smoothstep(closing_speed, closing_fade_start, closing_fade_end)
lateral_factor = 1.0
if y_rel is not None:
lateral_offset = abs(float(y_rel))
lateral_factor = 1.0 - _smoothstep(lateral_offset,
TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET,
TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET)
bias_cap = max(10.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego)
return (min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * speed_factor * cruise_factor *
entry_factor * exit_factor * closing_factor * lateral_factor)
def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float,
closing_speed: float, following_lead: bool) -> bool:
actual_hw = lead_distance / max(v_ego, 1e-3)
desired_hw = desired_gap / max(v_ego, 1e-3)
if following_lead or v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED:
return False
# Don't coast if we're already materially above the requested headway.
if actual_hw > max(desired_hw + 0.15, 1.75):
return False
coast_window_open = lead_distance > desired_gap + max(4.0, 0.15 * v_ego)
coast_window_far = lead_distance < desired_gap + max(12.0, 0.60 * v_ego)
gentle_closing = 0.35 < closing_speed < max(1.35, 0.05 * v_ego)
ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6
return coast_window_open and coast_window_far and gentle_closing and ttc > 7.5 and lead_distance > desired_gap + 7.0
selfdrive/controls/lib/longcontrol.py
+1 -45
View File
@@ -4,7 +4,6 @@ from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
from openpilot.common.pid import PIDController
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes import LongControlVehicleTuning
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
@@ -17,7 +16,6 @@ LEAD_GAP_SETTLE_MAX_START_ACCEL = 0.25
MOVING_STOP_FOLLOW_MIN_GAP = 0.25
NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
MODE_TRANSITION_MAX_DECEL = 4.0
LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -114,7 +112,6 @@ class LongControl:
def __init__(self, CP):
self.CP = CP
self.long_control_state = LongCtrlState.off
self.experimental_mode = False
self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=1 / DT_CTRL)
@@ -122,38 +119,10 @@ class LongControl:
kf = getattr(CP.longitudinalTuning, 'kfDEPRECATED', 0.0)
self.feedforward_gain = kf if kf != 0.0 else 1.0
self.v_pid = 0.0
self._mode_setup()
self.last_output_accel = 0.0
self.stop_release_counter = 0
self.vehicle_tuning = LongControlVehicleTuning(CP)
def update_mpc_mode(self, experimental_mode):
new_mode = 'blended' if experimental_mode else 'acc'
if self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc':
self.mode_transition_timer = 0.0
if new_mode != self.current_mode:
self.prev_mode = self.current_mode
self.transitioning = True
self.mode_transition_timer = 0.0
self.mode_transition_filter.x = self.last_output_accel
self.current_mode = new_mode
if self.transitioning:
self.mode_transition_timer += DT_CTRL
if self.mode_transition_timer >= self.mode_transition_duration:
self.transitioning = False
def _mode_setup(self):
self.prev_mode = 'acc'
self.current_mode = 'acc'
self.mode_transition_filter = FirstOrderFilter(0.0, 0.5, DT_CTRL)
self.mode_transition_timer = 0.0
self.mode_transition_duration = 1.0
self.transitioning = False
def reset(self, preserve_stop_release=False):
self.pid.reset()
self.vehicle_tuning.reset()
@@ -271,7 +240,6 @@ class LongControl:
else: # LongCtrlState.pid
a_target = self.vehicle_tuning.shape_gm_truck_accel_target(a_target, CS.vEgo, should_stop)
error = a_target - CS.aEgo
self.update_mpc_mode(self.experimental_mode)
self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo)
self._trim_positive_overshoot_integrator(a_target, error, CS)
self.vehicle_tuning.trim_gm_truck_positive_hold_integrator(
@@ -292,19 +260,7 @@ class LongControl:
raw_output_accel = self.vehicle_tuning.apply_pedal_long_brake_bias(raw_output_accel, a_target, CS)
if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended':
if raw_output_accel < 0 and raw_output_accel < self.last_output_accel:
progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration)
# Soften transition at low urgency, but keep sharp for high decel
# 20% smoother for chill decel (lower exponent)
urgency = abs(raw_output_accel / -MODE_TRANSITION_MAX_DECEL)
urgency_smooth = min(1.0, urgency ** 0.4) # 20% smoother for chill decel
blend_factor = 1.0 - (1.0 - progress) * (1.0 - urgency_smooth)
output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * blend_factor
else:
output_accel = raw_output_accel
else:
output_accel = raw_output_accel
output_accel = raw_output_accel
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return self.last_output_accel
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Executable → Regular
+247 -859
View File
File diff suppressed because it is too large Load Diff
selfdrive/controls/lib/longitudinal_planner.py
Executable → Regular
+374 -3757
View File
File diff suppressed because it is too large Load Diff
selfdrive/controls/lib/longitudinal_vehicle_tunes.py
+24 -14
View File
@@ -1,18 +1,28 @@
HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0
HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0
HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35
from dataclasses import dataclass
def get_far_follow_output_slew_rates(CP):
if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G":
return (
HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE,
HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE,
)
return 0.0, 0.0
@dataclass(frozen=True)
class LongitudinalPlannerTune:
lead_filter_tau: float = 0.45
accel_slew_rate: float = 0.90
brake_slew_rate: float = 1.40
launch_accel: float = 0.35
def get_untracked_slow_lead_decel_scale(CP):
if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G":
return HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE
return 1.0
DEFAULT_TUNE = LongitudinalPlannerTune()
# Vehicle exceptions are deliberately declarative and limited to physical
# response differences. Planning and safety logic remain global.
VEHICLE_TUNES = {
("honda", "HONDA_HRV_3G"): LongitudinalPlannerTune(
lead_filter_tau=0.65,
accel_slew_rate=0.65,
brake_slew_rate=1.00,
launch_accel=0.30,
),
}
def get_longitudinal_planner_tune(CP):
key = (str(getattr(CP, "brand", "")), str(getattr(CP, "carFingerprint", "")))
return VEHICLE_TUNES.get(key, DEFAULT_TUNE)
selfdrive/controls/tests/test_conditional_chill_mode.py
-463
View File
@@ -1,463 +0,0 @@
from types import SimpleNamespace
from openpilot.common.constants import CV
from openpilot.starpilot.common.experimental_state import CCStatus
from openpilot.starpilot.controls.lib.conditional_chill_mode import ConditionalChillMode
class FakeParams:
def __init__(self, bools=None, ints=None):
self.bools = dict(bools or {})
self.ints = dict(ints or {})
def get_bool(self, key):
return bool(self.bools.get(key, False))
def put_bool(self, key, value):
self.bools[key] = bool(value)
def get_int(self, key, default=0):
return int(self.ints.get(key, default))
def put_int(self, key, value):
self.ints[key] = int(value)
class FakeDetector:
def __init__(self):
self.curve_detected = False
self.slow_lead_detected = False
self.stop_light_detected = False
self.stop_light_model_detected = False
def curve_detection(self, *_args, **_kwargs):
return None
def slow_lead(self, *_args, **_kwargs):
return None
def stop_sign_and_light(self, *_args, **_kwargs):
return None
class FakeSubMaster:
def __init__(self, services):
self.services = dict(services)
def __getitem__(self, key):
return self.services[key]
def make_sm():
return {
"carState": SimpleNamespace(standstill=False, leftBlinker=False, rightBlinker=False),
"selfdriveState": SimpleNamespace(enabled=True),
"longitudinalPlan": SimpleNamespace(allowThrottle=True, shouldStop=False),
"starpilotCarState": SimpleNamespace(trafficModeEnabled=False),
"starpilotPlan": SimpleNamespace(redLight=False, forcingStop=False),
"starpilotRadarState": SimpleNamespace(
leadLeft=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
leadRight=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
),
}
def make_submaster_sm():
return FakeSubMaster(make_sm())
def make_toggles():
return SimpleNamespace(
conditional_chill_speed=45 * CV.MPH_TO_MS,
conditional_chill_speed_lead=35 * CV.MPH_TO_MS,
conditional_chill_speed_margin=3 * CV.MPH_TO_MS,
conditional_chill_lead=True,
conditional_chill_launch_assist=False,
)
def make_ccm():
planner = SimpleNamespace(
params=FakeParams(),
params_memory=FakeParams(),
starpilot_vcruise=SimpleNamespace(
stop_sign_confirmed=False,
forcing_stop=False,
slc=SimpleNamespace(experimental_mode=False),
),
raw_model_stopped=False,
model_stopped=False,
tracking_lead=False,
lead_one=SimpleNamespace(
status=False,
dRel=float("inf"),
vLead=0.0,
vRel=0.0,
aLeadK=0.0,
modelProb=0.0,
radar=False,
),
)
detector = FakeDetector()
return planner, detector, ConditionalChillMode(planner, detector)
def test_ccm_stays_experimental_when_no_chill_condition_matches(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
ccm.update(20 * CV.MPH_TO_MS, 21 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_enters_chill_for_open_road_speed_recovery(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
monotonic_values = iter([10.0, 10.5])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
v_ego = 55 * CV.MPH_TO_MS
v_cruise = v_ego + 5 * CV.MPH_TO_MS
ccm.update(v_ego, v_cruise, sm, toggles)
ccm.update(v_ego, v_cruise, sm, toggles)
assert not ccm.experimental_mode
assert ccm.status_value == CCStatus["SPEED"]
assert planner.params_memory.get_int("CCStatus") == CCStatus["SPEED"]
def test_ccm_enters_chill_for_stable_lead_cruising(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
monotonic_values = iter([10.0, 11.1])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
planner.tracking_lead = True
planner.lead_one.status = True
planner.lead_one.dRel = 45.0
planner.lead_one.vLead = 24.8
planner.lead_one.radar = True
v_ego = 58 * CV.MPH_TO_MS
ccm.update(v_ego, v_ego, sm, toggles)
ccm.update(v_ego, v_ego, sm, toggles)
assert not ccm.experimental_mode
assert ccm.status_value == CCStatus["LEAD"]
def test_ccm_stable_lead_requires_longer_entry_debounce(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
monotonic_values = iter([10.0, 10.6])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
planner.tracking_lead = True
planner.lead_one.status = True
planner.lead_one.dRel = 45.0
planner.lead_one.vLead = 24.8
planner.lead_one.radar = True
v_ego = 58 * CV.MPH_TO_MS
ccm.update(v_ego, v_ego, sm, toggles)
ccm.update(v_ego, v_ego, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_hard_vetoes_force_experimental(monkeypatch):
planner, detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
v_ego = 60 * CV.MPH_TO_MS
v_cruise = v_ego + 5 * CV.MPH_TO_MS
veto_scenes = []
detector.slow_lead_detected = True
veto_scenes.append(("slow_lead", make_sm()))
detector.slow_lead_detected = False
traffic_sm = make_sm()
traffic_sm["starpilotCarState"].trafficModeEnabled = True
veto_scenes.append(("traffic_mode", traffic_sm))
adjacent_sm = make_sm()
adjacent_sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=25.0, vLead=12.0)
veto_scenes.append(("adjacent_lead", adjacent_sm))
for index, (_name, scene_sm) in enumerate(veto_scenes, start=1):
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda idx=index: float(idx))
if _name == "slow_lead":
detector.slow_lead_detected = True
else:
detector.slow_lead_detected = False
ccm.update(v_ego, v_cruise, scene_sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
planner.starpilot_vcruise.slc.experimental_mode = True
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0)
ccm.update(v_ego, v_cruise, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_far_lateral_adjacent_lead_does_not_block_open_road_chill(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
monotonic_values = iter([10.0, 10.5])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=6.5)
v_ego = 55 * CV.MPH_TO_MS
v_cruise = v_ego + 5 * CV.MPH_TO_MS
ccm.update(v_ego, v_cruise, sm, toggles)
ccm.update(v_ego, v_cruise, sm, toggles)
assert not ccm.experimental_mode
assert ccm.status_value == CCStatus["SPEED"]
def test_ccm_immediate_adjacent_lead_still_blocks_open_road_chill(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0)
sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=4.5)
v_ego = 55 * CV.MPH_TO_MS
v_cruise = v_ego + 5 * CV.MPH_TO_MS
ccm.update(v_ego, v_cruise, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_immediately_exits_chill_when_scene_turns_into_slow_lead(monkeypatch):
planner, detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
monotonic_values = iter([10.0, 11.1, 11.2])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
planner.tracking_lead = True
planner.lead_one.status = True
planner.lead_one.dRel = 40.0
planner.lead_one.vLead = 24.9
planner.lead_one.radar = True
v_ego = 58 * CV.MPH_TO_MS
ccm.update(v_ego, v_ego, sm, toggles)
ccm.update(v_ego, v_ego, sm, toggles)
assert not ccm.experimental_mode
detector.slow_lead_detected = True
ccm.update(v_ego, v_ego, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_launch_assist_enters_chill_from_standstill_when_planner_wants_to_go(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
sm["carState"].standstill = True
toggles = make_toggles()
toggles.conditional_chill_launch_assist = True
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 20.0)
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert not ccm.experimental_mode
assert ccm.status_value == CCStatus["SPEED"]
def test_ccm_launch_assist_does_not_bypass_real_stop_scene(monkeypatch):
planner, detector, ccm = make_ccm()
sm = make_sm()
sm["carState"].standstill = True
sm["longitudinalPlan"].shouldStop = True
toggles = make_toggles()
toggles.conditional_chill_launch_assist = True
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 21.0)
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
detector.stop_light_detected = True
sm["longitudinalPlan"].shouldStop = False
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_launch_assist_rejects_red_light_stationary_lead_even_if_planner_says_go(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
sm["carState"].standstill = True
sm["starpilotPlan"].redLight = True
toggles = make_toggles()
toggles.conditional_chill_launch_assist = True
planner.tracking_lead = True
planner.lead_one.status = True
planner.lead_one.dRel = 6.1
planner.lead_one.vLead = 0.01
planner.lead_one.vRel = -0.08
planner.lead_one.aLeadK = 0.0
planner.lead_one.radar = True
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 22.0)
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_launch_assist_requires_real_lead_departure_signal(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
sm["carState"].standstill = True
toggles = make_toggles()
toggles.conditional_chill_launch_assist = True
planner.tracking_lead = True
planner.lead_one.status = True
planner.lead_one.dRel = 18.0
planner.lead_one.vLead = 0.12
planner.lead_one.vRel = 0.05
planner.lead_one.aLeadK = 0.02
planner.lead_one.radar = True
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.0)
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
planner.lead_one.vLead = 0.8
planner.lead_one.vRel = 0.5
planner.lead_one.aLeadK = 0.2
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.2)
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert not ccm.experimental_mode
assert ccm.status_value == CCStatus["LEAD"]
def test_ccm_launch_assist_exits_once_launch_speed_is_reached(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
sm["carState"].standstill = True
toggles = make_toggles()
toggles.conditional_chill_launch_assist = True
monotonic_values = iter([30.0, 30.2])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert not ccm.experimental_mode
sm["carState"].standstill = False
ccm.update(16 * CV.MPH_TO_MS, 25 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_launch_assist_exits_immediately_if_lead_slows_again(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
sm["carState"].standstill = True
toggles = make_toggles()
toggles.conditional_chill_launch_assist = True
monotonic_values = iter([40.0, 40.1])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values))
planner.tracking_lead = True
planner.lead_one.status = True
planner.lead_one.dRel = 18.0
planner.lead_one.vLead = 2.0
planner.lead_one.vRel = 2.0
planner.lead_one.aLeadK = 0.2
planner.lead_one.radar = True
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert not ccm.experimental_mode
assert ccm.status_value == CCStatus["LEAD"]
sm["carState"].standstill = False
planner.lead_one.vLead = 0.2
planner.lead_one.vRel = 0.0
planner.lead_one.aLeadK = -0.4
ccm.update(3 * CV.MPH_TO_MS, 25 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_respects_manual_chill_override(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
planner.params_memory.put_int("CCStatus", CCStatus["USER_CHILL"])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles)
assert not ccm.experimental_mode
assert ccm.status_value == CCStatus["USER_CHILL"]
def test_ccm_launch_assist_is_disabled_by_default(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
sm["carState"].standstill = True
toggles = make_toggles()
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 50.0)
ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
def test_ccm_restores_persisted_manual_experimental_override(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_sm()
toggles = make_toggles()
planner.params.put_bool("PersistChillState", True)
planner.params.put_int("PersistedCCStatus", CCStatus["USER_EXPERIMENTAL"])
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 1.0)
ccm.update(55 * CV.MPH_TO_MS, 60 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["USER_EXPERIMENTAL"]
assert planner.params_memory.get_int("CCStatus") == CCStatus["USER_EXPERIMENTAL"]
def test_ccm_adjacent_lead_veto_works_with_submaster_like_input(monkeypatch):
planner, _detector, ccm = make_ccm()
sm = make_submaster_sm()
toggles = make_toggles()
sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=20.0, vLead=12.0)
monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0)
ccm.update(60 * CV.MPH_TO_MS, 65 * CV.MPH_TO_MS, sm, toggles)
assert ccm.experimental_mode
assert ccm.status_value == CCStatus["OFF"]
selfdrive/controls/tests/test_conditional_experimental_mode.py
-1115
View File
File diff suppressed because it is too large Load Diff
selfdrive/controls/tests/test_following_distance.py
+1 -1
View File
@@ -30,7 +30,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personalit
[log.LongitudinalPersonality.relaxed, # personality
log.LongitudinalPersonality.standard,
log.LongitudinalPersonality.aggressive],
[0,10,35])) # speed
[1,10,35])) # speed; 0 m/s cannot converge from an arbitrary initial gap
class TestFollowingDistance:
def test_following_distance(self):
v_lead = float(self.speed)
selfdrive/controls/tests/test_lead_behavior.py
-196
View File
@@ -1,196 +0,0 @@
from openpilot.selfdrive.controls.lib.lead_behavior import (
get_tracked_lead_catchup_bias,
is_radarless_matched_follow_window,
should_hold_tracked_vision_lead,
should_track_lead,
should_disable_far_lead_throttle,
)
def test_tracked_lead_catchup_bias_for_hanging_gap():
bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1)
assert bias > 10.0
def test_tracked_lead_catchup_bias_ignores_near_desired_gap():
bias = get_tracked_lead_catchup_bias(31.4, 50.0, 38.0, 0.1)
assert bias == 0.0
def test_tracked_lead_catchup_bias_ignores_very_far_gap():
bias = get_tracked_lead_catchup_bias(31.4, 110.0, 38.0, 0.1)
assert bias == 0.0
def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap():
bias = get_tracked_lead_catchup_bias(30.4, 63.0, 40.0, 0.4)
assert bias > 9.0
def test_tracked_lead_catchup_bias_reduces_for_laterally_offset_lead():
centered = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=0.2)
offset = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9, y_rel=1.95)
assert centered > 0.0
assert offset == 0.0
def test_tracked_lead_catchup_bias_fades_before_very_far_gap_cutoff():
near_upper = get_tracked_lead_catchup_bias(34.0, 103.0, 73.0, 1.9)
smaller_gap = get_tracked_lead_catchup_bias(34.0, 96.0, 73.0, 1.9)
assert near_upper > 0.0
assert near_upper < smaller_gap
def test_tracked_lead_catchup_bias_stays_off_once_at_set_speed():
bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1, v_cruise=31.4)
assert bias == 0.0
def test_tracked_lead_catchup_bias_fades_smoothly_near_set_speed():
below_set = get_tracked_lead_catchup_bias(27.70, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8)
at_set = get_tracked_lead_catchup_bias(27.78, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8)
assert 0.0 < below_set < 0.25
assert at_set == 0.0
def test_tracked_lead_catchup_bias_dials_back_camry_bookmark_case():
bias = get_tracked_lead_catchup_bias(27.40, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8)
assert 0.0 < bias < 2.0
def test_tracked_lead_catchup_bias_fades_smoothly_at_closing_limit():
below_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.75)
above_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.77)
assert abs(below_limit - above_limit) < 0.1
def test_disable_far_lead_throttle_rejects_two_second_plus_gap():
should_disable = should_disable_far_lead_throttle(31.4, 78.7, 38.0, 0.1, False)
assert not should_disable
def test_disable_far_lead_throttle_keeps_mild_coast_near_target_gap():
should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 0.5, False)
assert should_disable
def test_disable_far_lead_throttle_rejects_fast_closing():
should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 3.5, False)
assert not should_disable
def test_disable_far_lead_throttle_rejects_route_like_highway_stab_case():
should_disable = should_disable_far_lead_throttle(34.69, 68.5, 63.0, 2.31, False)
assert not should_disable
def test_disable_far_lead_throttle_rejects_large_gap_near_pace_matched_case():
should_disable = should_disable_far_lead_throttle(32.43, 72.4, 56.0, 1.30, False)
assert not should_disable
def test_should_track_lead_keeps_radar_leads_on_model_horizon():
assert should_track_lead(True, 95.0, 100.0, 6.0, 30.0, v_lead=25.0, radar=True)
def test_should_track_lead_rejects_far_vision_only_highway_lead():
assert not should_track_lead(True, 82.0, 140.0, 6.0, 29.0, v_lead=25.0, radar=False)
def test_should_track_lead_accepts_closer_vision_only_highway_lead():
assert should_track_lead(True, 56.0, 140.0, 6.0, 29.0, v_lead=25.0, radar=False)
def test_should_track_lead_accepts_fast_closing_vision_lead_early():
assert should_track_lead(True, 90.0, 140.0, 6.0, 20.0, v_lead=0.0, radar=False)
def test_should_hold_tracked_vision_lead_keeps_honda_bookmark_case():
assert should_hold_tracked_vision_lead(
True, 44.5, 174.0, 6.0, 16.8,
model_prob=0.99, y_rel=-0.69, radar=False,
)
def test_should_hold_tracked_vision_lead_does_not_expand_initial_tracking_gate():
assert not should_track_lead(True, 44.5, 174.0, 6.0, 16.8, v_lead=16.7, radar=False)
def test_should_hold_tracked_vision_lead_releases_offcenter_lead():
assert not should_hold_tracked_vision_lead(
True, 44.5, 174.0, 6.0, 16.8,
model_prob=0.99, y_rel=-1.7, radar=False,
)
def test_should_hold_tracked_vision_lead_uses_path_relative_offset_on_curve():
assert should_hold_tracked_vision_lead(
True, 27.8, 174.0, 6.0, 16.0,
model_prob=1.0, y_rel=-2.11, path_y=1.32, radar=False,
)
def test_should_hold_tracked_vision_lead_releases_low_confidence_lead():
assert not should_hold_tracked_vision_lead(
True, 44.5, 174.0, 6.0, 16.8,
model_prob=0.69, y_rel=0.0, radar=False,
)
def test_should_hold_tracked_vision_lead_does_not_change_radar_tracking():
assert not should_hold_tracked_vision_lead(
True, 44.5, 174.0, 6.0, 16.8,
model_prob=0.99, y_rel=0.0, radar=True,
)
def test_should_hold_tracked_vision_lead_keeps_braking_lead():
assert should_hold_tracked_vision_lead(
True, 44.5, 174.0, 6.0, 16.8,
model_prob=0.99, y_rel=0.0, radar=False,
)
def test_should_hold_tracked_vision_lead_releases_beyond_exit_gap():
assert not should_hold_tracked_vision_lead(
True, 57.0, 174.0, 6.0, 16.8,
model_prob=0.99, y_rel=0.0, radar=False,
)
def test_should_hold_tracked_vision_lead_ignores_shortened_model_horizon_in_bolt_stutter_case():
assert should_hold_tracked_vision_lead(
True, 54.6, 40.0, 6.0, 19.4,
model_prob=1.0, y_rel=0.05, radar=False,
)
def test_should_hold_tracked_vision_lead_does_not_extend_low_confidence_short_horizon_case():
assert not should_hold_tracked_vision_lead(
True, 54.6, 40.0, 6.0, 19.4,
model_prob=0.90, y_rel=0.05, radar=False,
)
def test_radarless_matched_follow_window_accepts_pace_matched_highway_follow():
assert is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95)
def test_radarless_matched_follow_window_rejects_large_relative_speed():
assert not is_radarless_matched_follow_window(31.0, 48.0, 27.5, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95)
def test_radarless_matched_follow_window_rejects_far_headway():
assert not is_radarless_matched_follow_window(31.0, 82.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.95)
def test_radarless_matched_follow_window_rejects_low_confidence_lead():
assert not is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.55)
def test_radarless_matched_follow_window_keeps_default_low_speed_guard():
assert not is_radarless_matched_follow_window(14.4, 25.4, 16.2, 1.25, radar=False, lead_brake=0.0, lead_prob=1.0)
def test_radarless_matched_follow_window_accepts_lower_speed_when_requested():
assert is_radarless_matched_follow_window(14.4, 25.4, 16.2, 1.25, radar=False, lead_brake=0.0, lead_prob=1.0, min_speed=12.0)
selfdrive/controls/tests/test_longitudinal_planner.py
+249 -4576
View File
File diff suppressed because it is too large Load Diff
selfdrive/controls/tests/test_starpilot_acceleration.py
+2 -3
View File
@@ -18,7 +18,7 @@ from openpilot.starpilot.controls.lib.starpilot_acceleration import (
class FakePlanner:
def __init__(self, *, v_cruise=0.0, slc_target=0.0, slc_offset=0.0, overridden_speed=0.0,
red_light=False, forcing_stop=False, disable_throttle=False):
red_light=False, forcing_stop=False):
self.v_cruise = v_cruise
self.starpilot_weather = SimpleNamespace(weather_id=0, reduce_acceleration=0.0)
self.starpilot_vcruise = SimpleNamespace(
@@ -27,8 +27,7 @@ class FakePlanner:
forcing_stop=forcing_stop,
slc=SimpleNamespace(overridden_speed=overridden_speed),
)
self.starpilot_cem = SimpleNamespace(stop_light_detected=red_light)
self.starpilot_following = SimpleNamespace(disable_throttle=disable_throttle)
self.longitudinal_intent = SimpleNamespace(stop_detected=red_light)
def make_toggles(**overrides):
selfdrive/controls/tests/test_starpilot_planner.py
+1 -69
View File
@@ -21,7 +21,6 @@ class FakeSM(dict):
def make_toggles(**overrides):
defaults = {
"compass": False,
"conditional_experimental_mode": False,
"minimum_lane_change_speed": 100.0,
"pause_lateral_below_speed": 10.0,
"pause_lateral_below_signal": True,
@@ -61,8 +60,7 @@ def make_planner(monkeypatch):
monkeypatch.setattr(planner.starpilot_following, "update", lambda *args, **kwargs: None)
monkeypatch.setattr(planner.starpilot_vcruise, "update", lambda *args, **kwargs: 0.0)
monkeypatch.setattr(planner.starpilot_weather, "update_weather", lambda *args, **kwargs: None)
monkeypatch.setattr(planner.starpilot_cem, "stop_sign_and_light", lambda *args, **kwargs: None)
monkeypatch.setattr(planner, "update_lead_status", lambda *args, **kwargs: False)
monkeypatch.setattr(planner.longitudinal_intent, "update", lambda *args, **kwargs: None)
return planner
@@ -81,7 +79,6 @@ def test_lateral_resume_delay_zero_keeps_immediate_resume(monkeypatch):
finally:
planner.shutdown()
def test_lateral_resume_delay_holds_resume_after_low_speed_turn(monkeypatch):
planner = make_planner(monkeypatch)
@@ -116,68 +113,3 @@ def test_lateral_resume_delay_ignores_signal_cycles_that_never_slow_enough(monke
assert planner.blinker_delay_active is False
finally:
planner.shutdown()
def test_radarless_follow_hold_applies_to_tracked_vision_lead(monkeypatch):
planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager())
try:
monkeypatch.setattr(starpilot_planner_module.time, "monotonic", lambda: 100.0)
planner.model_length = 30.0
planner.tracking_lead = True
planner.starpilot_following.t_follow = 1.45
planner.lead_one = SimpleNamespace(
status=True,
dRel=46.0,
vLead=27.0,
aLeadK=0.0,
modelProb=0.98,
radar=False,
)
planner.update_lead_status(27.5)
assert planner.radarless_follow_hold_until > 100.0
finally:
planner.shutdown()
def test_tracked_vision_lead_uses_exit_hysteresis_at_mid_speed():
planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager())
try:
planner.model_length = 174.0
planner.tracking_lead = True
planner.tracking_lead_filter.x = 1.0
planner.lead_one = SimpleNamespace(
status=True,
dRel=44.5,
vLead=16.7,
yRel=-0.69,
aLeadK=0.0,
modelProb=0.99,
radar=False,
)
assert planner.update_lead_status(16.8)
finally:
planner.shutdown()
def test_untracked_vision_lead_still_uses_strict_entry_gate():
planner = StarPilotPlanner(Path("/tmp/nonexistent"), DummyThemeManager())
try:
planner.model_length = 174.0
planner.lead_one = SimpleNamespace(
status=True,
dRel=44.5,
vLead=16.7,
yRel=-0.69,
aLeadK=0.0,
modelProb=0.99,
radar=False,
)
assert not planner.update_lead_status(16.8)
finally:
planner.shutdown()
selfdrive/controls/tests/test_starpilot_vcruise.py
+5 -5
View File
@@ -34,7 +34,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
params=FakeParams(),
params_memory=FakeParams({"NavInstructionState": nav_state or {}}),
lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
starpilot_cem=SimpleNamespace(stop_light_detected=red_light),
longitudinal_intent=SimpleNamespace(stop_detected=red_light),
tracking_lead=False,
driving_in_curve=False,
model_length=60.0,
@@ -432,9 +432,9 @@ def test_stop_then_turn_override_releases_after_stop_seen_window_expires():
sm["carState"].steeringAngleDeg = 30.0
toggles = make_toggles()
planner.starpilot_cem.stop_light_detected = True
planner.longitudinal_intent.stop_detected = True
update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=7.0)
planner.starpilot_cem.stop_light_detected = False
planner.longitudinal_intent.stop_detected = False
now = FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME + 0.5
for frame in range(12):
@@ -521,7 +521,7 @@ def test_standstill_seeded_force_stop_hold_requires_clear_window_before_release(
assert first == pytest.approx(0.0)
assert vcruise.standstill_force_stop_hold
planner.starpilot_cem.stop_light_detected = False
planner.longitudinal_intent.stop_detected = False
second = vcruise.update(
controls_enabled=True,
now=0.4,
@@ -567,7 +567,7 @@ def test_standstill_seeded_force_stop_hold_accepts_datetime_now_without_crashing
assert first == pytest.approx(0.0)
assert vcruise.standstill_force_stop_hold
planner.starpilot_cem.stop_light_detected = False
planner.longitudinal_intent.stop_detected = False
second = vcruise.update(
controls_enabled=True,
now=base + datetime.timedelta(seconds=0.4),
selfdrive/controls/tests/test_unified_longitudinal_intent.py
+159
View File
@@ -0,0 +1,159 @@
from types import SimpleNamespace
import pytest
from openpilot.starpilot.common.experimental_state import CEStatus
from openpilot.starpilot.controls.lib.unified_longitudinal_intent import (
MODEL_STOP_ENTER,
UnifiedLongitudinalIntent,
)
class FakeParamsMemory:
def __init__(self):
self.values = {}
def put_int(self, key, value):
self.values[key] = value
def make_controller():
planner = SimpleNamespace(
params_memory=FakeParamsMemory(),
driving_in_curve=False,
road_curvature_detected=False,
starpilot_vcruise=SimpleNamespace(
forcing_stop=False,
stop_sign_confirmed=False,
slc=SimpleNamespace(experimental_mode=False),
),
)
return planner, UnifiedLongitudinalIntent(planner)
def make_sm(*, v_ego=10.0, should_stop=False, model_distance=100.0,
end_speed=10.0, traffic=False, lead_one=None, lead_two=None):
model = SimpleNamespace(
action=SimpleNamespace(shouldStop=should_stop),
position=SimpleNamespace(x=[0.0, model_distance]),
velocity=SimpleNamespace(x=[v_ego, end_speed]),
)
empty_lead = SimpleNamespace(status=False)
return {
"modelV2": model,
"carState": SimpleNamespace(
standstill=v_ego == 0.0,
leftBlinker=False,
rightBlinker=False,
steeringAngleDeg=0.0,
),
"radarState": SimpleNamespace(
leadOne=lead_one or empty_lead,
leadTwo=lead_two or empty_lead,
),
"starpilotCarState": SimpleNamespace(trafficModeEnabled=traffic),
}
def toggles(model_first=False):
return SimpleNamespace(longitudinal_model_preference=model_first)
def settle(controller, sm, *, v_ego=10.0, frames=20):
for _ in range(frames):
controller.update(v_ego, sm, toggles())
def test_open_road_has_no_stop_intent():
_, controller = make_controller()
sm = make_sm()
settle(controller, sm)
assert not controller.stop_detected
assert controller.status_value == CEStatus["OFF"]
def test_model_stop_is_filtered_then_latched():
_, controller = make_controller()
sm = make_sm(model_distance=5.0, end_speed=0.0)
while controller.stop_filter.x < MODEL_STOP_ENTER:
controller.update(10.0, sm, toggles())
assert controller.stop_detected
assert controller.status_value == CEStatus["STOP_LIGHT"]
def test_stop_intent_releases_with_hysteresis():
_, controller = make_controller()
stop_scene = make_sm(should_stop=True)
settle(controller, stop_scene)
assert controller.stop_detected
clear_scene = make_sm()
controller.update(10.0, clear_scene, toggles())
assert controller.stop_detected
settle(controller, clear_scene)
assert not controller.stop_detected
@pytest.mark.parametrize("hard_stop", ["forcing_stop", "stop_sign_confirmed"])
def test_explicit_stop_sources_pin_intent(hard_stop):
planner, controller = make_controller()
setattr(planner.starpilot_vcruise, hard_stop, True)
controller.update(5.0, make_sm(v_ego=5.0), toggles())
assert controller.stop_detected
assert controller.status_value == CEStatus["STOP_LIGHT"]
def test_committed_turn_vetoes_model_stop_false_positive():
planner, controller = make_controller()
planner.driving_in_curve = True
sm = make_sm(v_ego=4.0, should_stop=True)
sm["carState"].leftBlinker = True
sm["carState"].steeringAngleDeg = 60.0
settle(controller, sm, v_ego=4.0)
assert not controller.stop_detected
def test_traffic_mode_vetoes_model_stop_but_not_explicit_force_stop():
planner, controller = make_controller()
sm = make_sm(should_stop=True, traffic=True)
settle(controller, sm)
assert not controller.stop_detected
planner.starpilot_vcruise.forcing_stop = True
controller.update(10.0, sm, toggles())
assert controller.stop_detected
def test_either_credible_slow_lead_slot_sets_diagnostic_status():
_, controller = make_controller()
slow_lead = SimpleNamespace(
status=True,
dRel=25.0,
vLead=4.0,
modelProb=0.95,
radar=False,
)
sm = make_sm(v_ego=10.0, lead_two=slow_lead)
settle(controller, sm)
assert not controller.stop_detected
assert controller.status_value == CEStatus["LEAD"]
def test_model_first_preference_is_status_only_not_a_separate_controller():
planner, controller = make_controller()
controller.update(10.0, make_sm(), toggles(model_first=True))
assert not controller.stop_detected
assert controller.status_value == CEStatus["USER_OVERRIDDEN"]
assert planner.params_memory.values["CEStatus"] == CEStatus["USER_OVERRIDDEN"]
selfdrive/selfdrived/selfdrived.py
+1 -8
View File
@@ -736,10 +736,7 @@ class SelfdriveD:
self.starpilot_events.add_from_msg(self.sm['starpilotPlan'].starpilotEvents)
if self.starpilot_toggles.conditional_experimental_mode or getattr(self.starpilot_toggles, "conditional_chill_mode", False):
self.experimental_mode = self.sm['starpilotPlan'].experimentalMode
else:
self.experimental_mode |= self.sm['starpilotPlan'].experimentalMode
self.experimental_mode = False if self.safe_mode else self.sm['starpilotPlan'].experimentalMode
def data_sample(self):
_car_state = messaging.recv_one(self.car_state_sock)
@@ -885,10 +882,6 @@ class SelfdriveD:
self.is_metric = self.params.get_bool("IsMetric")
self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
if self.safe_mode:
self.experimental_mode = False
elif not self.starpilot_toggles.conditional_experimental_mode:
self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
self.personality = log.LongitudinalPersonality.relaxed if self.safe_mode else self.params.get("LongitudinalPersonality", return_default=True)
time.sleep(0.1)
selfdrive/test/longitudinal_maneuvers/maneuver.py
+8 -1
View File
@@ -25,6 +25,7 @@ class Maneuver:
self.e2e = kwargs.get("e2e", False)
self.personality = kwargs.get("personality", 0)
self.force_decel = kwargs.get("force_decel", False)
self.lead_move_started_at = None
self.duration = duration
self.title = title
@@ -68,7 +69,13 @@ class Maneuver:
print("Crashed!!!!")
valid = False
if self.ensure_start and log['v_rel'] > 0 and log['acceleration'] < 1e-3:
if self.ensure_start and speed_lead > 0 and self.lead_move_started_at is None:
self.lead_move_started_at = plant.current_time
start_wait_expired = (
self.lead_move_started_at is not None and
plant.current_time - self.lead_move_started_at > 0.5
)
if self.ensure_start and start_wait_expired and log['v_rel'] > 0 and log['acceleration'] < 1e-3:
print('LongitudinalPlanner not starting!')
valid = False
selfdrive/test/longitudinal_maneuvers/plant.py
+2 -32
View File
@@ -10,15 +10,11 @@ from types import SimpleNamespace
from cereal import log
import cereal.messaging as messaging
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import Ratekeeper, DT_MDL
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.lead_behavior import should_hold_tracked_vision_lead, should_track_lead
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
from openpilot.starpilot.common.starpilot_variables import THRESHOLD
class Plant:
@@ -91,7 +87,6 @@ class Plant:
self.e2e = e2e
self.personality = personality
self.force_decel = force_decel
self.tracking_lead_filter = FirstOrderFilter(0.0, 0.5, DT_MDL)
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
self.ts = 1. / self.rate
@@ -185,33 +180,8 @@ class Plant:
car_state.carState.vCruise = float(v_cruise * 3.6)
car_control.carControl.orientationNED = [0., float(pitch), 0.]
tracking_lead = bool(status)
if self.track_lead_with_gate:
tracking_candidate = should_track_lead(
status,
float(d_rel),
float(position.x[-1]) if len(position.x) else 0.0,
STOP_DISTANCE,
float(self.speed),
v_lead=float(v_lead),
radar=bool(self.only_radar),
)
continuity_candidate = self.tracking_lead_filter.x >= THRESHOLD * 0.6
if not tracking_candidate and continuity_candidate:
tracking_candidate = should_hold_tracked_vision_lead(
status,
float(d_rel),
float(position.x[-1]) if len(position.x) else 0.0,
STOP_DISTANCE,
float(self.speed),
model_prob=float(prob_lead),
y_rel=float(lead.yRel),
radar=bool(self.only_radar),
)
self.tracking_lead_filter.update(tracking_candidate)
tracking_lead = self.tracking_lead_filter.x >= THRESHOLD
else:
self.tracking_lead_filter.update(float(status))
# The planner must remain safe even if a legacy tracking diagnostic is false.
tracking_lead = bool(status and not self.track_lead_with_gate)
# ******** get controlsState messages for plotting ***
starpilot_plan = SimpleNamespace(
selfdrive/ui/layouts/settings/starpilot/longitudinal.py
+32 -262
View File
@@ -50,7 +50,6 @@ from openpilot.starpilot.common.accel_profile import (
normalize_acceleration_profile,
normalize_deceleration_profile,
)
from openpilot.starpilot.common.experimental_state import sync_persist_experimental_state, sync_persist_chill_state
from openpilot.common.constants import CV
@@ -94,8 +93,8 @@ class AdaptiveSpeedView(CardHubManagerView):
def _build_cards(self):
return [
{
"title": tr("Conditional Drive Mode"),
"desc": tr("Configure automated switching between Experimental and Chill Modes based on set conditions."),
"title": tr("Longitudinal Preference"),
"desc": tr("Choose whether set speed or the driving model leads on open roads. Lead and stop safety are identical."),
"icon": "steering",
"on_click": lambda: self._controller._navigate_to("ce"),
},
@@ -138,7 +137,7 @@ class LongitudinalManagerView(CardHubManagerView):
},
{
"title": tr("Adaptive Speed Controls"),
"desc": tr("Configure Curve Speed Controller and Conditional Experimental Mode triggers."),
"desc": tr("Configure Curve Speed Controller and the unified longitudinal preference."),
"icon": "display",
"on_click": lambda: self._controller._navigate_to("adaptive_speed"),
},
@@ -164,17 +163,15 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView):
def __init__(self, controller: StarPilotLongitudinalLayout):
super().__init__()
self._header_title = tr("Conditional Drive Mode")
self._header_title = tr("Longitudinal Preference")
self._controller = controller
self._init_segmented_control()
self._init_adjustors()
self._init_toggles()
def _init_segmented_control(self):
self._drive_mode_control = self._child(
AetherSegmentedControl(
[tr("OFF"), tr("Experimental"), tr("Chill")],
[tr("Set-Speed First"), tr("Model First")],
self._get_drive_mode_index,
self._on_drive_mode_change,
style=PANEL_STYLE,
@@ -183,67 +180,24 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView):
)
def _get_drive_mode_index(self):
if self._controller._params.get_bool("ConditionalExperimental"):
return 1
elif self._controller._params.get_bool("ConditionalChill"):
return 2
return 0
return int(self._controller._params.get_int("LongitudinalModelPreference"))
def _on_drive_mode_change(self, idx):
if idx == 0:
self._controller._params.put_bool("ConditionalExperimental", False)
self._controller._params.put_bool("ConditionalChill", False)
elif idx == 1:
self._controller._params.put_bool("ConditionalExperimental", True)
self._controller._params.put_bool("ConditionalChill", False)
elif idx == 2:
self._controller._params.put_bool("ConditionalExperimental", False)
self._controller._params.put_bool("ConditionalChill", True)
self._update_pagination()
self._controller._params.put_int("LongitudinalModelPreference", int(idx))
self._controller._params_memory.put_int("LongitudinalModelPreferenceOverride", -1)
def _init_toggles(self):
if PANEL_STYLE.toggle_row_mode:
self._toggle_grid = TileGrid(columns=1, padding=12, min_tile_height=TOGGLE_MIN_HEIGHT)
else:
self._toggle_grid = TileGrid(columns=2, padding=12, min_tile_height=130.0)
self._toggle_grid = TileGrid(columns=1, padding=12, min_tile_height=TOGGLE_MIN_HEIGHT)
self._child(self._toggle_grid)
self.register_page_grid(self._toggle_grid)
cem_defs = [
{"title": tr("Curves"), "subtitle": tr("Switch to Experimental Mode on open-road curves."), "get_state": lambda: self._controller._params.get_bool("CECurves"), "set_state": lambda v: self._controller._params.put_bool("CECurves", v)},
{"title": tr("Curves w/ Lead"), "subtitle": tr("Switch on curves even when following a lead."), "get_state": lambda: self._controller._params.get_bool("CECurvesLead"), "set_state": lambda v: self._controller._params.put_bool("CECurvesLead", v), "is_enabled": lambda: self._controller._params.get_bool("CECurves"), "disabled_label": tr("Turn on Curves first")},
{"title": tr("Stop Lights/Signs"), "subtitle": tr("Switch when openpilot detects a stop."), "get_state": lambda: self._controller._params.get_bool("CEStopLights"), "set_state": lambda v: self._controller._params.put_bool("CEStopLights", v)},
{"title": tr("Lead Ahead"), "subtitle": tr("Switch when a slower/stopped vehicle is ahead."), "get_state": lambda: self._controller._params.get_bool("CELead"), "set_state": lambda v: self._controller._params.put_bool("CELead", v)},
{"title": tr("Slower Lead"), "subtitle": tr("Switch specifically for slower leads."), "get_state": lambda: self._controller._params.get_bool("CESlowerLead"), "set_state": lambda v: self._controller._params.put_bool("CESlowerLead", v), "is_enabled": lambda: self._controller._params.get_bool("CELead"), "disabled_label": tr("Turn on Lead first")},
{"title": tr("Stopped Lead"), "subtitle": tr("Switch specifically for stopped leads."), "get_state": lambda: self._controller._params.get_bool("CEStoppedLead"), "set_state": lambda v: self._controller._params.put_bool("CEStoppedLead", v), "is_enabled": lambda: self._controller._params.get_bool("CELead"), "disabled_label": tr("Turn on Lead first")},
{"title": tr("Signal Lane Detect"), "subtitle": tr("Don't trigger on turn signal if lines are clear."), "get_state": lambda: self._controller._params.get_bool("CESignalLaneDetection"), "set_state": lambda v: self._controller._params.put_bool("CESignalLaneDetection", v), "is_enabled": lambda: self._controller._params.get_int("CESignalSpeed") > 0, "disabled_label": tr("Needs Turn Signal speed > 0")},
{"title": tr("Status Widget"), "subtitle": tr("Show condition trigger on the drive screen."), "get_state": lambda: self._controller._params.get_bool("ShowCEMStatus"), "set_state": lambda v: self._controller._params.put_bool("ShowCEMStatus", v)},
{"title": tr("Persist Exp State"), "subtitle": tr("Keep manual Experimental override through reboots."), "get_state": lambda: self._controller._params.get_bool("PersistExperimentalState"), "set_state": self._controller._set_persist_experimental_state},
]
ccm_defs = [
{"title": tr("Stable Lead Ahead"), "subtitle": tr("Switch to Chill Mode when following a steady lead."), "get_state": lambda: self._controller._params.get_bool("CCMLead"), "set_state": lambda v: self._controller._params.put_bool("CCMLead", v)},
{"title": tr("Launch Assist"), "subtitle": tr("Temporarily switch to Chill from a stop."), "get_state": lambda: self._controller._params.get_bool("CCMLaunchAssist"), "set_state": lambda v: self._controller._params.put_bool("CCMLaunchAssist", v)},
{"title": tr("Status Widget"), "subtitle": tr("Show condition trigger on the drive screen."), "get_state": lambda: self._controller._params.get_bool("ShowCCMStatus"), "set_state": lambda v: self._controller._params.put_bool("ShowCCMStatus", v)},
{"title": tr("Persist Chill State"), "subtitle": tr("Keep manual Chill override through reboots."), "get_state": lambda: self._controller._params.get_bool("PersistChillState"), "set_state": self._controller._set_persist_chill_state},
]
self._cem_toggle_defs = cem_defs
self._ccm_toggle_defs = ccm_defs
self._update_pagination()
def _update_pagination(self):
mode = self._get_drive_mode_index()
page_size = self._compute_page_size(TOGGLE_ROW_HEIGHT)
if mode == 1:
pages = [self._cem_toggle_defs[i:i+page_size] for i in range(0, len(self._cem_toggle_defs), page_size)]
self._set_toggle_pages(pages)
elif mode == 2:
pages = [self._ccm_toggle_defs[i:i+page_size] for i in range(0, len(self._ccm_toggle_defs), page_size)]
self._set_toggle_pages(pages)
else:
self._set_toggle_pages([])
self._set_toggle_pages([[
{
"title": tr("Status Widget"),
"subtitle": tr("Show the active longitudinal constraint on the drive screen."),
"get_state": lambda: self._controller._params.get_bool("ShowCEMStatus"),
"set_state": lambda value: self._controller._params.put_bool("ShowCEMStatus", value),
},
]])
def _make_toggle_tile(self, info: dict) -> ToggleTile:
cls = RowToggleTile if PANEL_STYLE.toggle_row_mode else ToggleTile
@@ -261,189 +215,28 @@ class ConditionalDriveModeView(AdjustorTogglesPanelView):
return cls(**kwargs)
def _init_adjustors(self):
speed_unit = self._controller._speed_unit()
is_metric = self._controller._is_metric()
max_speed = 150.0 if is_metric else 100.0
specs = {
"CESpeed": {"title": tr("Below Speed"), "subtitle": tr("Switch to Experimental Mode below this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 20, 35, 55, 75], "labels": {}, "get": lambda: float(self._controller._params.get_int("CESpeed"))},
"CESpeedLead": {"title": tr("Speed w/ Lead"), "subtitle": tr("Switch below this speed when following a lead."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 20, 35, 55, 75], "labels": {}, "get": lambda: float(self._controller._params.get_int("CESpeedLead"))},
"CESignalSpeed": {"title": tr("Turn Signal Below"), "subtitle": tr("Switch when turn signal is on below this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 20, 35, 55, 75], "labels": {0.0: tr("Off")}, "get": lambda: float(self._controller._params.get_int("CESignalSpeed"))},
"CEModelStopTime": {"title": tr("Predicted Stop In"), "subtitle": tr("Switch when openpilot predicts a stop within time."), "min": 0, "max": 10.0, "step": 1.0, "unit": "s", "presets": [0, 3, 5, 7, 10], "labels": {0.0: tr("Off")}, "get": lambda: float(self._controller._params.get_int("CEModelStopTime"))},
"CCMSpeed": {"title": tr("Above Speed"), "subtitle": tr("Switch to Chill Mode on open roads above this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 35, 55, 65, 80], "labels": {}, "get": lambda: float(self._controller._params.get_int("CCMSpeed"))},
"CCMSpeedLead": {"title": tr("Speed w/ Lead"), "subtitle": tr("Switch when following a stable lead above this speed."), "min": 0, "max": max_speed, "step": 1.0, "unit": speed_unit, "presets": [0, 35, 55, 65, 80], "labels": {}, "get": lambda: float(self._controller._params.get_int("CCMSpeedLead"))},
"CCMSetSpeedMargin": {"title": tr("Set Speed Margin"), "subtitle": tr("How far below set speed before Chill engages."), "min": 0, "max": 30.0 if is_metric else 15.0, "step": 1.0, "unit": speed_unit, "presets": [0, 5, 10, 15], "labels": {}, "get": lambda: float(self._controller._params.get_int("CCMSetSpeedMargin"))},
}
self._cem_keys = ["CESpeed", "CESpeedLead", "CESignalSpeed", "CEModelStopTime"]
self._ccm_keys = ["CCMSpeed", "CCMSpeedLead", "CCMSetSpeedMargin"]
for key, spec in specs.items():
adjustor = self._child(AetherAdjustorRow(
spec["title"], spec["subtitle"], spec["min"], spec["max"], spec["step"],
get_value=spec["get"],
on_change=lambda _v: None,
on_commit=None,
unit=spec["unit"],
labels=spec["labels"],
presets=spec["presets"],
is_active=lambda: False,
set_active=lambda active, k=key: self._show_slider(k) if active else None,
style=PANEL_STYLE, color=PANEL_STYLE.accent
))
adjustor.set_touch_valid_callback(lambda: self._scroll_panel.is_touch_valid())
self._adjustor_rows[key] = adjustor
def _show_slider(self, key: str):
is_metric = self._controller._is_metric()
speed_unit = self._controller._speed_unit()
max_speed = 150.0 if is_metric else 100.0
specs = {
"CESpeed": {"title": tr("Below Speed"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 20, 35, 55, 75]},
"CESpeedLead": {"title": tr("Speed w/ Lead"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 20, 35, 55, 75]},
"CESignalSpeed": {"title": tr("Turn Signal Below"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {0.0: tr("Off")}, "presets": [0, 20, 35, 55, 75]},
"CEModelStopTime": {"title": tr("Predicted Stop In"), "min": 0, "max": 10.0, "unit": "s", "labels": {0.0: tr("Off")}, "presets": [0, 3, 5, 7, 10]},
"CCMSpeed": {"title": tr("Above Speed"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 35, 55, 65, 80]},
"CCMSpeedLead": {"title": tr("Speed w/ Lead"), "min": 0, "max": max_speed, "unit": speed_unit, "labels": {}, "presets": [0, 35, 55, 65, 80]},
"CCMSetSpeedMargin": {"title": tr("Set Speed Margin"), "min": 0, "max": 30.0 if is_metric else 15.0, "unit": speed_unit, "labels": {}, "presets": [0, 5, 10, 15]},
}
spec = specs[key]
original_val = float(self._controller._params.get_int(key))
def on_close(res, val):
if res == DialogResult.CONFIRM:
self._controller._params.put_int(key, int(val))
gui_app.push_widget(AetherSliderDialog(
title=spec["title"],
min_val=float(spec["min"]), max_val=float(spec["max"]), step=1.0,
current_val=original_val,
on_close=on_close, presets=[float(p) for p in spec["presets"]],
unit=spec["unit"], labels=spec["labels"], color=PANEL_STYLE.accent
))
def _draw_header(self, rect: rl.Rectangle):
pass
def _measure_content_height(self, content_width: float) -> float:
mode = self._get_drive_mode_index()
if mode == 0:
return self.TAB_HEIGHT + self.TAB_BOTTOM_GAP
keys = self._cem_keys if mode == 1 else self._ccm_keys
grid = self._toggle_grid
col_width = (content_width - SECTION_GAP) / 2 if self._uses_two_columns(content_width) else content_width
for key in keys:
self._adjustor_rows[key].custom_row_height = None
default_adjustor_h = float(AETHER_LIST_METRICS.adjustor_row_height)
left_h = len(keys) * default_adjustor_h + 16.0
num_tiles = 4 if self._has_pagination else len(grid.tiles)
if PANEL_STYLE.toggle_row_mode:
rows = num_tiles
tile_h = TOGGLE_ROW_HEIGHT
else:
rows = (num_tiles + 1) // 2 if self._uses_two_columns(content_width) else num_tiles
tile_h = grid.min_tile_height
pagination_space = 32.0 if self._has_pagination else 0.0
tiles_h = rows * tile_h + (rows - 1) * grid.gap + grid.gap * 2 + pagination_space
right_h = tiles_h
if self._uses_two_columns(content_width):
max_natural_h = max(left_h, right_h)
section_overhead = SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
if self._scroll_rect:
available_h = self._scroll_rect.height - section_overhead - self.TAB_HEIGHT - self.TAB_BOTTOM_GAP - 6.0
else:
available_h = max_natural_h
max_container_h = available_h
left_row_h = max(95.0, (max_container_h - 16.0) / max(1, len(keys)))
for key in keys:
self._adjustor_rows[key].custom_row_height = left_row_h
self._left_container_h = max_container_h
self._tiles_container_h = max_container_h
return self._compute_two_column_height(section_overhead + max_container_h) + self.TAB_HEIGHT + self.TAB_BOTTOM_GAP
else:
self._left_container_h = left_h
self._tiles_container_h = right_h
total = left_h + SECTION_GAP + right_h + SECTION_HEADER_HEIGHT * 2 + SECTION_HEADER_GAP * 2
return total + self.TAB_HEIGHT + self.TAB_BOTTOM_GAP
self._tiles_container_h = TOGGLE_ROW_HEIGHT + 24.0
return self.TAB_HEIGHT + self.TAB_BOTTOM_GAP + SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP + self._tiles_container_h
def _draw_scroll_content(self, rect: rl.Rectangle, content_width: float):
y = rect.y + self._scroll_offset
header_w = content_width
bar_rect = rl.Rectangle(rect.x, y, header_w, self.TAB_HEIGHT)
bar_rect = rl.Rectangle(rect.x, y, content_width, self.TAB_HEIGHT)
draw_list_group_shell(bar_rect, style=PANEL_STYLE)
self._drive_mode_control.render(bar_rect)
y += self.TAB_HEIGHT + self.TAB_BOTTOM_GAP
mode = self._get_drive_mode_index()
if mode == 0:
return
keys = self._cem_keys if mode == 1 else self._ccm_keys
grid = self._toggle_grid
col_width = (content_width - SECTION_GAP) / 2 if self._uses_two_columns(content_width) else content_width
draw_section_header(rl.Rectangle(rect.x, y, col_width, SECTION_HEADER_HEIGHT), tr("Values"), style=PANEL_STYLE)
if self._uses_two_columns(content_width):
draw_section_header(rl.Rectangle(rect.x + col_width + SECTION_GAP, y, col_width, SECTION_HEADER_HEIGHT), tr("Triggers"), style=PANEL_STYLE)
draw_section_header(rl.Rectangle(rect.x, y, content_width, SECTION_HEADER_HEIGHT), tr("Diagnostics"), style=PANEL_STYLE)
y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
self._draw_adjustors(y, rect.x, col_width, keys)
tg_columns = 1 if PANEL_STYLE.toggle_row_mode else 2
if self._uses_two_columns(content_width):
self._draw_two_column_tile_grid(
grid, rect.x + col_width + SECTION_GAP, y, col_width,
self._tiles_container_h, title=None, style=PANEL_STYLE,
columns=tg_columns)
else:
y += self._left_container_h + SECTION_GAP
draw_section_header(rl.Rectangle(rect.x, y, col_width, SECTION_HEADER_HEIGHT), tr("Triggers"), style=PANEL_STYLE)
y += SECTION_HEADER_HEIGHT + SECTION_HEADER_GAP
self._draw_two_column_tile_grid(
grid, rect.x, y, col_width,
self._tiles_container_h, title=None, style=PANEL_STYLE,
columns=tg_columns)
def _draw_adjustors(self, y: float, x: float, width: float, keys: list[str]):
draw_list_group_shell(rl.Rectangle(x, y, width, self._left_container_h), style=PANEL_STYLE)
current_y = y + 4
for index, key in enumerate(keys):
adjustor = self._adjustor_rows[key]
row_h = adjustor.measure_height(width)
row_rect = rl.Rectangle(x, current_y, width, row_h)
adjustor.set_is_last(index == len(keys) - 1)
adjustor.set_parent_rect(self._scroll_rect)
adjustor.render(row_rect)
current_y += row_h
self._draw_two_column_tile_grid(
self._toggle_grid, rect.x, y, content_width, self._tiles_container_h,
title=None, style=PANEL_STYLE, columns=1,
)
def _get_active_elements(self):
mode = self._get_drive_mode_index()
elems = [self._drive_mode_control]
if mode == 1:
elems += [self._adjustor_rows[k] for k in self._cem_keys]
elif mode == 2:
elems += [self._adjustor_rows[k] for k in self._ccm_keys]
elems.append(self._toggle_grid)
return elems
return [self._drive_mode_control, self._toggle_grid]
# ═══════════════════════════════════════════════════════════════
@@ -479,9 +272,6 @@ class StarPilotLongitudinalLayout(_SettingsPage):
def _build_view(self):
ol = lambda: starpilot_state.car_state.hasOpenpilotLongitudinal
ce_on = lambda: self._params.get_bool("ConditionalExperimental")
cc_on = lambda: self._params.get_bool("ConditionalChill")
ce_lead = lambda: ce_on() and self._params.get_bool("CELead")
csc_on = lambda: self._params.get_bool("CurveSpeedController")
confirmation_on = lambda: self._params.get_bool("SLCConfirmation")
@@ -659,7 +449,7 @@ class StarPilotLongitudinalLayout(_SettingsPage):
on_click=lambda k=key: self._show_slider(k, *self._speed_range(), unit=self._speed_unit()),
))
# ── 4. Adaptive Speed Controls Rows (CES + CSC + CCM) ──
# ── 4. Adaptive Speed Controls Rows ──
self._curve_speed_controller_rows = [
SettingRow("CalibratedLatAccel", "value", tr_noop("Calibrated Lateral Accel"),
subtitle=tr_noop("The learned lateral acceleration from collected driving data. Higher values allow faster cornering."),
@@ -945,37 +735,19 @@ class StarPilotLongitudinalLayout(_SettingsPage):
if state:
self._params.put_bool("EVTuning", False)
def _set_persist_experimental_state(self, state: bool):
sync_persist_experimental_state(self._params, self._params_memory, state)
def _set_persist_chill_state(self, state: bool):
sync_persist_chill_state(self._params, self._params_memory, state)
def _get_conditional_mode_label(self) -> str:
if self._params.get_bool("ConditionalExperimental"):
return tr("Conditional Experimental")
elif self._params.get_bool("ConditionalChill"):
return tr("Conditional Chill")
else:
return tr("OFF")
return tr("Model First") if self._params.get_int("LongitudinalModelPreference") else tr("Set-Speed First")
def _show_conditional_mode_selector(self):
options = ["OFF", "Conditional Experimental", "Conditional Chill"]
options = ["Set-Speed First", "Model First"]
current = self._get_conditional_mode_label()
def on_select(res):
if res == DialogResult.CONFIRM and dialog.selection:
if dialog.selection == "OFF":
self._params.put_bool("ConditionalExperimental", False)
self._params.put_bool("ConditionalChill", False)
elif dialog.selection == "Conditional Experimental":
self._params.put_bool("ConditionalExperimental", True)
self._params.put_bool("ConditionalChill", False)
elif dialog.selection == "Conditional Chill":
self._params.put_bool("ConditionalExperimental", False)
self._params.put_bool("ConditionalChill", True)
self._params.put_int("LongitudinalModelPreference", int(dialog.selection == "Model First"))
self._params_memory.put_int("LongitudinalModelPreferenceOverride", -1)
dialog = MultiOptionDialog(tr("Conditional Drive Mode"), options, current, callback=on_select)
dialog = MultiOptionDialog(tr("Longitudinal Preference"), options, current, callback=on_select)
gui_app.push_widget(dialog)
def _reset_curve_data(self):
@@ -1003,8 +775,6 @@ class StarPilotLongitudinalLayout(_SettingsPage):
_SPEED_RESCALE_KEYS = (
"Offset1", "Offset2", "Offset3", "Offset4", "Offset5", "Offset6", "Offset7",
"CustomCruise", "CustomCruiseLong", "SetSpeedOffset",
"CESpeed", "CESpeedLead", "CESignalSpeed",
"CCMSpeed", "CCMSpeedLead", "CCMSetSpeedMargin",
)
# Distance-typed int params stored in the current unit (ft or m); rescaled
selfdrive/ui/lib/mode_banner.py
+3 -45
View File
@@ -8,17 +8,11 @@ from openpilot.starpilot.common.experimental_state import requested_experimental
class ModeBannerVariant(StrEnum):
CHILL = "chill"
EXPERIMENTAL = "experimental"
CONDITIONAL_EXPERIMENTAL = "conditional_experimental"
CONDITIONAL_CHILL = "conditional_chill"
def get_mode_banner_variant(params, params_memory=None) -> ModeBannerVariant:
if params.get_bool("SafeMode"):
return ModeBannerVariant.CHILL
if params.get_bool("ConditionalExperimental"):
return ModeBannerVariant.CONDITIONAL_EXPERIMENTAL
if params.get_bool("ConditionalChill"):
return ModeBannerVariant.CONDITIONAL_CHILL
if requested_experimental_mode(params, params_memory):
return ModeBannerVariant.EXPERIMENTAL
return ModeBannerVariant.CHILL
@@ -38,29 +32,11 @@ def _lerp_color(start: rl.Color, end: rl.Color, progress: float, alpha: int) ->
)
def _conditional_colors(variant: ModeBannerVariant, alpha: int) -> tuple[rl.Color, rl.Color, rl.Color, rl.Color]:
blue = _color(35, 149, 255, alpha)
mint = _color(20, 255, 171, alpha)
orange = _color(255, 138, 22, alpha)
red = _color(219, 56, 34, alpha)
if variant == ModeBannerVariant.CONDITIONAL_EXPERIMENTAL:
return blue, mint, orange, red
return orange, red, blue, mint
def mode_banner_color(variant: ModeBannerVariant, progress: float, alpha: int = 255) -> rl.Color:
progress = max(0.0, min(1.0, progress))
if variant == ModeBannerVariant.CHILL:
return _lerp_color(_color(20, 255, 171, alpha), _color(35, 149, 255, alpha), progress, alpha)
if variant == ModeBannerVariant.EXPERIMENTAL:
return _lerp_color(_color(255, 155, 63, alpha), _color(219, 56, 34, alpha), progress, alpha)
dominant_start, dominant_end, target_start, target_end = _conditional_colors(variant, alpha)
if progress <= 0.58:
return _lerp_color(dominant_start, dominant_end, progress / 0.58, alpha)
if progress <= 0.80:
return _lerp_color(dominant_end, target_start, (progress - 0.58) / 0.22, alpha)
return _lerp_color(target_start, target_end, ((progress - 0.80) / 0.20) * 0.67, alpha)
return _lerp_color(_color(255, 155, 63, alpha), _color(219, 56, 34, alpha), progress, alpha)
def mode_atom_color(variant: ModeBannerVariant, progress: float, alpha: int = 255) -> rl.Color:
@@ -71,25 +47,7 @@ def mode_atom_color(variant: ModeBannerVariant, progress: float, alpha: int = 25
def draw_mode_banner_gradient(rect: rl.Rectangle, variant: ModeBannerVariant, alpha: int = 255) -> None:
if variant in (ModeBannerVariant.CHILL, ModeBannerVariant.EXPERIMENTAL):
rl.draw_rectangle_gradient_h(
int(rect.x), int(rect.y), int(rect.width), int(rect.height),
mode_banner_color(variant, 0.0, alpha), mode_banner_color(variant, 1.0, alpha),
)
return
transition_start = int(rect.x + rect.width * 0.58)
transition_end = int(rect.x + rect.width * 0.80)
right = int(rect.x + rect.width)
rl.draw_rectangle_gradient_h(
int(rect.x), int(rect.y), transition_start - int(rect.x), int(rect.height),
mode_banner_color(variant, 0.0, alpha), mode_banner_color(variant, 0.58, alpha),
)
rl.draw_rectangle_gradient_h(
transition_start, int(rect.y), transition_end - transition_start, int(rect.height),
mode_banner_color(variant, 0.58, alpha), mode_banner_color(variant, 0.80, alpha),
)
rl.draw_rectangle_gradient_h(
transition_end, int(rect.y), right - transition_end, int(rect.height),
mode_banner_color(variant, 0.80, alpha), mode_banner_color(variant, 1.0, alpha),
int(rect.x), int(rect.y), int(rect.width), int(rect.height),
mode_banner_color(variant, 0.0, alpha), mode_banner_color(variant, 1.0, alpha),
)
selfdrive/ui/lib/starpilot_status.py
+4 -16
View File
@@ -64,33 +64,21 @@ def get_screen_edge_color(state: UIState):
def get_experimental_mode_banner_text(state: UIState):
conditional_enabled = state.params.get_bool("ConditionalExperimental")
# With CEM enabled, only surface banner text for explicit manual override states.
# Automatic CEM transitions should only be reflected by path/border coloring.
if conditional_enabled:
if state.conditional_status in CEM_MANUAL_OVERRIDE_STATUSES:
return "OVERRIDDEN"
return None
if state.sm["selfdriveState"].experimentalMode:
return "EXPERIMENTAL"
return "CHILL"
return "MODEL FIRST"
return "SET-SPEED FIRST"
def get_mode_transition_banner_text(state: UIState):
enabled = state.sm["selfdriveState"].enabled
lateral_active = enabled or state.always_on_lateral_active
conditional_enabled = state.params.get_bool("ConditionalExperimental")
if state.status == UIStatus.OVERRIDE:
return "OVERRIDE"
if state.switchback_mode_enabled and lateral_active:
return "SWITCHBACK"
if conditional_enabled and enabled and state.conditional_status in CEM_MANUAL_OVERRIDE_STATUSES:
return "OVERRIDDEN"
if enabled and state.sm["selfdriveState"].experimentalMode:
return "EXPERIMENTAL"
return "MODEL FIRST"
if enabled:
return "CHILL"
return "SET-SPEED FIRST"
return None
selfdrive/ui/onroad/exp_button.py
+6 -21
View File
@@ -6,9 +6,8 @@ from openpilot.system.ui.lib.application import gui_app
from openpilot.system.ui.widgets import Widget
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.starpilot.common.experimental_state import (
CEStatus,
next_manual_ce_status,
sync_manual_ce_state,
requested_experimental_mode,
toggle_longitudinal_model_preference,
)
@@ -35,7 +34,6 @@ class ExpButton(Widget):
"disengaged": rl.Color(0, 0, 0, 166),
"switchback": rl.Color(0x8b, 0x6c, 0xc5, 255),
"aol": rl.Color(0x0a, 0xba, 0xb5, 255),
"cem_disabled": rl.Color(0xff, 0xff, 0x00, 255),
"experimental": rl.Color(0xda, 0x6f, 0x25, 255),
"traffic": rl.Color(0xc9, 0x22, 0x31, 255),
}
@@ -50,7 +48,7 @@ class ExpButton(Widget):
def _update_state(self) -> None:
selfdrive_state = ui_state.sm["selfdriveState"]
self._experimental_mode = selfdrive_state.experimentalMode
self._experimental_mode = requested_experimental_mode(self._params, ui_state.params_memory)
self._engageable = selfdrive_state.engageable or selfdrive_state.enabled or ui_state.always_on_lateral_active
# Smooth steering angle for rotating wheel
@@ -65,8 +63,6 @@ class ExpButton(Widget):
self._bg_color = self._bg_colors["switchback"]
elif ui_state.always_on_lateral_active:
self._bg_color = self._bg_colors["aol"]
elif ui_state.conditional_status == 1:
self._bg_color = self._bg_colors["cem_disabled"]
elif self._held_or_actual_mode():
self._bg_color = self._bg_colors["experimental"]
elif ui_state.traffic_mode_enabled:
@@ -77,20 +73,9 @@ class ExpButton(Widget):
def _handle_mouse_release(self, _):
super()._handle_mouse_release(_)
if self._is_toggle_allowed():
if self._params.get_bool("ConditionalExperimental"):
current_status = ui_state.params_memory.get_int("CEStatus", default=CEStatus["OFF"])
override_value = next_manual_ce_status(current_status, self._experimental_mode)
ui_state.params_memory.put_int("CEStatus", override_value)
sync_manual_ce_state(self._params, override_value)
self._held_mode = None
self._hold_end_time = None
else:
new_mode = not self._experimental_mode
self._params.put_bool("ExperimentalMode", new_mode)
# Hold new state temporarily
self._held_mode = new_mode
self._hold_end_time = time.monotonic() + self._hold_duration
new_mode = toggle_longitudinal_model_preference(self._params, ui_state.params_memory)
self._held_mode = new_mode
self._hold_end_time = time.monotonic() + self._hold_duration
def _render(self, rect: rl.Rectangle) -> None:
center_x = int(self._rect.x + self._rect.width // 2)
selfdrive/ui/tests/test_mode_banner.py
+3 -23
View File
@@ -17,26 +17,10 @@ def _rgb(color):
return color.r, color.g, color.b
def test_mode_banner_variant_tracks_fixed_and_conditional_modes():
def test_mode_banner_variant_tracks_longitudinal_preference():
assert get_mode_banner_variant(FakeParams()) == ModeBannerVariant.CHILL
assert get_mode_banner_variant(FakeParams({"ExperimentalMode": True})) == ModeBannerVariant.EXPERIMENTAL
assert get_mode_banner_variant(FakeParams({"ConditionalExperimental": True})) == ModeBannerVariant.CONDITIONAL_EXPERIMENTAL
assert get_mode_banner_variant(FakeParams({"ConditionalChill": True})) == ModeBannerVariant.CONDITIONAL_CHILL
assert get_mode_banner_variant(FakeParams({"SafeMode": True, "ConditionalExperimental": True})) == ModeBannerVariant.CHILL
def test_conditional_gradients_keep_full_major_and_partial_minor_colors():
conditional_experimental = ModeBannerVariant.CONDITIONAL_EXPERIMENTAL
assert _rgb(mode_banner_color(conditional_experimental, 0.0)) == (35, 149, 255)
assert _rgb(mode_banner_color(conditional_experimental, 0.58)) == (20, 255, 171)
assert _rgb(mode_banner_color(conditional_experimental, 0.80)) == (255, 138, 22)
assert _rgb(mode_banner_color(conditional_experimental, 1.0)) == (231, 83, 30)
conditional_chill = ModeBannerVariant.CONDITIONAL_CHILL
assert _rgb(mode_banner_color(conditional_chill, 0.0)) == (255, 138, 22)
assert _rgb(mode_banner_color(conditional_chill, 0.58)) == (219, 56, 34)
assert _rgb(mode_banner_color(conditional_chill, 0.80)) == (35, 149, 255)
assert _rgb(mode_banner_color(conditional_chill, 1.0)) == (25, 220, 199)
assert get_mode_banner_variant(FakeParams(ints={"LongitudinalModelPreference": 1})) == ModeBannerVariant.EXPERIMENTAL
assert get_mode_banner_variant(FakeParams({"SafeMode": True}, {"LongitudinalModelPreference": 1})) == ModeBannerVariant.CHILL
def test_atom_gradients_use_compact_icon_directions():
@@ -44,7 +28,3 @@ def test_atom_gradients_use_compact_icon_directions():
assert _rgb(mode_atom_color(ModeBannerVariant.CHILL, 1.0)) == (20, 255, 171)
assert _rgb(mode_atom_color(ModeBannerVariant.EXPERIMENTAL, 0.0)) == (255, 155, 63)
assert _rgb(mode_atom_color(ModeBannerVariant.EXPERIMENTAL, 1.0)) == (219, 56, 34)
for variant in (ModeBannerVariant.CONDITIONAL_EXPERIMENTAL, ModeBannerVariant.CONDITIONAL_CHILL):
assert _rgb(mode_atom_color(variant, 0.0)) == _rgb(mode_banner_color(variant, 0.0))
assert _rgb(mode_atom_color(variant, 1.0)) == _rgb(mode_banner_color(variant, 1.0))
selfdrive/ui/widgets/exp_mode_button.py
+1 -6
View File
@@ -40,12 +40,7 @@ class ExperimentalModeButton(Widget):
rl.draw_line_ex(rl.Vector2(line_x, rect.y), rl.Vector2(line_x, rect.y + rect.height), 3, separator_color)
# Draw text label (left aligned)
if self.mode_variant == ModeBannerVariant.CONDITIONAL_EXPERIMENTAL:
text = tr("CONDITIONAL EXPERIMENTAL")
elif self.mode_variant == ModeBannerVariant.CONDITIONAL_CHILL:
text = tr("CONDITIONAL CHILL")
else:
text = tr("EXPERIMENTAL MODE ON") if self.experimental_mode else tr("CHILL MODE ON")
text = tr("MODEL FIRST") if self.experimental_mode else tr("SET-SPEED FIRST")
text_x = rect.x + self.horizontal_padding
font = gui_app.font(FontWeight.NORMAL)
starpilot/common/experimental_state.py
+14 -159
View File
@@ -1,15 +1,13 @@
#!/usr/bin/env python3
from __future__ import annotations
from openpilot.common.params import Params
PERSIST_EXPERIMENTAL_STATE_PARAM = "PersistExperimentalState"
PERSISTED_CE_STATUS_PARAM = "PersistedCEStatus"
CE_STATUS_PARAM = "CEStatus"
PERSIST_CHILL_STATE_PARAM = "PersistChillState"
PERSISTED_CC_STATUS_PARAM = "PersistedCCStatus"
CC_STATUS_PARAM = "CCStatus"
CE_STATUS_PARAM = "CEStatus"
CC_STATUS_PARAM = "CCStatus"
LONGITUDINAL_PREFERENCE_PARAM = "LongitudinalModelPreference"
LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM = "LongitudinalModelPreferenceOverride"
# Status values stay schema-compatible with the existing onroad widgets.
CEStatus = {
"OFF": 0,
"USER_DISABLED": 1,
@@ -22,6 +20,7 @@ CEStatus = {
"STOP_LIGHT": 8,
}
# Kept only so older/mici status renderers can safely read a stale CCStatus.
CCStatus = {
"OFF": 0,
"USER_EXPERIMENTAL": 1,
@@ -30,161 +29,17 @@ CCStatus = {
"SPEED": 6,
}
MANUAL_CE_STATUSES = {
CEStatus["USER_DISABLED"],
CEStatus["USER_OVERRIDDEN"],
}
MANUAL_CC_STATUSES = {
CCStatus["USER_EXPERIMENTAL"],
CCStatus["USER_CHILL"],
}
def is_manual_ce_status(status: int) -> bool:
return int(status) in MANUAL_CE_STATUSES
def is_manual_cc_status(status: int) -> bool:
return int(status) in MANUAL_CC_STATUSES
def normalize_persisted_ce_status(status: int) -> int:
status = int(status)
return status if status in MANUAL_CE_STATUSES else CEStatus["OFF"]
def normalize_persisted_cc_status(status: int) -> int:
status = int(status)
return status if status in MANUAL_CC_STATUSES else CCStatus["OFF"]
def get_persisted_ce_status(params: Params) -> int:
return normalize_persisted_ce_status(params.get_int(PERSISTED_CE_STATUS_PARAM, default=CEStatus["OFF"]))
def get_persisted_cc_status(params: Params) -> int:
return normalize_persisted_cc_status(params.get_int(PERSISTED_CC_STATUS_PARAM, default=CCStatus["OFF"]))
def set_persisted_ce_status(params: Params, status: int) -> int:
normalized = normalize_persisted_ce_status(status)
params.put_int(PERSISTED_CE_STATUS_PARAM, normalized)
return normalized
def set_persisted_cc_status(params: Params, status: int) -> int:
normalized = normalize_persisted_cc_status(status)
params.put_int(PERSISTED_CC_STATUS_PARAM, normalized)
return normalized
def clear_persisted_ce_status(params: Params) -> None:
params.put_int(PERSISTED_CE_STATUS_PARAM, CEStatus["OFF"])
def clear_persisted_cc_status(params: Params) -> None:
params.put_int(PERSISTED_CC_STATUS_PARAM, CCStatus["OFF"])
def sync_persist_experimental_state(params: Params, params_memory: Params | None, enabled: bool) -> None:
params.put_bool(PERSIST_EXPERIMENTAL_STATE_PARAM, enabled)
if enabled:
current_status = params_memory.get_int(CE_STATUS_PARAM, default=CEStatus["OFF"]) if params_memory is not None else CEStatus["OFF"]
set_persisted_ce_status(params, current_status)
else:
clear_persisted_ce_status(params)
def sync_persist_chill_state(params: Params, params_memory: Params | None, enabled: bool) -> None:
params.put_bool(PERSIST_CHILL_STATE_PARAM, enabled)
if enabled:
current_status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"]) if params_memory is not None else CCStatus["OFF"]
set_persisted_cc_status(params, current_status)
else:
clear_persisted_cc_status(params)
def sync_manual_ce_state(params: Params, status: int) -> int:
return set_persisted_ce_status(params, status) if params.get_bool(PERSIST_EXPERIMENTAL_STATE_PARAM) else clear_and_return_off(params)
def sync_manual_cc_state(params: Params, status: int) -> int:
return set_persisted_cc_status(params, status) if params.get_bool(PERSIST_CHILL_STATE_PARAM) else clear_cc_and_return_off(params)
def clear_and_return_off(params: Params) -> int:
clear_persisted_ce_status(params)
return CEStatus["OFF"]
def clear_cc_and_return_off(params: Params) -> int:
clear_persisted_cc_status(params)
return CCStatus["OFF"]
def next_manual_ce_status(current_status: int, experimental_mode: bool) -> int:
if is_manual_ce_status(current_status):
return CEStatus["OFF"]
return CEStatus["USER_DISABLED"] if experimental_mode else CEStatus["USER_OVERRIDDEN"]
def next_manual_cc_status(current_status: int, experimental_mode: bool) -> int:
if is_manual_cc_status(current_status):
return CCStatus["OFF"]
return CCStatus["USER_CHILL"] if experimental_mode else CCStatus["USER_EXPERIMENTAL"]
def requested_experimental_mode(params: Params, params_memory: Params | None = None) -> bool:
if params.get_bool("SafeMode"):
return False
if params.get_bool("ConditionalExperimental"):
status = params_memory.get_int(CE_STATUS_PARAM, default=CEStatus["OFF"]) if params_memory is not None else CEStatus["OFF"]
if not is_manual_ce_status(status):
status = get_persisted_ce_status(params)
return status == CEStatus["USER_OVERRIDDEN"]
if params.get_bool("ConditionalChill"):
status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"]) if params_memory is not None else CCStatus["OFF"]
if not is_manual_cc_status(status):
status = get_persisted_cc_status(params)
if not is_manual_cc_status(status):
return True
return status == CCStatus["USER_EXPERIMENTAL"]
return params.get_bool("ExperimentalMode")
persistent = params.get_int(LONGITUDINAL_PREFERENCE_PARAM, default=0)
override = params_memory.get_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM, default=-1) if params_memory is not None else -1
return bool(override if override in (0, 1) else persistent)
def restore_persisted_ce_state(params: Params, params_memory: Params) -> int:
current_status = params_memory.get_int(CE_STATUS_PARAM, default=CEStatus["OFF"])
if is_manual_ce_status(current_status):
sync_manual_ce_state(params, current_status)
return current_status
if not params.get_bool(PERSIST_EXPERIMENTAL_STATE_PARAM):
return current_status
restored_status = get_persisted_ce_status(params)
if restored_status != CEStatus["OFF"]:
params_memory.put_int(CE_STATUS_PARAM, restored_status)
return restored_status
return current_status
def restore_persisted_cc_state(params: Params, params_memory: Params) -> int:
current_status = params_memory.get_int(CC_STATUS_PARAM, default=CCStatus["OFF"])
if is_manual_cc_status(current_status):
sync_manual_cc_state(params, current_status)
return current_status
if not params.get_bool(PERSIST_CHILL_STATE_PARAM):
return current_status
restored_status = get_persisted_cc_status(params)
if restored_status != CCStatus["OFF"]:
params_memory.put_int(CC_STATUS_PARAM, restored_status)
return restored_status
return current_status
def toggle_longitudinal_model_preference(params: Params, params_memory: Params) -> bool:
requested = not requested_experimental_mode(params, params_memory)
params_memory.put_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM, int(requested))
return requested
starpilot/common/safe_mode.py
+2 -20
View File
@@ -107,26 +107,8 @@ SAFE_MODE_MANAGED_KEYS = (
"ReduceLateralAccelerationRain",
"ReduceLateralAccelerationRainStorm",
"ReduceLateralAccelerationSnow",
"ConditionalExperimental",
"ConditionalChill",
"CECurves",
"CECurvesLead",
"CELead",
"CESlowerLead",
"CEStoppedLead",
"CESpeed",
"CESpeedLead",
"CCMLead",
"CCMLaunchAssist",
"CCMSetSpeedMargin",
"CCMSpeed",
"CCMSpeedLead",
"CEModelStopTime",
"CEStopLights",
"CESignalSpeed",
"CESignalLaneDetection",
"PersistChillState",
"ShowCCMStatus",
"LongitudinalModelPreference",
"ShowCEMStatus",
"CurveSpeedController",
"SpeedLimitController",
"SetSpeedLimit",
starpilot/common/starpilot_variables.py
+5 -21
View File
@@ -802,28 +802,12 @@ class StarPilotVariables:
self.migrate_prius_cluster_offset(str(toggle.car_model))
toggle.cluster_offset = self.get_value("ClusterOffset", cast=float, condition=toggle.car_make == "toyota")
toggle.conditional_experimental_mode = toggle.openpilot_longitudinal and self.get_value("ConditionalExperimental")
toggle.conditional_chill_mode = toggle.openpilot_longitudinal and not toggle.conditional_experimental_mode and self.get_value("ConditionalChill")
toggle.conditional_curves = self.get_value("CECurves", condition=toggle.conditional_experimental_mode)
toggle.conditional_curves_lead = self.get_value("CECurvesLead", condition=toggle.conditional_curves)
toggle.conditional_lead = self.get_value("CELead", condition=toggle.conditional_experimental_mode)
toggle.conditional_slower_lead = self.get_value("CESlowerLead", condition=toggle.conditional_lead)
toggle.conditional_stopped_lead = self.get_value("CEStoppedLead", condition=toggle.conditional_lead)
toggle.conditional_limit = self.get_value("CESpeed", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion)
toggle.conditional_limit_lead = self.get_value("CESpeedLead", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion)
toggle.conditional_model_stop_time = self.get_value("CEModelStopTime", cast=float, condition=toggle.conditional_experimental_mode and self.get_value("CEStopLights"))
toggle.conditional_signal = self.get_value("CESignalSpeed", cast=float, condition=toggle.conditional_experimental_mode, conversion=speed_conversion)
toggle.conditional_signal_lane_detection = self.get_value("CESignalLaneDetection", condition=toggle.conditional_signal != 0)
toggle.conditional_chill_speed = self.get_value("CCMSpeed", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion)
toggle.conditional_chill_speed_lead = self.get_value("CCMSpeedLead", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion)
toggle.conditional_chill_speed_margin = self.get_value("CCMSetSpeedMargin", cast=float, condition=toggle.conditional_chill_mode, conversion=speed_conversion)
toggle.conditional_chill_lead = self.get_value("CCMLead", condition=toggle.conditional_chill_mode)
toggle.conditional_chill_launch_assist = self.get_value("CCMLaunchAssist", condition=toggle.conditional_chill_mode)
toggle.cem_status = (
self.get_value("ShowCEMStatus", condition=toggle.conditional_experimental_mode) or
self.get_value("ShowCCMStatus", condition=toggle.conditional_chill_mode) or
toggle.debug_mode
persistent_preference = self.get_value("LongitudinalModelPreference", cast=int, min=0, max=1)
drive_override = self.params_memory.get_int("LongitudinalModelPreferenceOverride", default=-1)
toggle.longitudinal_model_preference = not toggle.safe_mode and bool(
drive_override if drive_override in (0, 1) else persistent_preference
)
toggle.cem_status = self.get_value("ShowCEMStatus") or toggle.debug_mode
toggle.curve_speed_controller = toggle.openpilot_longitudinal and self.get_value("CurveSpeedController")
toggle.csc_status = self.get_value("ShowCSCStatus", condition=toggle.curve_speed_controller) or toggle.debug_mode
starpilot/common/tests/test_accel_profile.py
+8 -5
View File
@@ -17,12 +17,12 @@ def test_truck_curves_match_expected_values():
assert get_accel_profile_curve_values(ACCELERATION_PROFILES["SPORT_PLUS"], ev_tuning=False, truck_tuning=True) == A_CRUISE_MAX_VALS_SPORT_PLUS_TRUCK
def test_standard_truck_curve_keeps_mid_high_speed_authority():
def test_standard_truck_curve_relaxes_at_highway_speed():
values = get_accel_profile_curve_values(ACCELERATION_PROFILES["STANDARD"], ev_tuning=False, truck_tuning=True)
assert interpolate_accel_profile(20.0, values) >= 1.18
assert interpolate_accel_profile(25.0, values) >= 0.98
assert interpolate_accel_profile(30.0, values) >= 0.90
assert 0.5 <= interpolate_accel_profile(20.0, values) <= 0.6
assert interpolate_accel_profile(25.0, values) == 0.45
assert 0.35 < interpolate_accel_profile(30.0, values) < 0.45
def test_truck_profiles_remain_ordered():
@@ -32,4 +32,7 @@ def test_truck_profiles_remain_ordered():
sport_plus = get_accel_profile_curve_values(ACCELERATION_PROFILES["SPORT_PLUS"], ev_tuning=False, truck_tuning=True)
for e, s, sp, spp in zip(eco, standard, sport, sport_plus, strict=True):
assert e < s < sp < spp
assert e <= s <= sp <= spp
assert any(e < s for e, s in zip(eco, standard, strict=True))
assert any(s < sp for s, sp in zip(standard, sport, strict=True))
assert any(sp < spp for sp, spp in zip(sport, sport_plus, strict=True))
starpilot/common/tests/test_experimental_state.py
+23 -49
View File
@@ -1,10 +1,7 @@
from openpilot.starpilot.common.experimental_state import (
CC_STATUS_PARAM,
CCStatus,
CE_STATUS_PARAM,
CEStatus,
LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM,
requested_experimental_mode,
restore_persisted_cc_state,
toggle_longitudinal_model_preference,
)
@@ -16,9 +13,6 @@ class FakeParams:
def get_bool(self, key):
return bool(self.bools.get(key, False))
def put_bool(self, key, value):
self.bools[key] = bool(value)
def get_int(self, key, default=0):
return int(self.ints.get(key, default))
@@ -26,54 +20,34 @@ class FakeParams:
self.ints[key] = int(value)
def test_requested_experimental_mode_defaults_to_experimental_in_ccm_auto():
params = FakeParams(bools={"ConditionalChill": True})
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["OFF"]})
assert requested_experimental_mode(params, params_memory) is True
def test_set_speed_first_is_default():
assert not requested_experimental_mode(FakeParams(), FakeParams())
def test_requested_experimental_mode_respects_ccm_manual_override():
params = FakeParams(bools={"ConditionalChill": True})
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_CHILL"]})
assert requested_experimental_mode(params, params_memory) is False
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"]})
assert requested_experimental_mode(params, params_memory) is True
def test_persistent_model_first_preference_is_respected():
params = FakeParams(ints={"LongitudinalModelPreference": 1})
assert requested_experimental_mode(params, FakeParams())
def test_requested_experimental_mode_prefers_cem_if_both_conditional_modes_are_enabled():
params = FakeParams(bools={"ConditionalExperimental": True, "ConditionalChill": True})
def test_drive_override_takes_priority_without_changing_persistent_default():
params = FakeParams(ints={"LongitudinalModelPreference": 0})
params_memory = FakeParams(ints={LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM: 1})
params_memory = FakeParams(ints={
CE_STATUS_PARAM: CEStatus["OFF"],
CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"],
})
assert requested_experimental_mode(params, params_memory) is False
params_memory = FakeParams(ints={
CE_STATUS_PARAM: CEStatus["USER_OVERRIDDEN"],
CC_STATUS_PARAM: CCStatus["USER_CHILL"],
})
assert requested_experimental_mode(params, params_memory) is True
assert requested_experimental_mode(params, params_memory)
assert params.get_int("LongitudinalModelPreference") == 0
def test_requested_experimental_mode_safe_mode_overrides_ccm():
params = FakeParams(bools={"SafeMode": True, "ConditionalChill": True})
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["USER_EXPERIMENTAL"]})
def test_exp_button_flips_preference_for_current_drive():
params = FakeParams(ints={"LongitudinalModelPreference": 0})
params_memory = FakeParams()
assert requested_experimental_mode(params, params_memory) is False
assert toggle_longitudinal_model_preference(params, params_memory)
assert params_memory.get_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM) == 1
assert not toggle_longitudinal_model_preference(params, params_memory)
assert params_memory.get_int(LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM) == 0
def test_restore_persisted_cc_state_rehydrates_manual_override():
params = FakeParams(
bools={"PersistChillState": True},
ints={"PersistedCCStatus": CCStatus["USER_CHILL"]},
)
params_memory = FakeParams(ints={CC_STATUS_PARAM: CCStatus["OFF"]})
restored_status = restore_persisted_cc_state(params, params_memory)
assert restored_status == CCStatus["USER_CHILL"]
assert params_memory.get_int(CC_STATUS_PARAM) == CCStatus["USER_CHILL"]
def test_safe_mode_forces_set_speed_first():
params = FakeParams(bools={"SafeMode": True}, ints={"LongitudinalModelPreference": 1})
params_memory = FakeParams(ints={LONGITUDINAL_PREFERENCE_OVERRIDE_PARAM: 1})
assert not requested_experimental_mode(params, params_memory)
starpilot/controls/lib/conditional_chill_mode.py
-350
View File
@@ -1,350 +0,0 @@
#!/usr/bin/env python3
import time
from openpilot.common.constants import CV
from openpilot.starpilot.common.experimental_state import (
CCStatus,
is_manual_cc_status,
restore_persisted_cc_state,
)
class ConditionalChillMode:
CCM_STOP_MODEL_TIME = 7.0
CHILL_SPEED_ENTRY_CONFIRM_TIME = 0.35
CHILL_LEAD_ENTRY_CONFIRM_TIME = 1.0
CHILL_LAUNCH_ENTRY_CONFIRM_TIME = 0.0
CHILL_EXIT_BUFFER_TIME = 0.35
CHILL_MIN_DWELL_TIME = 1.2
CHILL_LAUNCH_EXIT_SPEED = 15 * CV.MPH_TO_MS
CHILL_LAUNCH_MAX_ENTRY_SPEED = 1.0
CHILL_LAUNCH_MAX_BRAKE = 0.2
CHILL_LAUNCH_MAX_CLOSING_SPEED = 0.75
CHILL_LAUNCH_MIN_LEAD_SPEED = 0.35
CHILL_LAUNCH_MIN_LEAD_DELTA = 0.2
CHILL_LAUNCH_MIN_LEAD_VREL = 0.2
CHILL_LAUNCH_MIN_LEAD_ACCEL = 0.08
STABLE_LEAD_MIN_MODEL_PROB = 0.9
STABLE_LEAD_MAX_BRAKE = 0.2
STABLE_LEAD_MIN_SPEED = 1.5
STABLE_LEAD_MAX_DISTANCE = 90.0
STABLE_LEAD_MAX_DISTANCE_TIME = 4.5
STABLE_LEAD_MAX_CLOSING_SPEED = 1.25
STABLE_LEAD_MAX_CLOSING_RATIO = 0.05
ADJACENT_LEAD_VETO_MIN_SPEED = 1.0
ADJACENT_LEAD_VETO_MAX_DISTANCE = 65.0
ADJACENT_LEAD_VETO_MAX_DISTANCE_TIME = 3.5
ADJACENT_LEAD_VETO_MAX_LATERAL_OFFSET = 5.5
LOW_SPEED_STOP_SCENE_MAX_SPEED = 18 * CV.MPH_TO_MS
def __init__(self, StarPilotPlanner, detector):
self.starpilot_planner = StarPilotPlanner
self.detector = detector
self.params = self.starpilot_planner.params
self.params_memory = self.starpilot_planner.params_memory
self.experimental_mode = True
self.status_value = CCStatus["OFF"]
self._active_auto_status = CCStatus["OFF"]
self._candidate_since = 0.0
self._soft_exit_since = 0.0
self._chill_hold_until = 0.0
self._prev_cc_status = None
self._launch_active = False
self._launch_forced_exit = False
def update(self, v_ego, v_cruise, sm, starpilot_toggles):
now = time.monotonic()
safe_mode = self.params.get_bool("SafeMode")
self.status_value = CCStatus["OFF"] if safe_mode else restore_persisted_cc_state(self.params, self.params_memory)
if is_manual_cc_status(self.status_value):
self._reset_timers()
self.experimental_mode = self.status_value == CCStatus["USER_EXPERIMENTAL"]
self._write_status(self.status_value)
return
self._refresh_detector(v_ego, sm)
auto_status, launch_candidate = self._get_chill_status(v_ego, v_cruise, sm, starpilot_toggles)
if safe_mode or self._has_hard_veto(v_ego, sm, allow_launch=launch_candidate):
self._reset_timers()
self.experimental_mode = False if safe_mode else True
self.status_value = CCStatus["OFF"]
self._write_status(CCStatus["OFF"])
return
chill_candidate = auto_status != CCStatus["OFF"]
entry_confirm_time = self._get_entry_confirm_time(auto_status, launch_candidate)
if chill_candidate:
if self._candidate_since == 0.0:
self._candidate_since = now
self._soft_exit_since = 0.0
if not self.experimental_mode or (now - self._candidate_since) >= entry_confirm_time:
self.experimental_mode = False
self._active_auto_status = auto_status
self._chill_hold_until = max(self._chill_hold_until, now + self.CHILL_MIN_DWELL_TIME)
self.status_value = self._active_auto_status if not self.experimental_mode else CCStatus["OFF"]
else:
self._candidate_since = 0.0
if not self.experimental_mode:
if self._launch_forced_exit:
self.experimental_mode = True
self._active_auto_status = CCStatus["OFF"]
self.status_value = CCStatus["OFF"]
self._soft_exit_since = 0.0
self._chill_hold_until = 0.0
self._launch_forced_exit = False
self._write_status(CCStatus["OFF"])
return
if self._soft_exit_since == 0.0:
self._soft_exit_since = now
hold_active = now < self._chill_hold_until
exit_buffer_active = (now - self._soft_exit_since) < self.CHILL_EXIT_BUFFER_TIME
if hold_active or exit_buffer_active:
self.status_value = self._active_auto_status
else:
self.experimental_mode = True
self._active_auto_status = CCStatus["OFF"]
self.status_value = CCStatus["OFF"]
else:
self._soft_exit_since = 0.0
self.status_value = CCStatus["OFF"]
self._write_status(self.status_value if not self.experimental_mode else CCStatus["OFF"])
def _reset_timers(self):
self._active_auto_status = CCStatus["OFF"]
self._candidate_since = 0.0
self._soft_exit_since = 0.0
self._chill_hold_until = 0.0
self._launch_active = False
self._launch_forced_exit = False
def _refresh_detector(self, v_ego, sm):
detector_toggles = type("DetectorToggles", (), {
"conditional_curves": True,
"conditional_curves_lead": True,
"conditional_lead": True,
"conditional_slower_lead": True,
"conditional_stopped_lead": True,
})()
self.detector.curve_detection(v_ego, detector_toggles)
self.detector.slow_lead(detector_toggles, v_ego)
self.detector.stop_sign_and_light(v_ego, sm, self.CCM_STOP_MODEL_TIME)
def _has_hard_veto(self, v_ego, sm, allow_launch=False):
if sm["carState"].standstill and not allow_launch:
return True
if sm["carState"].leftBlinker or sm["carState"].rightBlinker:
return True
if sm["starpilotCarState"].trafficModeEnabled:
return True
if self.starpilot_planner.starpilot_vcruise.slc.experimental_mode:
return True
if self.detector.curve_detected or self.detector.slow_lead_detected or self.detector.stop_light_detected:
return True
if self.starpilot_planner.starpilot_vcruise.stop_sign_confirmed or self.starpilot_planner.starpilot_vcruise.forcing_stop:
return True
if self._adjacent_lead_ambiguous(sm, v_ego):
return True
return self._low_speed_stop_scene(v_ego) and not allow_launch
def _low_speed_stop_scene(self, v_ego):
if v_ego >= self.LOW_SPEED_STOP_SCENE_MAX_SPEED:
return False
if self.starpilot_planner.raw_model_stopped or self.starpilot_planner.model_stopped or self.detector.stop_light_model_detected:
return True
lead = self.starpilot_planner.lead_one
if not getattr(lead, "status", False):
return False
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", float("inf")))
lead_distance_limit = max(40.0, v_ego * self.STABLE_LEAD_MAX_DISTANCE_TIME)
return lead_distance < lead_distance_limit and lead_speed < max(6.0, v_ego + 0.5)
def _get_chill_status(self, v_ego, v_cruise, sm, starpilot_toggles):
if getattr(starpilot_toggles, "conditional_chill_launch_assist", False):
launch_status = self._get_launch_status(v_ego, sm)
if launch_status != CCStatus["OFF"]:
return launch_status, True
lead = self.starpilot_planner.lead_one
lead_status = bool(getattr(lead, "status", False))
tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False))
set_speed_error = max(0.0, v_cruise - v_ego)
if (not lead_status and not tracking_lead and
v_ego >= starpilot_toggles.conditional_chill_speed and
set_speed_error >= starpilot_toggles.conditional_chill_speed_margin):
return CCStatus["SPEED"], False
if not starpilot_toggles.conditional_chill_lead:
return CCStatus["OFF"], False
if v_ego < starpilot_toggles.conditional_chill_speed_lead or not lead_status or not tracking_lead:
return CCStatus["OFF"], False
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", 0.0))
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
lead_prob = float(getattr(lead, "modelProb", 0.0))
closing_speed = max(0.0, v_ego - lead_speed)
max_closing_speed = max(self.STABLE_LEAD_MAX_CLOSING_SPEED, self.STABLE_LEAD_MAX_CLOSING_RATIO * v_ego)
max_distance = min(self.STABLE_LEAD_MAX_DISTANCE, max(35.0, v_ego * self.STABLE_LEAD_MAX_DISTANCE_TIME))
lead_confident = bool(getattr(lead, "radar", False)) or lead_prob >= self.STABLE_LEAD_MIN_MODEL_PROB
if not lead_confident:
return CCStatus["OFF"], False
if lead_distance >= max_distance or lead_speed <= self.STABLE_LEAD_MIN_SPEED:
return CCStatus["OFF"], False
if lead_brake > self.STABLE_LEAD_MAX_BRAKE or closing_speed > max_closing_speed:
return CCStatus["OFF"], False
return CCStatus["LEAD"], False
def _get_launch_status(self, v_ego, sm):
if self._launch_active and self._launch_exit_required(v_ego, sm):
self._launch_active = False
self._launch_forced_exit = True
return CCStatus["OFF"]
if self._launch_active:
return self._get_launch_cc_status()
if not self._launch_scene_eligible(v_ego, sm):
return CCStatus["OFF"]
self._launch_forced_exit = False
self._launch_active = True
return self._get_launch_cc_status()
def _launch_scene_eligible(self, v_ego, sm):
if v_ego > self.CHILL_LAUNCH_MAX_ENTRY_SPEED and not self._launch_active:
return False
selfdrive_state = self._get_sm_service(sm, "selfdriveState")
longitudinal_plan = self._get_sm_service(sm, "longitudinalPlan")
starpilot_plan = self._get_sm_service(sm, "starpilotPlan")
if selfdrive_state is None or longitudinal_plan is None:
return False
if not bool(getattr(selfdrive_state, "enabled", False)):
return False
if (
self.detector.stop_light_detected or
self.detector.stop_light_model_detected or
self.starpilot_planner.raw_model_stopped or
self.starpilot_planner.model_stopped or
self.starpilot_planner.starpilot_vcruise.stop_sign_confirmed or
self.starpilot_planner.starpilot_vcruise.forcing_stop or
bool(getattr(starpilot_plan, "redLight", False)) or
bool(getattr(starpilot_plan, "forcingStop", False))
):
return False
if bool(getattr(longitudinal_plan, "shouldStop", False)) or not bool(getattr(longitudinal_plan, "allowThrottle", False)):
return False
lead = getattr(self.starpilot_planner, "lead_one", None)
lead_status = bool(getattr(lead, "status", False))
tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False))
if not lead_status and not tracking_lead:
return True
lead_speed = float(getattr(lead, "vLead", 0.0))
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
lead_accel = float(getattr(lead, "aLeadK", 0.0))
lead_delta = lead_speed - float(v_ego)
lead_vrel = float(getattr(lead, "vRel", lead_delta))
closing_speed = max(0.0, v_ego - lead_speed)
return (
lead_speed >= self.CHILL_LAUNCH_MIN_LEAD_SPEED and
lead_delta >= self.CHILL_LAUNCH_MIN_LEAD_DELTA and
lead_vrel >= self.CHILL_LAUNCH_MIN_LEAD_VREL and
lead_accel >= self.CHILL_LAUNCH_MIN_LEAD_ACCEL and
lead_brake <= self.CHILL_LAUNCH_MAX_BRAKE and
closing_speed <= self.CHILL_LAUNCH_MAX_CLOSING_SPEED
)
def _launch_exit_required(self, v_ego, sm):
if v_ego >= self.CHILL_LAUNCH_EXIT_SPEED:
return True
if not self._launch_scene_eligible(v_ego, sm):
return True
return False
def _get_launch_cc_status(self):
lead = getattr(self.starpilot_planner, "lead_one", None)
lead_status = bool(getattr(lead, "status", False))
tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False))
return CCStatus["LEAD"] if lead_status or tracking_lead else CCStatus["SPEED"]
def _get_entry_confirm_time(self, auto_status, launch_candidate):
if launch_candidate:
return self.CHILL_LAUNCH_ENTRY_CONFIRM_TIME
if auto_status == CCStatus["LEAD"]:
return self.CHILL_LEAD_ENTRY_CONFIRM_TIME
return self.CHILL_SPEED_ENTRY_CONFIRM_TIME
def _adjacent_lead_ambiguous(self, sm, v_ego):
radar_state = self._get_sm_service(sm, "starpilotRadarState")
if radar_state is None:
return False
max_distance = min(self.ADJACENT_LEAD_VETO_MAX_DISTANCE, max(25.0, v_ego * self.ADJACENT_LEAD_VETO_MAX_DISTANCE_TIME))
for lead in (getattr(radar_state, "leadLeft", None), getattr(radar_state, "leadRight", None)):
if lead is None or not getattr(lead, "status", False):
continue
lateral_offset = abs(float(getattr(lead, "yRel", 0.0)))
if lateral_offset > self.ADJACENT_LEAD_VETO_MAX_LATERAL_OFFSET:
continue
if float(getattr(lead, "dRel", float("inf"))) < max_distance and float(getattr(lead, "vLead", 0.0)) > self.ADJACENT_LEAD_VETO_MIN_SPEED:
return True
return False
@staticmethod
def _get_sm_service(sm, key):
if isinstance(sm, dict):
return sm.get(key)
try:
return sm[key]
except (KeyError, IndexError, TypeError, AttributeError):
return None
def _write_status(self, status_value):
if status_value != self._prev_cc_status:
self.params_memory.put_int("CCStatus", status_value)
self._prev_cc_status = status_value
starpilot/controls/lib/conditional_experimental_mode.py
-521
View File
@@ -1,521 +0,0 @@
#!/usr/bin/env python3
import time
import numpy as np
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.common.constants import CV
from openpilot.starpilot.common.experimental_state import (
CEStatus,
is_manual_ce_status,
restore_persisted_ce_state,
)
from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, THRESHOLD
def interp(x, xp, fp):
return float(np.interp(x, xp, fp))
def scale_threshold(v_ego):
# Speed-based lead threshold behavior (v_ego in m/s)
return interp(v_ego, [0.0, 17.9, 26.8, 35.8, 44.7], [0.58, 0.60, 0.62, 0.75, 0.90])
class ConditionalExperimentalMode:
# ===== CONDITIONAL EXPERIMENTAL MODE SPEED-BASED TUNING =====
# Speed ranges: [0-35, 35-55, 55-70, 70+ mph]
# FILTER TIME CONSTANTS (Lower = More responsive, Higher = Smoother)
# [City, Urban Hwy, Rural Hwy, High Speed]
FILTER_TIME_CURVES = [0.9, 0.8, 0.6, 0.5] # Faster detection at highway speeds
FILTER_TIME_LEADS = [0.9, 0.8, 0.7, 0.5] # Less sensitive at 70+ mph for slow leads
FILTER_TIME_LIGHTS = [0.9, 0.8, 0.75, 0.55] # Less sensitive at 60+ mph for stoplights
# HIGHWAY LIGHT DETECTION MULTIPLIERS
# How much to increase model stop time at highway speeds
LIGHT_BOOSTS = [1.0, 1.2, 1.1, 1.0] # Keep conservative boost for highest speeds
LIGHT_SPEED_LOW = 50 * CV.MPH_TO_MS # 50 mph threshold
LIGHT_SPEED_HIGH = 60 * CV.MPH_TO_MS # 60 mph threshold
LIGHT_MAX_TIME = 9 # Balanced max time preserving city performance
LOW_SPEED_LIGHT_FILTER_TIME = 0.35
LEAD_CLEAR_FILTER_TIME_LOW = 0.6
LEAD_CLEAR_FILTER_TIME_HIGH = 0.35
STOP_LIGHT_ON_MARGIN = 2.5
STOP_LIGHT_OFF_MARGIN = 4.0
STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN = 10.0
STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0
STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0
STOP_LIGHT_DETECTED_HOLD_TIME = 4.0
STOP_APPROACH_LATCH_TIME = 1.0
STOP_APPROACH_MAX_LEAD_SPEED = 4.5
STOP_APPROACH_MIN_MODEL_PROB = 0.9
SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB = 0.85
SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME = 4.0
SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5
SLOW_LEAD_CONTINUITY_HOLD_TIME = 1.25
SLOW_LEAD_FORCE_CLEAR_TIME = 0.75
SLOW_LEAD_MODE_RELEASE_HOLD_TIME = 1.5
SLOW_LEAD_MIN_CLOSING_SPEED = 0.75
SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5
SLOW_RADAR_LEAD_TRIGGER_MAX_DISTANCE_TIME = 2.5
SLOW_RADAR_LEAD_TRIGGER_MIN_DISTANCE = 40.0
POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0
TURN_STOP_LIGHT_VETO_MAX_SPEED = 15 * CV.MPH_TO_MS
TURN_STOP_LIGHT_VETO_STEERING_ANGLE = 45.0
# ===== END TUNING PARAMETERS =====
# Current active values
FILTER_TIME_CURVE = 0.8
FILTER_TIME_LEAD = 0.8
FILTER_TIME_LIGHT = 0.8
LIGHT_BOOST_LOW = 1.15
LIGHT_BOOST_HIGH = 1.2
# Small latch to avoid frame-to-frame mode chatter.
CEM_TRANSITION_GUARD_TIME = 0.50
CEM_TRANSITION_BUFFER_TIME = 0.25
@staticmethod
def get_speed_based_param(speed_mph, param_array):
"""Get parameter value based on current speed using smooth interpolation between breakpoints [0, 35, 55, 70]"""
return interp(speed_mph, [0, 35, 55, 70], param_array)
def __init__(self, StarPilotPlanner):
self.starpilot_planner = StarPilotPlanner
self.params = self.starpilot_planner.params
self.params_memory = self.starpilot_planner.params_memory
# Faster filters with hysteresis for better responsiveness
self.curvature_filter = FirstOrderFilter(0, self.FILTER_TIME_CURVE, DT_MDL)
self.slow_lead_filter = FirstOrderFilter(0, self.FILTER_TIME_LEAD, DT_MDL)
self.stop_light_filter = FirstOrderFilter(0, self.FILTER_TIME_LIGHT, DT_MDL)
self.lead_clear_filter = FirstOrderFilter(0, self.LEAD_CLEAR_FILTER_TIME_LOW, DT_MDL)
self.curve_detected = False
self.slow_lead_detected = False
self.prev_tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False))
self.slow_lead_clear_since = 0.0
self.slow_lead_continuity_until = 0.0
self.experimental_mode = False
self.stop_light_detected = False
self.stop_light_model_detected = False
self.stop_light_detected_hold_until = 0.0
self.stop_approach_hold_until = 0.0
self.standstill_stop_reason = None
self.prev_experimental_mode = False # For hysteresis
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self.slow_lead_mode_hold_until = 0.0
self._prev_ce_status = None
self.prev_standstill = False
self.prev_standstill_stop_hold = False
self.standstill_stop_release_pending = False
self.post_stop_launch_trigger_suppress_until = 0.0
def update(self, v_ego, sm, starpilot_toggles):
now = time.monotonic()
standstill = bool(sm["carState"].standstill)
current_standstill_stop_hold = False
released_standstill_stop_hold = self.prev_standstill and self.prev_standstill_stop_hold and not standstill
completed_pending_stop_release = self.standstill_stop_release_pending and not standstill
if released_standstill_stop_hold or completed_pending_stop_release:
self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self.slow_lead_mode_hold_until = 0.0
self.prev_experimental_mode = False
self.standstill_stop_release_pending = False
if not standstill:
self.standstill_stop_reason = None
self.status_value = CEStatus["OFF"] if self.params.get_bool("SafeMode") else restore_persisted_ce_state(self.params, self.params_memory)
if not is_manual_ce_status(self.status_value) and not standstill:
self.update_conditions(v_ego, sm, starpilot_toggles)
triggered = self.check_conditions(v_ego, sm, starpilot_toggles)
if triggered:
self.mode_hold_until = now + self.CEM_TRANSITION_GUARD_TIME
self.mode_false_since = 0.0
if self.status_value == CEStatus["LEAD"]:
self.slow_lead_mode_hold_until = now + self.SLOW_LEAD_MODE_RELEASE_HOLD_TIME
else:
self.slow_lead_mode_hold_until = 0.0
elif self.prev_experimental_mode and self.mode_false_since == 0.0:
self.mode_false_since = now
elif not self.prev_experimental_mode:
self.mode_false_since = 0.0
hold_active = now < self.mode_hold_until
transition_buffer_active = self.mode_false_since != 0.0 and (now - self.mode_false_since) < self.CEM_TRANSITION_BUFFER_TIME
slow_lead_hold_active = bool(
starpilot_toggles.conditional_lead and
now < self.slow_lead_mode_hold_until and
self.has_credible_slow_lead_context(v_ego)
)
if slow_lead_hold_active and not triggered:
self.status_value = CEStatus["LEAD"]
elif not slow_lead_hold_active:
self.slow_lead_mode_hold_until = 0.0
self.experimental_mode = triggered or slow_lead_hold_active or hold_active or transition_buffer_active
self.prev_experimental_mode = self.experimental_mode
ce_write_value = self.status_value if self.experimental_mode else CEStatus["OFF"]
if ce_write_value != self._prev_ce_status:
self.params_memory.put_int("CEStatus", ce_write_value)
self._prev_ce_status = ce_write_value
elif not is_manual_ce_status(self.status_value):
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self.slow_lead_mode_hold_until = 0.0
# Keep the stop-light path live at standstill so EXP stays pinned for a red
# light / stop sign. Stop signs latch until pedal, while stop lights can
# immediately release to CHILL when the model clears the stop.
self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time)
standstill_stop_hold = self.get_standstill_stop_hold(sm)
current_standstill_stop_hold = standstill_stop_hold
if current_standstill_stop_hold:
self.standstill_stop_release_pending = False
elif self.prev_standstill_stop_hold:
self.standstill_stop_release_pending = True
if self.standstill_stop_release_pending:
self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME
self.experimental_mode = standstill_stop_hold
self.prev_experimental_mode = self.experimental_mode
self.status_value = CEStatus["STOP_LIGHT"] if self.experimental_mode else CEStatus["OFF"]
ce_write_value = self.status_value
if ce_write_value != self._prev_ce_status:
self.params_memory.put_int("CEStatus", ce_write_value)
self._prev_ce_status = ce_write_value
else:
self.mode_hold_until = 0.0
self.mode_false_since = 0.0
self.slow_lead_mode_hold_until = 0.0
self._prev_ce_status = None
self.standstill_stop_release_pending = False
self.experimental_mode = self.status_value == CEStatus["USER_OVERRIDDEN"]
self.prev_experimental_mode = self.experimental_mode
self.stop_light_detected &= not is_manual_ce_status(self.status_value)
self.stop_light_filter.x = 0
self.prev_standstill = standstill
self.prev_standstill_stop_hold = current_standstill_stop_hold
def has_credible_slow_lead_context(self, v_ego):
lead = self.starpilot_planner.lead_one
if lead is None or not bool(getattr(lead, "status", False)):
return False
lead_radar = bool(getattr(lead, "radar", False))
lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
if not lead_radar and lead_prob < self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB:
return False
lead_distance = float(getattr(lead, "dRel", float("inf")))
return lead_distance < max(40.0, float(v_ego) * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME)
def get_standstill_stop_hold(self, sm):
dash_stop_sign = (
bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or
bool(getattr(sm["starpilotCarState"], "dashboardStopSign", 0) > 0)
)
force_stop_active = bool(getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False))
model_stopped = bool(getattr(self.starpilot_planner, "model_stopped", False))
pedal_override = bool(getattr(sm["carState"], "gasPressed", False) or getattr(sm["starpilotCarState"], "accelPressed", False))
if pedal_override or not bool(sm["carState"].standstill):
self.standstill_stop_reason = None
return False
if dash_stop_sign:
self.standstill_stop_reason = "sign"
elif self.stop_light_detected or force_stop_active or model_stopped:
if self.standstill_stop_reason is None:
self.standstill_stop_reason = "light"
elif self.standstill_stop_reason == "light":
self.standstill_stop_reason = None
if self.standstill_stop_reason == "sign":
return True
return bool(self.stop_light_detected or force_stop_active or model_stopped)
def check_conditions(self, v_ego, sm, starpilot_toggles):
launch_trigger_suppressed = time.monotonic() < self.post_stop_launch_trigger_suppress_until
below_speed = not launch_trigger_suppressed and starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead
below_speed_with_lead = not launch_trigger_suppressed and starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead
if below_speed or below_speed_with_lead:
self.status_value = CEStatus["SPEED"]
return True
desired_lane = self.starpilot_planner.lane_width_left if sm["carState"].leftBlinker else self.starpilot_planner.lane_width_right
lane_available = desired_lane >= starpilot_toggles.lane_detection_width or not starpilot_toggles.conditional_signal_lane_detection
if v_ego < starpilot_toggles.conditional_signal and (sm["carState"].leftBlinker or sm["carState"].rightBlinker) and not lane_available:
self.status_value = CEStatus["SIGNAL"]
return True
if starpilot_toggles.conditional_curves and self.curve_detected and (starpilot_toggles.conditional_curves_lead or not self.starpilot_planner.starpilot_following.following_lead):
self.status_value = CEStatus["CURVATURE"]
return True
if not launch_trigger_suppressed and starpilot_toggles.conditional_lead and self.slow_lead_detected and v_ego <= 35.31:
self.status_value = CEStatus["LEAD"]
return True
if starpilot_toggles.conditional_model_stop_time != 0 and self.stop_light_detected:
self.status_value = CEStatus["STOP_LIGHT"]
return True
if self.starpilot_planner.starpilot_vcruise.slc.experimental_mode:
self.status_value = CEStatus["SPEED_LIMIT"]
return True
return False
def update_conditions(self, v_ego, sm, starpilot_toggles):
self.curve_detection(v_ego, starpilot_toggles)
self.slow_lead(starpilot_toggles, v_ego)
self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time)
def curve_detection(self, v_ego, starpilot_toggles):
self.curvature_filter.update(self.starpilot_planner.road_curvature_detected or self.starpilot_planner.driving_in_curve)
self.curve_detected = bool(self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED)
def slow_lead(self, starpilot_toggles, v_ego):
now = time.monotonic()
lead = self.starpilot_planner.lead_one
tracking_lead = bool(getattr(self.starpilot_planner, "tracking_lead", False))
lead_status = bool(getattr(lead, "status", False))
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", float("inf")))
lead_prob = float(getattr(lead, "modelProb", 1.0))
lead_radar = bool(getattr(lead, "radar", False))
closing_speed = max(0.0, v_ego - lead_speed)
min_closing_speed = max(self.SLOW_LEAD_MIN_CLOSING_SPEED, 0.04 * v_ego)
if not starpilot_toggles.conditional_stopped_lead and v_ego < self.SLOW_LEAD_CONTINUITY_MIN_EGO:
self.clear_slow_lead_state(tracking_lead)
return
radar_slow_lead_in_range = bool(
not lead_radar or
lead_distance < max(self.SLOW_RADAR_LEAD_TRIGGER_MIN_DISTANCE,
v_ego * self.SLOW_RADAR_LEAD_TRIGGER_MAX_DISTANCE_TIME)
)
slower_lead = bool(
starpilot_toggles.conditional_slower_lead and
self.starpilot_planner.starpilot_following.slower_lead and
radar_slow_lead_in_range
)
stopped_lead = bool(
starpilot_toggles.conditional_stopped_lead and
lead_status and
lead_speed < 1 and
lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME)
)
vision_slow_lead_candidate = bool(
lead_status and
not lead_radar and
lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and
lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) and
closing_speed >= min_closing_speed and
lead_speed < max(v_ego - 0.5, 2.0)
)
lead_threshold = scale_threshold(v_ego)
adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence
if lead_status and not slower_lead and not stopped_lead and closing_speed < (min_closing_speed * self.SLOW_LEAD_CLEAR_FASTER_FACTOR):
self.clear_slow_lead_state(tracking_lead)
return
if tracking_lead and (slower_lead or stopped_lead or vision_slow_lead_candidate):
self.slow_lead_continuity_until = now + self.SLOW_LEAD_CONTINUITY_HOLD_TIME
elif self.prev_tracking_lead and not tracking_lead and self.slow_lead_detected and vision_slow_lead_candidate:
self.slow_lead_continuity_until = now + self.SLOW_LEAD_CONTINUITY_HOLD_TIME
raw_vision_slow_lead = bool(
starpilot_toggles.conditional_slower_lead and
not tracking_lead and
now < self.slow_lead_continuity_until and
vision_slow_lead_candidate
)
tracked_vision_mode_continuation = bool(
starpilot_toggles.conditional_slower_lead and
tracking_lead and
self.prev_experimental_mode and
vision_slow_lead_candidate
)
slow_lead_active = bool(slower_lead or raw_vision_slow_lead or stopped_lead or tracked_vision_mode_continuation)
if slow_lead_active:
self.slow_lead_clear_since = 0.0
self.slow_lead_filter.update(True)
self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold)
elif tracking_lead:
if self.slow_lead_clear_since == 0.0:
self.slow_lead_clear_since = now
if (now - self.slow_lead_clear_since) >= self.SLOW_LEAD_FORCE_CLEAR_TIME:
self.clear_slow_lead_state(tracking_lead)
else:
self.slow_lead_filter.update(False)
self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold)
else:
self.clear_slow_lead_state(tracking_lead)
self.prev_tracking_lead = tracking_lead
def clear_slow_lead_state(self, tracking_lead):
self.slow_lead_filter.update(False)
self.slow_lead_detected = False
self.slow_lead_clear_since = 0.0
self.slow_lead_continuity_until = 0.0
self.prev_tracking_lead = tracking_lead
def reset_stop_light_state(self):
self.stop_light_filter.x = 0
self.stop_light_detected = False
self.stop_light_model_detected = False
self.stop_light_detected_hold_until = 0.0
self.lead_clear_filter.x = 0
self.stop_approach_hold_until = 0.0
def in_committed_turn_scene(self, v_ego, sm):
car_state = sm["carState"]
if bool(getattr(car_state, "standstill", False)) or v_ego > self.TURN_STOP_LIGHT_VETO_MAX_SPEED:
return False
if not (bool(getattr(car_state, "leftBlinker", False)) or bool(getattr(car_state, "rightBlinker", False))):
return False
steering_angle = abs(float(getattr(car_state, "steeringAngleDeg", 0.0)))
return bool(
steering_angle >= self.TURN_STOP_LIGHT_VETO_STEERING_ANGLE or
getattr(self.starpilot_planner, "driving_in_curve", False)
)
def stop_sign_and_light(self, v_ego, sm, model_time):
now = time.monotonic()
# 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 self.in_committed_turn_scene(v_ego, sm):
self.reset_stop_light_state()
return
if not sm["starpilotCarState"].trafficModeEnabled:
speed_mph = v_ego * CV.MS_TO_MPH # Convert m/s to mph
# Interp for smooth scaling in 35-45 mph
bp = [0, 35, 45]
low_filter_time = 0.0 # No filtering under 35 mph
tuned_filter_time_curves = self.FILTER_TIME_CURVES[1] # At 35-55 mph
tuned_filter_time_leads = self.FILTER_TIME_LEADS[1]
tuned_filter_time_lights = self.FILTER_TIME_LIGHTS[1]
low_boost = 1.0
tuned_boost = self.LIGHT_BOOSTS[1]
low_cap_factor = 0.0 # No cap under 35 mph
tuned_cap_factor = 1.0
filter_time_curves = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_curves])
filter_time_leads = interp(speed_mph, bp, [low_filter_time, low_filter_time, tuned_filter_time_leads])
filter_time_lights = interp(speed_mph, bp, [self.LOW_SPEED_LIGHT_FILTER_TIME, self.LOW_SPEED_LIGHT_FILTER_TIME, tuned_filter_time_lights])
lead_clear_filter_time = interp(speed_mph, bp, [self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_LOW, self.LEAD_CLEAR_FILTER_TIME_HIGH])
light_boost = interp(speed_mph, bp, [low_boost, low_boost, tuned_boost])
cap_factor = interp(speed_mph, bp, [low_cap_factor, low_cap_factor, tuned_cap_factor])
# Update filter times with interp
self.curvature_filter.update_alpha(filter_time_curves)
self.slow_lead_filter.update_alpha(filter_time_leads)
self.stop_light_filter.update_alpha(filter_time_lights)
self.lead_clear_filter.update_alpha(lead_clear_filter_time)
# Disable stoplight detection at very high speeds to prevent false positives
if speed_mph > 75: # Disable above 75 mph
self.reset_stop_light_state()
return
# Adjust model time with interp boost and gradual cap
adjusted_model_time = model_time * light_boost
if cap_factor > 0:
adjusted_model_time = min(adjusted_model_time, self.LIGHT_MAX_TIME * cap_factor + model_time * (1 - cap_factor)) # Gradual cap
stop_threshold = max(v_ego * adjusted_model_time, 0.0)
if self.stop_light_model_detected:
model_stopping = self.starpilot_planner.model_length < stop_threshold + self.STOP_LIGHT_OFF_MARGIN
else:
model_stopping = self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_ON_MARGIN, 0.0)
self.stop_light_model_detected = model_stopping
# `model_stopped` is a coarse horizon-length check (< 50 m with current constants)
# used elsewhere for force-stop/green-light behavior. Reusing it here causes
# ordinary low-speed cruising to look like a stop prediction and can latch the
# STOP_LIGHT CEM trigger. For the CEM detector, key strictly off the configured
# "predicted stop within N seconds" threshold.
# Key off relevant raw lead presence, not trackingLead. Vision-only GM can
# flap trackingLead around the model-length threshold while leadOne remains
# present; far/stale leads should not suppress true stop-light detection.
lead = getattr(self.starpilot_planner, "lead_one", None)
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", float("inf")))
lead_radar = bool(getattr(lead, "radar", False))
lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
tracking_lead = bool(self.starpilot_planner.tracking_lead)
lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN
vision_stop_approach = (
lead_relevant and
not lead_radar and
lead_prob >= self.STOP_APPROACH_MIN_MODEL_PROB and
lead_speed < self.STOP_APPROACH_MAX_LEAD_SPEED
)
stop_approach_hold_active = now < self.stop_approach_hold_until
trackable_stop_approach = vision_stop_approach and not tracking_lead
if (self.stop_light_detected or self.stop_light_model_detected or stop_approach_hold_active) and trackable_stop_approach:
self.stop_approach_hold_until = now + self.STOP_APPROACH_LATCH_TIME
stop_approach_latched = now < self.stop_approach_hold_until and trackable_stop_approach
handoff_to_stopped_lead = (
lead_relevant and
not tracking_lead and
(
(self.stop_light_detected and lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED) or
stop_approach_latched
)
)
if handoff_to_stopped_lead:
lead_cleared = True
else:
self.lead_clear_filter.update(not lead_relevant)
lead_cleared = self.lead_clear_filter.x >= THRESHOLD
self.stop_light_filter.update(model_stopping and lead_cleared)
model_detector_active = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared)
detector_active = bool(model_detector_active or handoff_to_stopped_lead or stop_approach_latched)
model_hold_qualifies = bool(
self.starpilot_planner.model_stopped or
self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN, 0.0)
)
if model_detector_active and model_hold_qualifies:
self.stop_light_detected_hold_until = now + self.STOP_LIGHT_DETECTED_HOLD_TIME
hold_context_ok = bool((not lead_relevant) or trackable_stop_approach)
self.stop_light_detected = bool(
detector_active or
(hold_context_ok and now < self.stop_light_detected_hold_until)
)
else:
self.reset_stop_light_state()
starpilot/controls/lib/starpilot_acceleration.py
+2 -3
View File
@@ -212,9 +212,8 @@ class StarPilotAcceleration:
stop_context = (
sm["carState"].standstill or
getattr(sm["controlsState"], "forceDecel", False) or
getattr(self.starpilot_planner.starpilot_cem, "stop_light_detected", False) or
getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False) or
getattr(self.starpilot_planner.starpilot_following, "disable_throttle", False)
self.starpilot_planner.longitudinal_intent.stop_detected or
getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False)
)
if (getattr(starpilot_toggles, "speed_limit_controller", False) and
v_ego > SLC_COAST_MIN_SPEED and
starpilot/controls/lib/starpilot_events.py
+1 -1
View File
@@ -71,7 +71,7 @@ class StarPilotEvents:
if not self.starpilot_planner.model_stopped and self.stopped_for_light and starpilot_toggles.green_light_alert:
self.events.add(StarPilotEventName.greenLight)
self.stopped_for_light = self.starpilot_planner.starpilot_cem.stop_light_detected
self.stopped_for_light = self.starpilot_planner.longitudinal_intent.stop_detected
else:
self.stopped_for_light = False
starpilot/controls/lib/starpilot_following.py
+1 -24
View File
@@ -2,8 +2,7 @@
import numpy as np
from openpilot.common.constants import CV
from openpilot.selfdrive.controls.lib.lead_behavior import should_disable_far_lead_throttle
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, MAX_T_FOLLOW
@@ -18,10 +17,6 @@ class StarPilotFollowing:
def __init__(self, StarPilotPlanner):
self.starpilot_planner = StarPilotPlanner
self.disable_throttle = False
self.following_lead = False
self.slower_lead = False
self.acceleration_jerk = 0
self.danger_jerk = 0
self.desired_follow_distance = 0
@@ -78,28 +73,10 @@ class StarPilotFollowing:
self.danger_jerk = self.base_danger_jerk
self.speed_jerk = self.base_speed_jerk
self.following_lead = self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego
self.slower_lead = False
if self.starpilot_planner.starpilot_weather.weather_id != 0:
self.t_follow = min(self.t_follow + self.starpilot_planner.starpilot_weather.increase_following_distance, MAX_T_FOLLOW)
self.disable_throttle = False
if self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.status:
lead_distance = self.starpilot_planner.lead_one.dRel
v_lead = self.starpilot_planner.lead_one.vLead
closing_speed = max(0.0, v_ego - v_lead)
desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow))
self.disable_throttle = should_disable_far_lead_throttle(v_ego, lead_distance, desired_gap, closing_speed, self.following_lead)
if long_control_active and self.starpilot_planner.tracking_lead:
self.update_follow_values(self.starpilot_planner.lead_one.dRel, v_ego, self.starpilot_planner.lead_one.vLead, starpilot_toggles)
self.desired_follow_distance = int(desired_follow_distance(v_ego, self.starpilot_planner.lead_one.vLead, self.t_follow))
else:
self.desired_follow_distance = 0
def update_follow_values(self, lead_distance, v_ego, v_lead, starpilot_toggles):
if starpilot_toggles.conditional_slower_lead and v_lead < v_ego:
distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)
braking_offset = float(np.clip(min(v_ego - v_lead, v_lead) - COMFORT_BRAKE, 1, distance_factor))
self.slower_lead = braking_offset > 1
starpilot/controls/lib/starpilot_vcruise.py
+5 -5
View File
@@ -270,7 +270,7 @@ class StarPilotVCruise:
long_control_active = sm["carControl"].longActive
raw_stop_seen = bool(
self.starpilot_planner.starpilot_cem.stop_light_detected
self.starpilot_planner.longitudinal_intent.stop_detected
or getattr(self.starpilot_planner, "raw_model_stopped", False)
or sm["starpilotCarState"].dashboardStopSign > 0
)
@@ -303,11 +303,11 @@ class StarPilotVCruise:
and not stop_then_turn
)
# CEM/model path: model predicted stop within ACTIVATION_M.
# Unified model path: model predicted stop within ACTIVATION_M.
# Exclude when a lead is present (raw or filtered) — the handoff_to_stopped_lead path
# in CEM can set stop_light_detected even with a lead present, which would incorrectly
# can report stop intent even with a lead present, which would incorrectly
# activate Force Stop and stop the car far behind the lead instead of letting ACC handle it.
cem_path = (self.starpilot_planner.starpilot_cem.stop_light_detected
cem_path = (self.starpilot_planner.longitudinal_intent.stop_detected
and controls_enabled and starpilot_toggles.force_stops
and self.starpilot_planner.model_length < ACTIVATION_M
and self.override_force_stop_timer <= 0
@@ -388,7 +388,7 @@ class StarPilotVCruise:
elif self.standstill_force_stop_hold:
self.force_stop_timer = max(self.force_stop_timer, 0.5)
elif (self.forcing_stop and sm["carState"].standstill and not dash_active and
not self.starpilot_planner.starpilot_cem.stop_light_detected and not raw_model_stopped):
not self.starpilot_planner.longitudinal_intent.stop_detected and not raw_model_stopped):
self.force_stop_timer = 0.0
else:
self.force_stop_timer = max(self.force_stop_timer - DT_MDL * 0.25, 0.0)
starpilot/controls/lib/unified_longitudinal_intent.py
+116
View File
@@ -0,0 +1,116 @@
#!/usr/bin/env python3
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.experimental_state import CEStatus
MODEL_STOP_TIME = 7.0
MODEL_STOP_ENTER = 0.63
MODEL_STOP_EXIT = 0.28
MODEL_STOP_FILTER_TIME = 0.35
SLOW_LEAD_FILTER_TIME = 0.35
TURN_VETO_MAX_SPEED = 15.0 * CV.MPH_TO_MS
TURN_VETO_MIN_STEERING_ANGLE = 45.0
MAX_STOP_DETECTION_SPEED = 75.0 * CV.MPH_TO_MS
class UnifiedLongitudinalIntent:
"""Small scene detector for continuous longitudinal planning.
This class never selects a planner mode. It reports model stop intent and a
UI reason while the longitudinal planner continuously considers cruise,
model, curves, and both leads.
"""
def __init__(self, starpilot_planner):
self.starpilot_planner = starpilot_planner
self.params_memory = starpilot_planner.params_memory
self.stop_filter = FirstOrderFilter(0.0, MODEL_STOP_FILTER_TIME, DT_MDL)
self.lead_filter = FirstOrderFilter(0.0, SLOW_LEAD_FILTER_TIME, DT_MDL)
self.stop_detected = False
self.status_value = CEStatus["OFF"]
self._last_status = None
@staticmethod
def _committed_turn(v_ego, car_state, driving_in_curve):
if car_state.standstill or v_ego > TURN_VETO_MAX_SPEED:
return False
if not (car_state.leftBlinker or car_state.rightBlinker):
return False
return abs(float(car_state.steeringAngleDeg)) >= TURN_VETO_MIN_STEERING_ANGLE or driving_in_curve
def _model_stop_candidate(self, v_ego, sm):
model = sm["modelV2"]
if bool(getattr(model.action, "shouldStop", False)):
return True
if not len(model.position.x):
return False
model_length = max(float(model.position.x[-1]), 0.0)
end_speed = float(model.velocity.x[-1]) if len(model.velocity.x) else v_ego
stop_distance = max(v_ego * MODEL_STOP_TIME - 2.5, 0.0)
return model_length < stop_distance and end_speed < max(2.0, 0.2 * v_ego)
@staticmethod
def _slow_lead_candidate(v_ego, sm):
candidates = []
for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo):
if not bool(getattr(lead, "status", False)):
continue
d_rel = float(getattr(lead, "dRel", np.inf))
v_lead = float(getattr(lead, "vLead", v_ego))
model_prob = float(getattr(lead, "modelProb", 1.0 if getattr(lead, "radar", False) else 0.0))
credible = bool(getattr(lead, "radar", False)) or model_prob >= 0.85
if credible and d_rel < max(40.0, 3.0 * v_ego) and v_lead < v_ego - 0.75:
candidates.append(lead)
return bool(candidates)
def update(self, v_ego, sm, starpilot_toggles):
car_state = sm["carState"]
force_stop = bool(getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False))
stop_sign = bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False))
traffic_mode = bool(sm["starpilotCarState"].trafficModeEnabled)
turn_veto = self._committed_turn(v_ego, car_state, self.starpilot_planner.driving_in_curve)
model_stop = self._model_stop_candidate(v_ego, sm)
model_stop &= not traffic_mode and not turn_veto and v_ego <= MAX_STOP_DETECTION_SPEED
self.stop_filter.update(model_stop)
if force_stop or stop_sign:
self.stop_detected = True
self.stop_filter.x = 1.0
elif self.stop_detected:
self.stop_detected = self.stop_filter.x > MODEL_STOP_EXIT
else:
self.stop_detected = self.stop_filter.x >= MODEL_STOP_ENTER
slow_lead = self._slow_lead_candidate(v_ego, sm)
self.lead_filter.update(slow_lead)
slow_lead = self.lead_filter.x >= MODEL_STOP_ENTER
signal = bool(car_state.leftBlinker or car_state.rightBlinker) and v_ego < 15.0
curve = bool(self.starpilot_planner.road_curvature_detected or self.starpilot_planner.driving_in_curve)
slc_request = bool(self.starpilot_planner.starpilot_vcruise.slc.experimental_mode)
if self.stop_detected:
status = CEStatus["STOP_LIGHT"]
elif slow_lead:
status = CEStatus["LEAD"]
elif signal:
status = CEStatus["SIGNAL"]
elif curve:
status = CEStatus["CURVATURE"]
elif slc_request:
status = CEStatus["SPEED_LIMIT"]
elif bool(getattr(starpilot_toggles, "longitudinal_model_preference", False)):
status = CEStatus["USER_OVERRIDDEN"]
else:
status = CEStatus["OFF"]
self.status_value = status
if status != self._last_status:
self.params_memory.put_int("CEStatus", status)
self._last_status = status
starpilot/controls/starpilot_card.py
+2 -20
View File
@@ -6,14 +6,7 @@ from openpilot.common.params import Params
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType
from openpilot.selfdrive.selfdrived.events import ET
from openpilot.starpilot.common.experimental_state import (
CCStatus,
CEStatus,
next_manual_cc_status,
next_manual_ce_status,
sync_manual_cc_state,
sync_manual_ce_state,
)
from openpilot.starpilot.common.experimental_state import toggle_longitudinal_model_preference
from openpilot.starpilot.common.favorite_slots import toggle_favorite_slot
from openpilot.starpilot.common.starpilot_utilities import is_FrogsGoMoo
from openpilot.starpilot.common.starpilot_variables import ERROR_LOGS_PATH, GearShifter, NON_DRIVING_GEARS
@@ -104,18 +97,7 @@ class StarPilotCard:
if getattr(starpilot_toggles, "safe_mode", False):
return
if starpilot_toggles.conditional_experimental_mode:
current_status = self.params_memory.get_int("CEStatus", default=CEStatus["OFF"])
override_value = next_manual_ce_status(current_status, sm["selfdriveState"].experimentalMode)
self.params_memory.put_int("CEStatus", override_value)
sync_manual_ce_state(self.params, override_value)
elif getattr(starpilot_toggles, "conditional_chill_mode", False):
current_status = self.params_memory.get_int("CCStatus", default=CCStatus["OFF"])
override_value = next_manual_cc_status(current_status, sm["selfdriveState"].experimentalMode)
self.params_memory.put_int("CCStatus", override_value)
sync_manual_cc_state(self.params, override_value)
else:
self.params.put_bool_nonblocking("ExperimentalMode", not sm["selfdriveState"].experimentalMode)
toggle_longitudinal_model_preference(self.params, self.params_memory)
def update(self, carState, starpilotCarState, sm, starpilot_toggles):
self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled")
starpilot/controls/starpilot_planner.py
+14 -96
View File
@@ -4,33 +4,23 @@ import math
import time
import cereal.messaging as messaging
import numpy as np
from openpilot.common.constants import CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.gps import get_gps_location_service
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.selfdrive.controls.lib.lead_behavior import (
is_radarless_matched_follow_window,
should_hold_tracked_vision_lead,
should_track_lead,
)
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_mpc_lib.long_mpc import A_CHANGE_COST, DANGER_ZONE_COST, J_EGO_COST
from openpilot.starpilot.common.starpilot_utilities import calculate_lane_width, calculate_road_curvature
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
from openpilot.starpilot.common.starpilot_variables import CRUISING_SPEED, MINIMUM_LATERAL_ACCELERATION, PLANNER_TIME
from openpilot.starpilot.controls.lib.starpilot_acceleration import StarPilotAcceleration
from openpilot.starpilot.controls.lib.starpilot_events import StarPilotEvents
from openpilot.starpilot.controls.lib.starpilot_following import StarPilotFollowing
from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
from openpilot.starpilot.controls.lib.unified_longitudinal_intent import UnifiedLongitudinalIntent
from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
RADARLESS_TRACK_HOLD_TIME = 0.45
def _sanitize_json_value(value):
if isinstance(value, float):
@@ -52,11 +42,10 @@ class StarPilotPlanner:
self.params_memory = Params(memory=True)
self.starpilot_acceleration = StarPilotAcceleration(self)
self.starpilot_cem = ConditionalExperimentalMode(self)
self.starpilot_ccm = ConditionalChillMode(self, self.starpilot_cem)
self.starpilot_events = StarPilotEvents(self, error_log, ThemeManager)
self.starpilot_following = StarPilotFollowing(self)
self.starpilot_vcruise = StarPilotVCruise(self)
self.longitudinal_intent = UnifiedLongitudinalIntent(self)
self.starpilot_weather = WeatherChecker(self)
self.driving_in_curve = False
@@ -82,7 +71,6 @@ class StarPilotPlanner:
self._lane_width_counter = 0
self.lateral_acceleration = 0
self.model_length = 0
self.lead_path_y = 0
self.road_curvature = 0
self.time_to_curve = 0
self.v_cruise = 0
@@ -91,9 +79,6 @@ class StarPilotPlanner:
self.gps_location_service = get_gps_location_service(self.params)
self.tracking_lead_filter = FirstOrderFilter(0, 0.5, DT_MDL)
self.radarless_follow_hold_until = 0.0
def shutdown(self):
self.starpilot_vcruise.slc.shutdown()
self.starpilot_weather.executor.shutdown(wait=False, cancel_futures=True)
@@ -189,13 +174,6 @@ class StarPilotPlanner:
self.CS_prev_right_blinker = CS.rightBlinker
self.model_length = sm["modelV2"].position.x[-1]
model_position = sm["modelV2"].position
model_path_y = getattr(model_position, "y", [])
if len(model_path_y) == len(model_position.x):
self.lead_path_y = float(np.interp(self.lead_one.dRel, model_position.x, model_path_y))
else:
self.lead_path_y = 0.0
self.raw_model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop
@@ -203,24 +181,11 @@ class StarPilotPlanner:
self.road_curvature_detected = (1 / abs(self.road_curvature))**0.5 < v_ego > CRUISING_SPEED and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker)
if not sm["carState"].standstill:
self.tracking_lead = self.update_lead_status(v_ego)
self.tracking_lead = bool(self.lead_one.status)
self.starpilot_following.update(controls_enabled, v_ego, sm, starpilot_toggles)
conditional_tracking_active = controls_enabled or sm["starpilotCarState"].alwaysOnLateralEnabled
if conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_experimental_mode", False)):
# Keep CEM's filters warm in AOL so engagement can inherit the current scene.
self.starpilot_cem.update(v_ego, sm, starpilot_toggles)
self.starpilot_ccm.experimental_mode = True
elif conditional_tracking_active and bool(getattr(starpilot_toggles, "conditional_chill_mode", False)):
self.starpilot_ccm.update(v_ego, v_cruise, sm, starpilot_toggles)
self.starpilot_cem.experimental_mode = False
else:
self.starpilot_ccm.experimental_mode = True
self.starpilot_cem.experimental_mode = False
self.starpilot_cem.curve_detected = False
self.starpilot_cem.stop_sign_and_light(v_ego, sm, PLANNER_TIME - 2)
self.longitudinal_intent.update(v_ego, sm, starpilot_toggles)
self.v_cruise = self.starpilot_vcruise.update(controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles)
@@ -231,52 +196,6 @@ class StarPilotPlanner:
else:
self.starpilot_weather.weather_id = 0
def update_lead_status(self, v_ego):
following_lead = should_track_lead(
self.lead_one.status,
self.lead_one.dRel,
self.model_length,
STOP_DISTANCE,
v_ego,
v_lead=self.lead_one.vLead,
radar=bool(getattr(self.lead_one, "radar", False)),
)
continuity_candidate = self.tracking_lead or self.tracking_lead_filter.x >= THRESHOLD * 0.6
if not following_lead and continuity_candidate:
following_lead = should_hold_tracked_vision_lead(
self.lead_one.status,
self.lead_one.dRel,
self.model_length,
STOP_DISTANCE,
v_ego,
model_prob=float(getattr(self.lead_one, "modelProb", 0.0)),
y_rel=float(getattr(self.lead_one, "yRel", 0.0)),
path_y=self.lead_path_y,
radar=bool(getattr(self.lead_one, "radar", False)),
)
now_t = time.monotonic()
lead_radar = bool(getattr(self.lead_one, "radar", False))
t_follow = max(float(getattr(self.starpilot_following, "t_follow", 0.0)), 1.45)
matched_follow_window = self.lead_one.status and is_radarless_matched_follow_window(
v_ego,
self.lead_one.dRel,
self.lead_one.vLead,
t_follow,
radar=lead_radar,
lead_brake=max(0.0, -float(getattr(self.lead_one, "aLeadK", 0.0))),
lead_prob=float(getattr(self.lead_one, "modelProb", 0.0)),
)
if matched_follow_window and (following_lead or self.tracking_lead or self.tracking_lead_filter.x >= THRESHOLD * 0.6):
self.radarless_follow_hold_until = now_t + RADARLESS_TRACK_HOLD_TIME
elif lead_radar or not self.lead_one.status:
self.radarless_follow_hold_until = 0.0
if not following_lead and matched_follow_window and now_t < self.radarless_follow_hold_until:
following_lead = True
self.tracking_lead_filter.update(following_lead)
return self.tracking_lead_filter.x >= THRESHOLD
def publish(self, theme_updated, sm, pm, starpilot_toggles, serialized_toggles=""):
starpilot_plan_send = messaging.new_message("starpilotPlan")
starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"])
@@ -293,16 +212,15 @@ class StarPilotPlanner:
starpilotPlan.cscTraining = self.starpilot_vcruise.csc.enable_training
starpilotPlan.desiredFollowDistance = int(self.starpilot_following.desired_follow_distance)
starpilotPlan.disableThrottle = self.starpilot_following.disable_throttle
starpilotPlan.disableThrottle = False
starpilotPlan.trackingLead = self.tracking_lead
conditional_experimental_mode = False
if starpilot_toggles.conditional_experimental_mode:
conditional_experimental_mode = self.starpilot_cem.experimental_mode
elif starpilot_toggles.conditional_chill_mode:
conditional_experimental_mode = self.starpilot_ccm.experimental_mode
starpilotPlan.experimentalMode = conditional_experimental_mode or self.starpilot_vcruise.slc.experimental_mode
starpilotPlan.experimentalMode = bool(
not getattr(starpilot_toggles, "safe_mode", False) and (
starpilot_toggles.longitudinal_model_preference or
self.longitudinal_intent.status_value != 0
)
)
starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop
starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length
@@ -327,7 +245,7 @@ class StarPilotPlanner:
starpilotPlan.maxAcceleration = float(self.starpilot_acceleration.max_accel)
starpilotPlan.minAcceleration = float(self.starpilot_acceleration.min_accel)
starpilotPlan.redLight = self.starpilot_cem.stop_light_detected
starpilotPlan.redLight = self.longitudinal_intent.stop_detected
starpilotPlan.roadCurvature = self.road_curvature
starpilot/controls/tests/test_starpilot_card.py
+6 -8
View File
@@ -70,7 +70,6 @@ def make_toggles(**overrides):
"bookmark_via_cancel_long": False,
"bookmark_via_cancel_very_long": False,
"bookmark_via_lkas": False,
"conditional_experimental_mode": False,
"experimental_mode_via_lkas": False,
"force_coast_via_lkas": False,
"lkas_allowed_for_aol": False,
@@ -592,21 +591,20 @@ def test_pacifica_hybrid_main_aol_waits_for_set_press(monkeypatch, tmp_path):
assert ret.alwaysOnLateralEnabled is False
def test_conditional_chill_wheel_override_cycles_manual_state(monkeypatch, tmp_path):
def test_wheel_override_temporarily_flips_model_preference(monkeypatch, tmp_path):
monkeypatch.setattr(spc, "Params", FakeParams)
monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0))
sm = make_sm()
toggles = make_toggles(conditional_chill_mode=True)
sm["selfdriveState"].experimentalMode = True
card.handle_experimental_mode(sm, toggles)
assert card.params_memory.get_int("CCStatus") == spc.CCStatus["USER_CHILL"]
toggles = make_toggles()
card.handle_experimental_mode(sm, toggles)
assert card.params_memory.get_int("CCStatus") == spc.CCStatus["OFF"]
assert card.params_memory.get_int("LongitudinalModelPreferenceOverride") == 1
card.handle_experimental_mode(sm, toggles)
assert card.params_memory.get_int("LongitudinalModelPreferenceOverride") == 0
def test_cancel_button_short_press_can_run_independent_mapping(monkeypatch, tmp_path):
starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json
+16 -191
View File
@@ -589,123 +589,29 @@
"settings_tier": "advanced"
},
{
"key": "ConditionalExperimental",
"label": "Conditional Experimental Mode",
"description": "Automatically switch to \"Experimental Mode\" when set conditions are met. Allows the model to handle challenging situations with smarter decision making.",
"data_type": "bool",
"ui_type": "toggle",
"is_parent_toggle": true,
"settings_tier": "simple"
},
{
"key": "PersistExperimentalState",
"label": "Persist Experimental State",
"description": "Keep your manual Conditional Experimental override through reboots until you manually clear it.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
"key": "CESpeed",
"label": "Below",
"description": "Switch to \"Experimental Mode\" when driving below this speed without a lead to help openpilot handle low-speed situations more smoothly.",
"key": "LongitudinalModelPreference",
"label": "Longitudinal Preference",
"description": "Set-Speed First prioritizes steady cruise on open roads. Model First gives the driving model more influence. Both use the same lead and stop safety.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
"key": "CESpeedLead",
"label": "Below (With Lead)",
"description": "Switch to \"Experimental Mode\" when driving below this speed with a lead.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
"key": "CECurves",
"label": "Curve Detected Ahead",
"description": "Switch to \"Experimental Mode\" when a curve is detected to allow the model to set an appropriate speed for the curve.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
"key": "CEStopLights",
"label": "\\\"Detected\\\" Stop Lights/Signs",
"description": "Switch to \"Experimental Mode\" whenever the driving model \"detects\" a red light or stop sign.\n\nDisclaimer: 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!",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
"key": "CELead",
"label": "Lead Detected Ahead",
"description": "Switch to \"Experimental Mode\" when a slower or stopped vehicle is detected. Can make braking smoother and more reliable on some vehicles.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
"is_parent_toggle": true,
"settings_tier": "simple"
},
{
"key": "CESlowerLead",
"label": "Slower Lead",
"description": "Switch to \"Experimental Mode\" when a slower lead vehicle is detected ahead.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "CELead",
"settings_tier": "simple"
},
{
"key": "CEStoppedLead",
"label": "Stopped Lead",
"description": "Switch to \"Experimental Mode\" when a stopped lead vehicle is detected ahead.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "CELead",
"settings_tier": "simple"
},
{
"key": "CEModelStopTime",
"label": "Predicted Stop In",
"description": "Switch to \"Experimental Mode\" when openpilot predicts a stop within the set time. This is usually triggered when the model \"sees\" a red light or stop sign ahead.\n\nDisclaimer: 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!",
"data_type": "float",
"ui_type": "numeric",
"min": 0.0,
"max": 9.0,
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
"key": "CESignalSpeed",
"label": "Turn Signal Below",
"description": "Switch to \"Experimental Mode\" when using a turn signal below the set speed to allow the model to choose an appropriate speed for smoother left and right turns.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"parent_key": "ConditionalExperimental",
"ui_type": "dropdown",
"options": [
{
"value": 0,
"label": "Set-Speed First"
},
{
"value": 1,
"label": "Model First"
}
],
"settings_tier": "simple"
},
{
"key": "ShowCEMStatus",
"label": "Status Widget",
"description": "Show which condition triggered \"Experimental Mode\" on the driving screen.",
"label": "Longitudinal Status",
"description": "Show which constraint is currently influencing the unified longitudinal planner.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalExperimental",
"settings_tier": "simple"
},
{
@@ -1860,87 +1766,6 @@
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
{
"key": "ConditionalChill",
"label": "Conditional Chill Mode",
"description": "Keep \"Experimental Mode\" on by default, but temporarily switch to \"Chill Mode\" in simple cruising scenes where speed holding is usually better.",
"data_type": "bool",
"ui_type": "toggle",
"is_parent_toggle": true,
"settings_tier": "advanced"
},
{
"key": "PersistChillState",
"label": "Persist Chill State",
"description": "Keep your manual Conditional Chill override through reboots until you manually clear it.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
{
"key": "CCMSpeed",
"label": "Above",
"description": "Switch to \"Chill Mode\" on open roads above this speed when no lead is detected and the car is still below the set speed.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
{
"key": "CCMSpeedLead",
"label": "Above (With Lead)",
"description": "Switch to \"Chill Mode\" when following a stable lead above this speed.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
{
"key": "CCMLead",
"label": "Stable Lead Ahead",
"description": "Switch to \"Chill Mode\" when following a steady, well-tracked lead vehicle at cruising speeds.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
{
"key": "CCMLaunchAssist",
"label": "Launch Assist",
"description": "Temporarily switch to \"Chill Mode\" when starting from a stop if planner is already allowing throttle. Useful if your car launches too slowly from lights or stop signs.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
{
"key": "CCMSetSpeedMargin",
"label": "Set Speed Margin",
"description": "How far below the set speed the car must be before open-road Conditional Chill can engage.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 15.0,
"step": 1.0,
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
{
"key": "ShowCCMStatus",
"label": "Status Widget",
"description": "Show which condition triggered \"Chill Mode\" on the driving screen.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "ConditionalChill",
"settings_tier": "advanced"
},
{
"key": "SLCAbbreviatedSources",
"label": "Show Abbreviated Icon Sources",
starpilot/system/the_galaxy/tests/test_device_settings_layout.py
+2 -3
View File
@@ -43,7 +43,7 @@ def test_galaxy_layout_contains_basic_mode_controls():
assert {"AlwaysOnLateral", "LaneChanges", "QOLLateral"} <= sections["Lateral (Steering)"].keys()
assert {
"ConditionalExperimental",
"LongitudinalModelPreference",
"CurveSpeedController",
"AccelerationProfile",
"DecelerationProfile",
@@ -86,7 +86,7 @@ def test_requested_simple_and_advanced_settings_tiers():
assert lateral[key]["settings_tier"] == "advanced"
for key in (
"ConditionalExperimental",
"LongitudinalModelPreference",
"CurveSpeedController",
"LongitudinalTune",
"AccelerationProfile",
@@ -102,7 +102,6 @@ def test_requested_simple_and_advanced_settings_tiers():
"TacoTune",
"NavLongitudinalAllowed",
"SpeedLimitController",
"ConditionalChill",
):
assert longitudinal[key]["settings_tier"] == "advanced"
starpilot/system/the_galaxy/the_galaxy.py
+1 -50
View File
@@ -61,7 +61,6 @@ from openpilot.starpilot.common.maps_catalog import (
schedule_label,
schedule_param_value,
)
from openpilot.starpilot.common.experimental_state import sync_persist_chill_state, sync_persist_experimental_state
from openpilot.starpilot.common.favorite_slots import (
FAVORITE_ACTION_OPTIONS,
FAVORITE_SLOTS_PARAM,
@@ -936,15 +935,7 @@ _TROUBLESHOOT_PERSONALITY_KEYS = [
]
_TROUBLESHOOT_CEM_KEYS = [
"ConditionalExperimental",
"CESpeed",
"CESpeedLead",
"CECurves",
"CELead",
"CESlowerLead",
"CEStoppedLead",
"CEModelStopTime",
"CESignalSpeed",
"LongitudinalModelPreference",
"ShowCEMStatus",
]
@@ -4510,22 +4501,6 @@ def setup(app):
"updated": updated,
}), 200
if key in {"ConditionalExperimental", "ConditionalChill"}:
enabled = str_val.strip() in ("1", "true", "True")
params.put_bool(key, enabled)
updated = {key: enabled}
if enabled:
other_key = "ConditionalChill" if key == "ConditionalExperimental" else "ConditionalExperimental"
params.put_bool(other_key, False)
updated[other_key] = False
update_starpilot_toggles()
return jsonify({
"message": f"Parameter '{key}' updated successfully.",
"updated": updated,
}), 200
if key == "CustomAccelProfile":
enabled = str_val.strip() in ("1", "true", "True")
params.put_bool(key, enabled)
@@ -4545,30 +4520,6 @@ def setup(app):
"updated": updated,
}), 200
if key == "PersistExperimentalState":
enabled = str_val.strip() in ("1", "true", "True")
sync_persist_experimental_state(params, params_memory, enabled)
update_starpilot_toggles()
return jsonify({
"message": f"Parameter '{key}' updated successfully.",
"updated": {
"PersistExperimentalState": enabled,
"PersistedCEStatus": params.get_int("PersistedCEStatus", default=0),
},
}), 200
if key == "PersistChillState":
enabled = str_val.strip() in ("1", "true", "True")
sync_persist_chill_state(params, params_memory, enabled)
update_starpilot_toggles()
return jsonify({
"message": f"Parameter '{key}' updated successfully.",
"updated": {
"PersistChillState": enabled,
"PersistedCCStatus": params.get_int("PersistedCCStatus", default=0),
},
}), 200
if key == "IsRHD":
enabled = str_val.strip() in ("1", "true", "True")
params.put_bool("IsRHD", enabled)
tools/StarPilot/generate_galaxy_layout.py
+43 -53
View File
@@ -25,6 +25,26 @@ DROPDOWN_MAPPING = {
# Custom controls implemented outside the tuple vectors in Qt settings panels.
# Inject these so regenerated galaxy layouts retain equivalent functionality.
INJECTED_SECTION_PARAMS = {
"Longitudinal (Speed & Following)": [
{
"key": "LongitudinalModelPreference",
"label": "Longitudinal Preference",
"description": "Set-Speed First prioritizes steady cruise on open roads. Model First gives the driving model more influence. Both use the same lead and stop safety.",
"data_type": "int",
"ui_type": "dropdown",
"options": [
{"value": 0, "label": "Set-Speed First"},
{"value": 1, "label": "Model First"},
],
},
{
"key": "ShowCEMStatus",
"label": "Longitudinal Status",
"description": "Show which constraint is currently influencing the unified longitudinal planner.",
"data_type": "bool",
"ui_type": "toggle",
},
],
"Vehicle": [
{
"key": "CarMake",
@@ -61,13 +81,35 @@ INJECTED_SECTION_PARAMS = {
# Keys explicitly hidden from The Galaxy's generic settings UI.
HIDDEN_KEYS = {
"CCMLaunchAssist",
"CCMLead",
"CCMSetSpeedMargin",
"CCMSpeed",
"CCMSpeedLead",
"CECurves",
"CECurvesLead",
"CELead",
"CEModelStopTime",
"CESignalLaneDetection",
"CESignalSpeed",
"CESlowerLead",
"CESpeed",
"CESpeedLead",
"CEStopLights",
"CEStoppedLead",
"ConditionalChill",
"ConditionalExperimental",
"FrogsGoMoosTweak",
"HumanAcceleration",
"DisableWideRoad",
"LockDoorsTimer",
"NewLongAPI",
"ToyotaDoors",
"PersistChillState",
"PersistExperimentalState",
"ReverseCruise",
"ShowCCMStatus",
"ShowCEMStatus",
"ToyotaDoors",
}
HIDDEN_SECTION_NAMES = {"Model & Customization"}
@@ -168,8 +210,6 @@ PARENT_KEYS_MAPPING = {
"longitudinal_settings.cc": {
"advancedLongitudinalTuneKeys": "AdvancedLongitudinalTune",
"aggressivePersonalityKeys": "AggressivePersonalityProfile",
"conditionalChillKeys": "ConditionalChill",
"conditionalExperimentalKeys": "ConditionalExperimental",
"curveSpeedKeys": "CurveSpeedController",
"customDrivingPersonalityKeys": "CustomPersonalities",
"longitudinalTuneKeys": "LongitudinalTune",
@@ -546,58 +586,8 @@ def parse_cpp_file(filename):
if key in child_to_parent: s["parent_key"] = child_to_parent[key]
if key in ALL_PARENT_KEYS: s["is_parent_toggle"] = True
if key == "CELead":
s["is_parent_toggle"] = True
items.append(s)
# Mirror CELead's split sub-toggles from StarPilotButtonToggleControl.
if key == "CELead":
items.extend([
{
"key": "CESlowerLead",
"label": "Slower Lead",
"description": "Switch to \"Experimental Mode\" when a slower lead vehicle is detected ahead.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "CELead",
},
{
"key": "CEStoppedLead",
"label": "Stopped Lead",
"description": "Switch to \"Experimental Mode\" when a stopped lead vehicle is detected ahead.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "CELead",
},
])
# Mirror CESpeed/CCMSpeed's dual sliders (with-lead variants) from Qt.
if key == "CESpeed":
items.append({
"key": "CESpeedLead",
"label": "Below (With Lead)",
"description": "Switch to \"Experimental Mode\" when driving below this speed with a lead.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"parent_key": "ConditionalExperimental",
})
elif key == "CCMSpeed":
items.append({
"key": "CCMSpeedLead",
"label": "Above (With Lead)",
"description": "Switch to \"Chill Mode\" when following a stable lead above this speed.",
"data_type": "int",
"ui_type": "numeric",
"min": 0.0,
"max": 99.0,
"step": 1.0,
"parent_key": "ConditionalChill",
})
return items