From ef57854b813c92d3f36a4714e78d7916c5f5abbe Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 28 Apr 2026 18:53:29 -0500 Subject: [PATCH] Doms Plan v5 --- .../controls/lib/longitudinal_planner.py | 89 +++++++++++++++++-- .../tests/test_longitudinal_planner.py | 43 ++++++++- .../tests/test_starpilot_acceleration.py | 42 ++++++++- .../controls/tests/test_starpilot_vcruise.py | 30 +++++++ .../settings/starpilot/longitudinal.py | 2 +- .../controls/lib/starpilot_acceleration.py | 19 +++- starpilot/controls/lib/starpilot_vcruise.py | 23 ++++- 7 files changed, 229 insertions(+), 19 deletions(-) create mode 100644 selfdrive/controls/tests/test_starpilot_vcruise.py diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 62b8aa5be..594c5335e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -32,13 +32,16 @@ RAW_LEAD_SAFETY_TTC = 7.0 RAW_LEAD_SAFETY_DISTANCE = 40.0 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0 -VISION_LEAD_APPROACH_TRIGGER_TIME = 4.0 +VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5 VISION_LEAD_APPROACH_FULL_TIME = 1.0 VISION_LEAD_APPROACH_TIGHT_BUFFER = 2.0 -VISION_LEAD_APPROACH_MAX_DECEL = 0.45 -VISION_LEAD_APPROACH_MIN_DECEL = 0.12 +VISION_LEAD_APPROACH_MAX_DECEL = 0.65 +VISION_LEAD_APPROACH_MIN_DECEL = 0.15 VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85 VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 +VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.15 +VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0 +VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20 VISION_SLOW_LEAD_MAX_SPEED = 5.0 VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5 VISION_SLOW_LEAD_TRIGGER_TTC = 4.5 @@ -56,7 +59,22 @@ LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE = 0.2 LEAD_APPROACH_TFOLLOW_WINDOW_MIN = 6.0 LEAD_APPROACH_TFOLLOW_WINDOW_GAIN = 0.35 LEAD_APPROACH_TFOLLOW_RATE_UP = 1.0 -LEAD_APPROACH_TFOLLOW_RATE_DOWN = 0.18 +LEAD_APPROACH_TFOLLOW_RATE_DOWN = 0.60 +VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA = 0.24 +VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED = 20.0 +VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN = 8.0 +VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN = 0.35 +VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 5.5 +VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 1.75 +VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9 +VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35 +VISION_LOW_SPEED_STOP_BUFFER_BASE = 2.8 +VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80 +VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25 +VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.75 +VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.6 +VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 0.9 +VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25 # Uncertainty-based filter disable thresholds UNCERT_SLOPE_TRIG = 0.12 # per second @@ -207,6 +225,7 @@ class LongitudinalPlanner: self._uncert_last = 0.0 self._uncert_last_t = None self.effective_t_follow = None + self.vision_low_speed_stop_hold_until = 0.0 @property def mlsim(self): @@ -305,7 +324,17 @@ class LongitudinalPlanner: (VISION_LEAD_APPROACH_TRIGGER_TIME - VISION_LEAD_APPROACH_FULL_TIME), 0.0, 1.0)) prob_factor = float(np.clip((lead_prob - VISION_LEAD_APPROACH_MIN_MODEL_PROB) / (VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_LEAD_APPROACH_MIN_MODEL_PROB), 0.0, 1.0)) - approach_decel = VISION_LEAD_APPROACH_MAX_DECEL * time_factor * prob_factor + closing_factor = float(np.clip(projected_closing_speed / (VISION_LEAD_APPROACH_MIN_CLOSING_SPEED + 2.5), 0.0, 1.0)) + tight_follow_deficit = max(tight_follow_gap - float(lead.dRel), 0.0) + tight_follow_buffer = max(VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN, + VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN * float(v_ego) + 1.0) + deficit_factor = float(np.clip(tight_follow_deficit / tight_follow_buffer, 0.0, 1.0)) + + approach_decel = VISION_LEAD_APPROACH_MAX_DECEL * time_factor * (0.45 + 0.55 * prob_factor) + approach_decel *= 0.6 + 0.4 * closing_factor + deficit_decel = VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL * deficit_factor * prob_factor + deficit_decel *= 0.5 + 0.5 * closing_factor + approach_decel = max(approach_decel, deficit_decel) if approach_decel < VISION_LEAD_APPROACH_MIN_DECEL: return None @@ -371,6 +400,17 @@ class LongitudinalPlanner: brake_factor = float(np.clip(lead_brake / LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE, 0.0, 1.0)) target_delta = LEAD_APPROACH_TFOLLOW_MAX_DELTA * np.clip( 0.55 * time_factor + 0.25 * closing_factor + 0.20 * brake_factor, 0.0, 1.0) + if not bool(getattr(lead, "radar", False)): + gap_deficit = max(desired_gap - float(lead.dRel), 0.0) + gap_buffer = max(VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN, + VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN * float(v_ego)) + gap_factor = float(np.clip(gap_deficit / gap_buffer, 0.0, 1.0)) + slow_lead_factor = float(np.clip((VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED - float(lead.vLead)) / + VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED, 0.0, 1.0)) + vision_extra = VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA * np.clip( + 0.40 * time_factor + 0.30 * gap_factor + 0.20 * slow_lead_factor + 0.10 * closing_factor, + 0.0, 1.0) + target_delta += vision_extra target_t_follow = base_t_follow + float(target_delta) if self.effective_t_follow is None: @@ -382,6 +422,38 @@ class LongitudinalPlanner: self.effective_t_follow = max(base_t_follow, self.effective_t_follow) return self.effective_t_follow + def get_vision_low_speed_stop_buffer_cap(self, lead, v_ego, accel_min): + if lead is None or not lead.status or bool(getattr(lead, "radar", False)): + return None, False + + lead_prob = float(getattr(lead, "modelProb", 0.0)) + if lead_prob < VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB: + return None, False + + lead_speed = max(float(lead.vLead), 0.0) + closing_speed = max(0.0, v_ego - lead_speed) + valid_context = ( + v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and + lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED and + closing_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED + ) + + now_t = time.monotonic() + entry_buffer = max(3.2, VISION_LOW_SPEED_STOP_BUFFER_BASE + + VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN * float(v_ego) + + VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN * lead_speed) + release_buffer = entry_buffer + VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN + if valid_context and float(lead.dRel) <= entry_buffer: + self.vision_low_speed_stop_hold_until = now_t + VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME + + latched = now_t < self.vision_low_speed_stop_hold_until + active = valid_context and (float(lead.dRel) <= entry_buffer or (latched and float(lead.dRel) <= release_buffer)) + if not active: + return None, False + + min_stop_brake = VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE + VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN * float(v_ego) + return max(accel_min, -min_stop_brake), True + @staticmethod def raw_close_lead_needs_control(lead, v_ego): if lead is None or not lead.status: @@ -667,6 +739,7 @@ class LongitudinalPlanner: output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] close_lead_caps = [] + vision_low_speed_stop_active = False if lead_control_active: for lead in (self.lead_one, self.lead_two): cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min) @@ -678,6 +751,10 @@ class LongitudinalPlanner: 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) + low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, output_accel_min) + if low_speed_stop_cap is not None: + close_lead_caps.append(low_speed_stop_cap) + vision_low_speed_stop_active |= low_speed_stop_active if close_lead_caps: close_lead_brake_cap = min(close_lead_caps) self.a_desired = min(self.a_desired, close_lead_brake_cap) @@ -699,7 +776,7 @@ class LongitudinalPlanner: output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) self.output_a_target = output_a_target - self.output_should_stop = bool(output_should_stop) + self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active) def publish(self, sm, pm): plan_send = messaging.new_message('longitudinalPlan') diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3d20204ac..bcdf09917 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -213,7 +213,19 @@ def test_vision_lead_approach_cap_brakes_before_hard_cap(): assert hard_cap == pytest.approx(-0.212, abs=1e-2) assert approach_cap is not None assert approach_cap < hard_cap - assert approach_cap > -0.6 + assert approach_cap > -1.2 + + +def test_vision_lead_approach_cap_brakes_harder_when_inside_tight_gap(): + v_ego = 26.18 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=39.72, v_lead=22.46, a_lead=-0.15, radar=False, model_prob=0.97) + + approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.49) + + assert approach_cap is not None + assert approach_cap < -0.5 def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap(): @@ -267,8 +279,8 @@ def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version): planner.update(sm, make_toggles(model_version)) assert planner.effective_t_follow is not None - assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.05 - assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.2 + assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.15 + assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.45 @pytest.mark.parametrize("model_version", ["v11", "v12"]) @@ -354,10 +366,33 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers assert planner_approach.mode == "acc" assert planner_close.mode == "acc" - assert planner_approach.output_a_target < -0.3 + assert planner_approach.output_a_target < -0.5 assert planner_close.output_a_target < planner_approach.output_a_target - 0.25 +@pytest.mark.parametrize("model_version", ["v11", "v12"]) +def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version): + v_ego = 3.8 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.1, + min_accel=-3.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=5.75, v_lead=0.58, a_lead=-0.1, radar=False, model_prob=0.99), + ) + sm["starpilotPlan"].vCruise = v_ego + 4.0 + + planner.update(sm, make_toggles(model_version)) + + assert planner.mode == "acc" + assert planner.output_should_stop + assert planner.output_a_target < -1.0 + + @pytest.mark.parametrize("model_version", ["v11", "v12"]) def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): far_v_ego = 29.26 diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py index 15b399f9a..93c6897bc 100644 --- a/selfdrive/controls/tests/test_starpilot_acceleration.py +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -13,10 +13,16 @@ from openpilot.starpilot.controls.lib.starpilot_acceleration import ( class FakePlanner: - def __init__(self, *, v_cruise=0.0, slc_target=0.0, red_light=False, forcing_stop=False, disable_throttle=False): + 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): self.v_cruise = v_cruise self.starpilot_weather = SimpleNamespace(weather_id=0, reduce_acceleration=0.0) - self.starpilot_vcruise = SimpleNamespace(slc_target=slc_target, forcing_stop=forcing_stop) + self.starpilot_vcruise = SimpleNamespace( + slc_target=slc_target, + slc_offset=slc_offset, + 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) @@ -32,6 +38,7 @@ def make_toggles(**overrides): "human_acceleration": False, "map_acceleration": False, "map_deceleration": False, + "set_speed_limit": True, "set_speed_offset": 0, "speed_limit_controller": True, } @@ -44,9 +51,9 @@ def make_lead(status=False, d_rel=150.0, v_lead=0.0, a_lead_k=0.0): def make_sm(*, set_speed_kph=100.0, lead_one=None, lead_two=None, standstill=False, force_decel=False, - eco_gear=False, sport_gear=False, force_coast=False, traffic_mode=False): + eco_gear=False, sport_gear=False, force_coast=False, traffic_mode=False, v_ego_cluster=0.0): return { - "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill), + "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill, vEgoCluster=v_ego_cluster), "controlsState": SimpleNamespace(forceDecel=force_decel), "radarState": SimpleNamespace( leadOne=lead_one or make_lead(), @@ -81,6 +88,33 @@ def test_slc_coast_window_does_not_require_starpilot_plan_message(): assert accel.min_accel == pytest.approx(-0.02, abs=1e-3) +def test_slc_coast_window_uses_effective_target_with_offset_and_cluster_diff(): + raw_target = 58.0 * CV.MPH_TO_MS + slc_target = 45.0 * CV.MPH_TO_MS + slc_offset = 3.0 * CV.MPH_TO_MS + v_ego = 48.0 * CV.MPH_TO_MS + v_ego_cluster = v_ego + 0.4 + accel = StarPilotAcceleration(FakePlanner(v_cruise=raw_target, slc_target=slc_target, slc_offset=slc_offset)) + sm = make_sm(set_speed_kph=100.0, v_ego_cluster=v_ego_cluster) + + accel.update(v_ego, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"])) + + assert accel.min_accel == pytest.approx(-0.02, abs=1e-3) + + +def test_slc_coast_window_disabled_when_set_speed_limit_is_off(): + raw_target = 58.0 * CV.MPH_TO_MS + slc_target = 45.0 * CV.MPH_TO_MS + slc_offset = 3.0 * CV.MPH_TO_MS + v_ego = 48.0 * CV.MPH_TO_MS + accel = StarPilotAcceleration(FakePlanner(v_cruise=raw_target, slc_target=slc_target, slc_offset=slc_offset)) + sm = make_sm(set_speed_kph=100.0, v_ego_cluster=v_ego + 0.4) + + accel.update(v_ego, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"], set_speed_limit=False)) + + assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO) + + 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 diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py new file mode 100644 index 000000000..82738c3f9 --- /dev/null +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -0,0 +1,30 @@ +import pytest + +from openpilot.common.constants import CV +from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target + + +def test_active_slc_control_target_requires_set_speed_limit(): + target = get_active_slc_control_target( + speed_limit_controller=True, + set_speed_limit=False, + slc_target=45.0 * CV.MPH_TO_MS, + slc_offset=3.0 * CV.MPH_TO_MS, + overridden_speed=0.0, + v_ego_diff=0.4, + ) + + assert target == 0.0 + + +def test_active_slc_control_target_applies_offset_and_cluster_diff(): + target = get_active_slc_control_target( + speed_limit_controller=True, + set_speed_limit=True, + slc_target=45.0 * CV.MPH_TO_MS, + slc_offset=3.0 * CV.MPH_TO_MS, + overridden_speed=0.0, + v_ego_diff=0.4, + ) + + assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4) diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 0cabd49ea..834f65571 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -945,7 +945,7 @@ class StarPilotSLCQOLLayout(StarPilotPanel): super().__init__() self.CATEGORIES = [ { - "title": tr_noop("Match Speed on Engage"), + "title": tr_noop("Auto Match Speed Limits"), "type": "toggle", "get_state": lambda: self._params.get_bool("SetSpeedLimit"), "set_state": lambda s: self._params.put_bool("SetSpeedLimit", s), diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index 240c79ed4..9f4a7266e 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -15,6 +15,7 @@ from openpilot.starpilot.common.accel_profile import ( interpolate_accel_profile, normalize_deceleration_profile, ) +from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT def cubic_interp(x, xp, fp): @@ -196,9 +197,23 @@ class StarPilotAcceleration: raw_v_cruise_kph += starpilot_toggles.set_speed_offset raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS + v_ego_cluster = getattr(sm["carState"], "vEgoCluster", v_ego) + if v_ego_cluster is None: + v_ego_cluster = v_ego + v_ego_cluster = max(v_ego_cluster, v_ego) + v_ego_diff = v_ego_cluster - v_ego + effective_slc_target = get_active_slc_control_target( + getattr(starpilot_toggles, "speed_limit_controller", False), + getattr(starpilot_toggles, "set_speed_limit", False), + getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), + getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), + getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), + v_ego_diff, + ) v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise) - slc_target = float(getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 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 + if effective_slc_target > 0.0: + v_target = min(v_target, effective_slc_target) + slc_limited = effective_slc_target > 0.0 and abs(v_target - effective_slc_target) <= SLC_TARGET_EPS and effective_slc_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 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 7f4746c92..134659bb6 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -27,6 +27,17 @@ OFFSET_FT_MIN = -20 OFFSET_FT_MAX = 20 +def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_target, slc_offset, overridden_speed, v_ego_diff): + if not speed_limit_controller or not set_speed_limit: + return 0.0 + + base_target = max(float(overridden_speed), float(slc_target) + float(slc_offset)) + if base_target <= 0.0: + return 0.0 + + return max(0.0, base_target - float(v_ego_diff)) + + class StarPilotVCruise: def __init__(self, StarPilotPlanner): self.starpilot_planner = StarPilotPlanner @@ -184,8 +195,16 @@ class StarPilotVCruise: self.tracked_model_length = self.starpilot_planner.model_length targets = [self.csc_target, v_cruise] - if starpilot_toggles.speed_limit_controller: - targets.append(max(self.slc.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff) + slc_control_target = get_active_slc_control_target( + starpilot_toggles.speed_limit_controller, + getattr(starpilot_toggles, "set_speed_limit", False), + self.slc_target, + self.slc_offset, + self.slc.overridden_speed, + v_ego_diff, + ) + if slc_control_target > 0.0: + targets.append(slc_control_target) v_cruise = min([target if target >= CSC_MIN_SPEED else v_cruise for target in targets]) return v_cruise