From 0889b3b0c258ed5a40a254c7e8b69f370fbd6667 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 28 Apr 2026 00:50:20 -0500 Subject: [PATCH] Dom's Plan 4 --- selfdrive/controls/lib/longcontrol.py | 42 ++++++- .../controls/lib/longitudinal_planner.py | 47 +++++++ .../test_conditional_experimental_mode.py | 35 +++++- selfdrive/controls/tests/test_longcontrol.py | 49 ++++++++ .../tests/test_longitudinal_planner.py | 22 ++++ .../tests/test_starpilot_acceleration.py | 117 ++++++++++++++++++ .../lib/conditional_experimental_mode.py | 24 +++- .../controls/lib/starpilot_acceleration.py | 90 +++++++++++++- 8 files changed, 410 insertions(+), 16 deletions(-) create mode 100644 selfdrive/controls/tests/test_starpilot_acceleration.py diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 454f6779d..cb3604bac 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -11,6 +11,8 @@ from openpilot.starpilot.common.testing_grounds import testing_ground CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] clip = np.clip interp = np.interp +STOPPING_RELEASE_HYSTERESIS = 0.35 +STOPPING_RELEASE_MIN_ACCEL = 0.2 LongCtrlState = car.CarControl.Actuators.LongControlState @@ -25,13 +27,15 @@ def apply_deadzone(error, deadzone): def long_control_state_trans(CP, active, long_control_state, v_ego, - should_stop, brake_pressed, cruise_standstill, starpilot_toggles): + should_stop, brake_pressed, cruise_standstill, starpilot_toggles, + allow_stopping_release=True): # Ignore cruise standstill if car has a gas interceptor cruise_standstill = cruise_standstill and not CP.enableGasInterceptorDEPRECATED stopping_condition = should_stop starting_condition = (not should_stop and not cruise_standstill and not brake_pressed) + stopping_release_condition = starting_condition and allow_stopping_release started_condition = v_ego > starpilot_toggles.vEgoStarting if not active: @@ -48,9 +52,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, long_control_state = LongCtrlState.pid elif long_control_state == LongCtrlState.stopping: - if starting_condition and CP.startingState: + if stopping_release_condition and CP.startingState: long_control_state = LongCtrlState.starting - elif starting_condition: + elif stopping_release_condition: long_control_state = LongCtrlState.pid elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]: @@ -115,6 +119,7 @@ class LongControl: self.last_output_accel = 0.0 self.last_a_target = 0.0 self.integrator_hold_frames = 0 + self.stop_release_counter = 0 self.is_gm_pedal_long = bool( CP.brand == "gm" and CP.enableGasInterceptorDEPRECATED and (CP.flags & GMFlags.PEDAL_LONG.value) ) @@ -149,10 +154,33 @@ class LongControl: self.mode_transition_duration = 1.0 self.transitioning = False - def reset(self): + def reset(self, preserve_stop_release=False): self.pid.reset() self.last_a_target = 0.0 self.integrator_hold_frames = 0 + if not preserve_stop_release: + self.stop_release_counter = 0 + + def _stop_release_ready(self, CS, a_target, should_stop, starpilot_toggles): + if self.long_control_state != LongCtrlState.stopping: + self.stop_release_counter = 0 + return True + + if should_stop or CS.brakePressed or CS.cruiseState.standstill: + self.stop_release_counter = 0 + return False + + if CS.vEgo > starpilot_toggles.vEgoStarting: + self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) + return True + + if a_target > STOPPING_RELEASE_MIN_ACCEL: + max_frames = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) + self.stop_release_counter = min(self.stop_release_counter + 1, max_frames) + else: + self.stop_release_counter = 0 + + return self.stop_release_counter >= int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) def _get_pedal_long_freeze(self, a_target, error, v_ego, accel_limits): volt_test_tune_handoff = self.is_volt and testing_ground.use_2 @@ -198,9 +226,11 @@ class LongControl: self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] + allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, starpilot_toggles) self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, should_stop, CS.brakePressed, - CS.cruiseState.standstill, starpilot_toggles) + CS.cruiseState.standstill, starpilot_toggles, + allow_stopping_release=allow_stopping_release) if self.long_control_state == LongCtrlState.off: self.reset() output_accel = 0. @@ -210,7 +240,7 @@ class LongControl: if output_accel > starpilot_toggles.stopAccel: output_accel = min(output_accel, 0.0) output_accel -= starpilot_toggles.stoppingDecelRate * DT_CTRL - self.reset() + self.reset(preserve_stop_release=True) elif self.long_control_state == LongCtrlState.starting: if starpilot_toggles.human_acceleration: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1cf2f9909..62b8aa5be 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -39,6 +39,13 @@ VISION_LEAD_APPROACH_MAX_DECEL = 0.45 VISION_LEAD_APPROACH_MIN_DECEL = 0.12 VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85 VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 +VISION_SLOW_LEAD_MAX_SPEED = 5.0 +VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5 +VISION_SLOW_LEAD_TRIGGER_TTC = 4.5 +VISION_SLOW_LEAD_FULL_TTC = 2.0 +VISION_SLOW_LEAD_MAX_DECEL = 1.2 +VISION_SLOW_LEAD_MIN_DECEL = 0.18 +VISION_SLOW_LEAD_MIN_MODEL_PROB = 0.9 LEAD_APPROACH_TFOLLOW_TRIGGER_TIME = 4.5 LEAD_APPROACH_TFOLLOW_FULL_TIME = 1.5 LEAD_APPROACH_TFOLLOW_MAX_DELTA = 0.18 @@ -304,6 +311,43 @@ class LongitudinalPlanner: return max(accel_min, -approach_decel) + def get_vision_slow_stopped_lead_cap(self, lead, v_ego, accel_min, t_follow): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + if lead_prob < VISION_SLOW_LEAD_MIN_MODEL_PROB or float(lead.vLead) > VISION_SLOW_LEAD_MAX_SPEED: + return None + + lead_brake = max(0.0, -float(lead.aLeadK)) + reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) + closing_speed = max(0.0, v_ego - lead.vLead) + projected_closing_speed = closing_speed + lead_brake * reaction_t + if projected_closing_speed < VISION_SLOW_LEAD_MIN_CLOSING_SPEED: + return None + + stop_gap = float(max(STOP_DISTANCE + 1.0, 2.5 + 0.15 * max(float(lead.vLead), 0.0))) + delay_buffer = projected_closing_speed * reaction_t + available_gap = max(float(lead.dRel) - stop_gap - delay_buffer, 0.5) + projected_ttc = available_gap / max(projected_closing_speed, 0.1) + if projected_ttc > VISION_SLOW_LEAD_TRIGGER_TTC: + return None + + time_factor = float(np.clip((VISION_SLOW_LEAD_TRIGGER_TTC - projected_ttc) / + (VISION_SLOW_LEAD_TRIGGER_TTC - VISION_SLOW_LEAD_FULL_TTC), 0.0, 1.0)) + prob_factor = float(np.clip((lead_prob - VISION_SLOW_LEAD_MIN_MODEL_PROB) / + (VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_SLOW_LEAD_MIN_MODEL_PROB), 0.0, 1.0)) + speed_factor = float(np.clip((VISION_SLOW_LEAD_MAX_SPEED - max(float(lead.vLead), 0.0)) / + VISION_SLOW_LEAD_MAX_SPEED, 0.0, 1.0)) + required_decel = (projected_closing_speed ** 2) / (2.0 * available_gap) + decel_scale = 0.45 + 0.35 * time_factor + 0.20 * speed_factor + approach_decel = min(VISION_SLOW_LEAD_MAX_DECEL, required_decel * decel_scale) + approach_decel *= 0.65 + 0.35 * prob_factor + if approach_decel < VISION_SLOW_LEAD_MIN_DECEL: + return None + + return max(accel_min, -approach_decel) + def get_dynamic_t_follow(self, base_t_follow, lead, v_ego): base_t_follow = float(base_t_follow) target_t_follow = base_t_follow @@ -628,6 +672,9 @@ class LongitudinalPlanner: cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min) if cap is not None: close_lead_caps.append(cap) + slow_stop_cap = self.get_vision_slow_stopped_lead_cap(lead, v_ego, output_accel_min, effective_t_follow) + if slow_stop_cap is not None: + close_lead_caps.append(slow_stop_cap) approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, effective_t_follow) if approach_cap is not None: close_lead_caps.append(approach_cap) diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index cb0874890..4b6ea7af1 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -4,18 +4,22 @@ from types import SimpleNamespace from openpilot.common.constants import CV from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module +import openpilot.starpilot.controls.lib.conditional_experimental_mode as conditional_experimental_mode_module from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False, - lead_status: bool = False, lead_d_rel: float = float("inf")): + lead_status: bool = False, lead_d_rel: float = float("inf"), + lead_v_lead: float = 0.0, lead_model_prob: float = 0.0, lead_radar: bool = False): planner = SimpleNamespace( params=None, params_memory=None, model_length=model_length, model_stopped=model_stopped, tracking_lead=tracking_lead, - lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel), + starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=False), + lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel, vLead=lead_v_lead, + modelProb=lead_model_prob, radar=lead_radar), ) return ConditionalExperimentalMode(planner) @@ -102,6 +106,33 @@ def test_stop_light_stays_latched_until_untracked_stopped_lead_handoff(): assert cem.stop_light_detected +def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_flicker(monkeypatch): + v_ego = 40 * CV.MPH_TO_MS + model_length = v_ego * 3.8 + cem = make_cem( + model_length=model_length, + lead_status=True, + lead_d_rel=model_length - 5.0, + lead_v_lead=3.0, + lead_model_prob=0.98, + ) + + run_stop_light_detector(cem, v_ego, steps=20) + assert cem.stop_light_detected + + monotonic_values = iter([10.0, 10.2]) + monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values)) + + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + cem.stop_light_detected = False + cem.stop_light_model_detected = False + cem.stop_light_filter.x = 0.0 + cem.starpilot_planner.model_length = v_ego * 9.0 + cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0) + + assert cem.stop_light_detected + + class DummyThemeManager: def update_wheel_image(self, *args, **kwargs): pass diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index a4ccb0ffd..fb56c9989 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -77,6 +77,18 @@ def test_starting(): assert next_state == LongCtrlState.pid +def test_stopping_release_hysteresis_blocks_immediate_launch(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + toggles = make_toggles(vEgoStarting=0.5) + active = True + current_state = LongCtrlState.stopping + + next_state = long_control_state_trans(CP, active, current_state, v_ego=0.0, + should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles, + allow_stopping_release=False) + assert next_state == LongCtrlState.stopping + + def test_starting_accel_unchanged_when_custom_profile_disabled(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] @@ -125,6 +137,43 @@ def test_starting_accel_obeys_a_target_cap_when_custom_profile_enabled(): assert output_accel == 0.1 +def test_update_requires_sustained_positive_target_to_leave_stopping(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.stopping + CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL)) + for _ in range(release_frames - 1): + output_accel = lc.update( + active=True, + CS=CS, + a_target=0.5, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5), + ) + assert lc.long_control_state == LongCtrlState.stopping + assert output_accel <= 0.0 + + lc.update( + active=True, + CS=CS, + a_target=0.5, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5), + ) + + assert lc.long_control_state == LongCtrlState.starting + + def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch): CP = car.CarParams.new_message() CP.brand = "gm" diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 70354e067..3d20204ac 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -225,6 +225,28 @@ def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap(): assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None +def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop(): + v_ego = 13.207 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=49.131, v_lead=1.837, a_lead=-0.312, radar=False, model_prob=0.942) + + slow_stop_cap = planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.75) + + assert slow_stop_cap is not None + assert slow_stop_cap < -0.9 + assert slow_stop_cap > -1.25 + + +def test_vision_slow_stopped_lead_cap_ignores_far_high_speed_stop_candidate(): + v_ego = 33.5 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=183.0, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.995) + + assert planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.45) is None + + @pytest.mark.parametrize("model_version", ["v11", "v12"]) def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version): v_ego = 21.535 diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py new file mode 100644 index 000000000..09e58c479 --- /dev/null +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -0,0 +1,117 @@ +from types import SimpleNamespace + +import pytest + +from openpilot.common.constants import CV +from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN +from openpilot.starpilot.common.accel_profile import ACCELERATION_PROFILES, DECELERATION_PROFILES +from openpilot.starpilot.controls.lib.starpilot_acceleration import ( + A_CRUISE_MIN_ECO, + StarPilotAcceleration, + get_slc_shaped_min_accel, +) + + +class FakePlanner: + def __init__(self): + self.v_cruise = 0.0 + self.starpilot_weather = SimpleNamespace(weather_id=0, reduce_acceleration=0.0) + + +def make_toggles(**overrides): + defaults = { + "acceleration_profile": ACCELERATION_PROFILES["STANDARD"], + "deceleration_profile": DECELERATION_PROFILES["ECO"], + "custom_accel_profile": False, + "custom_accel_profile_values": [], + "ev_tuning": True, + "truck_tuning": False, + "human_acceleration": False, + "map_acceleration": False, + "map_deceleration": False, + "set_speed_offset": 0, + "speed_limit_controller": True, + } + defaults.update(overrides) + return SimpleNamespace(**defaults) + + +def make_lead(status=False, d_rel=150.0, v_lead=0.0, a_lead_k=0.0): + return SimpleNamespace(status=status, dRel=d_rel, vLead=v_lead, aLeadK=a_lead_k) + + +def make_sm(*, set_speed_kph=100.0, v_target=0.0, slc_target=0.0, lead_one=None, lead_two=None, + standstill=False, force_decel=False, red_light=False, forcing_stop=False, + disable_throttle=False, eco_gear=False, sport_gear=False, force_coast=False, + traffic_mode=False): + return { + "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill), + "controlsState": SimpleNamespace(forceDecel=force_decel), + "radarState": SimpleNamespace( + leadOne=lead_one or make_lead(), + leadTwo=lead_two or make_lead(), + ), + "starpilotCarState": SimpleNamespace( + ecoGear=eco_gear, + sportGear=sport_gear, + forceCoast=force_coast, + trafficModeEnabled=traffic_mode, + ), + "starpilotPlan": SimpleNamespace( + vCruise=v_target, + slcSpeedLimit=slc_target, + redLight=red_light, + forcingStop=forcing_stop, + disableThrottle=disable_throttle, + ), + } + + +def test_slc_coast_window_prefers_coast_for_small_overspeed(): + accel = StarPilotAcceleration(FakePlanner()) + target = 56.0 * CV.MPH_TO_MS + sm = make_sm(set_speed_kph=100.0, v_target=target, slc_target=target) + + accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"])) + + assert accel.min_accel == pytest.approx(-0.02, abs=1e-3) + + +def test_slc_coast_window_scales_by_profile_strength(): + v_ego = 65.0 * CV.MPH_TO_MS + v_target = 60.0 * CV.MPH_TO_MS + + eco = get_slc_shaped_min_accel(v_ego, v_target, DECELERATION_PROFILES["ECO"], A_CRUISE_MIN_ECO) + standard = get_slc_shaped_min_accel(v_ego, v_target, DECELERATION_PROFILES["STANDARD"], A_CRUISE_MIN) + sport = get_slc_shaped_min_accel(v_ego, v_target, DECELERATION_PROFILES["SPORT"], A_CRUISE_MIN * 2) + + assert eco > standard > sport + + +def test_slc_coast_window_disabled_for_relevant_lead(): + accel = StarPilotAcceleration(FakePlanner()) + target = 56.0 * CV.MPH_TO_MS + sm = make_sm( + set_speed_kph=100.0, + v_target=target, + slc_target=target, + lead_one=make_lead(status=True, d_rel=20.0, v_lead=45.0 * CV.MPH_TO_MS, a_lead_k=0.0), + ) + + accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"])) + + assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO) + + +def test_slc_coast_window_disabled_when_target_drop_is_not_slc(): + accel = StarPilotAcceleration(FakePlanner()) + slc_target = 60.0 * CV.MPH_TO_MS + sm = make_sm( + set_speed_kph=100.0, + v_target=55.0 * CV.MPH_TO_MS, + slc_target=slc_target, + ) + + accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"])) + + assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO) diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index ec21db78f..c56bda3e8 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -45,6 +45,9 @@ class ConditionalExperimentalMode: STOP_LIGHT_OFF_MARGIN = 4.0 STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0 STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0 + STOP_APPROACH_LATCH_TIME = 1.0 + STOP_APPROACH_MAX_LEAD_SPEED = 4.5 + STOP_APPROACH_MIN_MODEL_PROB = 0.9 # ===== END TUNING PARAMETERS ===== @@ -80,6 +83,7 @@ class ConditionalExperimentalMode: self.experimental_mode = False self.stop_light_detected = False self.stop_light_model_detected = False + self.stop_approach_hold_until = 0.0 self.prev_experimental_mode = False # For hysteresis self.mode_hold_until = 0.0 self.mode_false_since = 0.0 @@ -170,6 +174,8 @@ class ConditionalExperimentalMode: self.slow_lead_detected = 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 @@ -237,12 +243,25 @@ class ConditionalExperimentalMode: 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)) 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 + ) + if (self.stop_light_detected or self.stop_light_model_detected) and vision_stop_approach: + self.stop_approach_hold_until = now + self.STOP_APPROACH_LATCH_TIME + stop_approach_latched = now < self.stop_approach_hold_until and vision_stop_approach handoff_to_stopped_lead = ( - self.stop_light_detected and lead_relevant and not self.starpilot_planner.tracking_lead and - lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED + ( + (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 @@ -258,3 +277,4 @@ class ConditionalExperimentalMode: self.stop_light_detected = False self.stop_light_model_detected = False self.lead_clear_filter.x = 0 + self.stop_approach_hold_until = 0.0 diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index 4ceeda07e..57f57953c 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -1,7 +1,9 @@ #!/usr/bin/env python3 import numpy as np +from openpilot.common.constants import CV from openpilot.common.params import Params +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel from openpilot.starpilot.common.accel_profile import ( @@ -11,6 +13,7 @@ from openpilot.starpilot.common.accel_profile import ( get_accel_profile_curve_values, get_max_allowed_accel as get_profile_max_allowed_accel, interpolate_accel_profile, + normalize_deceleration_profile, ) from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT @@ -53,6 +56,24 @@ def akima_interp(x, xp, fp): A_CRUISE_MIN_ECO = A_CRUISE_MIN / 2 A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2 +SLC_COAST_WINDOW_BP = [0.0, 10.0, 20.0, 35.0] +SLC_COAST_WINDOW_BASE = [0.20, 0.40, 0.65, 1.10] +SLC_EXCESS_SCALE_BP = [0.0, 10.0, 20.0, 35.0] +SLC_EXCESS_SCALE_V = [0.8, 1.8, 3.5, 5.5] +SLC_COAST_WINDOW_MULTIPLIER = { + DECELERATION_PROFILES["ECO"]: 1.20, + DECELERATION_PROFILES["STANDARD"]: 1.00, + DECELERATION_PROFILES["SPORT"]: 0.75, +} +SLC_COAST_FLOOR = { + DECELERATION_PROFILES["ECO"]: -0.02, + DECELERATION_PROFILES["STANDARD"]: -0.03, + DECELERATION_PROFILES["SPORT"]: -0.04, +} +SLC_COAST_MIN_SPEED = 4.0 +SLC_TARGET_EPS = 0.15 +RELEVANT_LEAD_MIN_CLOSING_SPEED = 0.5 +RELEVANT_LEAD_MIN_BRAKE = -0.4 def get_max_accel_eco(v_ego, ev_tuning=True, truck_tuning=False): return interpolate_accel_profile(v_ego, get_accel_profile_curve_values(ACCELERATION_PROFILES["ECO"], ev_tuning, truck_tuning)) @@ -78,6 +99,41 @@ def get_max_accel_ramp_off(max_accel, v_cruise, v_ego): def get_max_allowed_accel(v_ego, ev_tuning=True, truck_tuning=False): return float(get_profile_max_allowed_accel(v_ego, ev_tuning, truck_tuning)) +def get_profile_min_accel_floor(deceleration_profile): + if deceleration_profile == DECELERATION_PROFILES["ECO"]: + return A_CRUISE_MIN_ECO + if deceleration_profile == DECELERATION_PROFILES["SPORT"]: + return A_CRUISE_MIN_SPORT + return A_CRUISE_MIN + +def lead_is_braking_relevant(lead, v_ego): + if lead is None or not getattr(lead, "status", False): + return False + + closing_speed = float(v_ego - getattr(lead, "vLead", 0.0)) + if closing_speed > RELEVANT_LEAD_MIN_CLOSING_SPEED: + return True + + if float(getattr(lead, "aLeadK", 0.0)) < RELEVANT_LEAD_MIN_BRAKE: + return True + + return float(getattr(lead, "dRel", 1e6)) < max(18.0, 2.0 * float(v_ego)) + +def get_slc_shaped_min_accel(v_ego, v_target, deceleration_profile, full_brake_floor): + profile = DECELERATION_PROFILES["STANDARD"] if deceleration_profile is None else deceleration_profile + coast_floor = SLC_COAST_FLOOR.get(profile, SLC_COAST_FLOOR[DECELERATION_PROFILES["STANDARD"]]) + coast_window = float(akima_interp(v_ego, SLC_COAST_WINDOW_BP, SLC_COAST_WINDOW_BASE)) + coast_window *= SLC_COAST_WINDOW_MULTIPLIER.get(profile, 1.0) + excess_scale = float(akima_interp(v_ego, SLC_EXCESS_SCALE_BP, SLC_EXCESS_SCALE_V)) + excess_scale = max(excess_scale, coast_window + 0.1) + + excess = max(0.0, float(v_ego) - float(v_target)) + if excess <= coast_window: + return coast_floor + + t = float(np.clip((excess - coast_window) / (excess_scale - coast_window), 0.0, 1.0)) ** 2 + return coast_floor + t * (full_brake_floor - coast_floor) + class StarPilotAcceleration: def __init__(self, StarPilotPlanner): self.starpilot_planner = StarPilotPlanner @@ -95,6 +151,9 @@ class StarPilotAcceleration: truck_tuning = getattr(starpilot_toggles, "truck_tuning", False) custom_accel_profile = getattr(starpilot_toggles, "custom_accel_profile", False) custom_accel_profile_values = getattr(starpilot_toggles, "custom_accel_profile_values", []) + deceleration_profile = normalize_deceleration_profile( + getattr(starpilot_toggles, "deceleration_profile", DECELERATION_PROFILES["STANDARD"]) + ) if custom_accel_profile: self.max_accel = get_max_accel_custom(v_ego, custom_accel_profile_values, starpilot_toggles.acceleration_profile, ev_tuning, truck_tuning) @@ -130,12 +189,31 @@ class StarPilotAcceleration: else: self.min_accel = A_CRUISE_MIN_SPORT else: - if starpilot_toggles.deceleration_profile == DECELERATION_PROFILES["ECO"]: - self.min_accel = A_CRUISE_MIN_ECO - elif starpilot_toggles.deceleration_profile == DECELERATION_PROFILES["SPORT"]: - self.min_accel = A_CRUISE_MIN_SPORT - else: - self.min_accel = A_CRUISE_MIN + self.min_accel = get_profile_min_accel_floor(deceleration_profile) + + raw_v_cruise_kph = 0.0 if sm["carState"].vCruise == V_CRUISE_UNSET else min(sm["carState"].vCruise, V_CRUISE_MAX) + if 0 < raw_v_cruise_kph < V_CRUISE_UNSET and getattr(starpilot_toggles, "set_speed_offset", 0) > 0: + raw_v_cruise_kph += starpilot_toggles.set_speed_offset + raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS + + v_target = float(getattr(sm["starpilotPlan"], "vCruise", raw_v_cruise)) + slc_target = float(getattr(sm["starpilotPlan"], "slcSpeedLimit", 0.0)) + slc_limited = slc_target > 0.0 and abs(v_target - slc_target) <= SLC_TARGET_EPS and v_target < raw_v_cruise - SLC_TARGET_EPS + has_relevant_lead = any(lead_is_braking_relevant(lead, v_ego) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo)) + stop_context = ( + sm["carState"].standstill or + getattr(sm["controlsState"], "forceDecel", False) or + getattr(sm["starpilotPlan"], "redLight", False) or + getattr(sm["starpilotPlan"], "forcingStop", False) or + getattr(sm["starpilotPlan"], "disableThrottle", False) + ) + if (getattr(starpilot_toggles, "speed_limit_controller", False) and + v_ego > SLC_COAST_MIN_SPEED and + v_ego > v_target + 0.05 and + slc_limited and + not has_relevant_lead and + not stop_context): + self.min_accel = get_slc_shaped_min_accel(v_ego, v_target, deceleration_profile, self.min_accel) # Sync AccelerationProfile and DecelerationProfile params so the UI reflects the active drive mode # Eco → Eco, Normal → Standard, Sport → Sport+