diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index df366af2d..cc8324ce4 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -3,7 +3,11 @@ import datetime import pytest from openpilot.common.constants import CV -from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise, get_active_slc_control_target +from openpilot.starpilot.controls.lib.starpilot_vcruise import ( + StarPilotVCruise, + get_active_slc_control_target, + get_slc_lead_drop_relaxed_target, +) from types import SimpleNamespace @@ -108,6 +112,82 @@ def test_active_slc_control_target_applies_offset_and_cluster_diff(): assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4) +def test_slc_lead_drop_relaxed_target_softens_map_stepdown_for_harmless_lead(): + raw_target = 55.0 * CV.MPH_TO_MS + previous_target = 65.0 * CV.MPH_TO_MS + v_ego = 65.0 * CV.MPH_TO_MS + lead = SimpleNamespace(status=True, dRel=46.0, vLead=71.0 * CV.MPH_TO_MS, aLeadK=0.08) + + relaxed = get_slc_lead_drop_relaxed_target( + raw_target, + previous_target, + v_ego, + tracking_lead=True, + lead=lead, + override_active=False, + source="Map Data", + ) + + assert raw_target < relaxed < previous_target + + +def test_slc_lead_drop_relaxed_target_bails_out_for_override(): + raw_target = 55.0 * CV.MPH_TO_MS + previous_target = 65.0 * CV.MPH_TO_MS + v_ego = 65.0 * CV.MPH_TO_MS + lead = SimpleNamespace(status=True, dRel=46.0, vLead=71.0 * CV.MPH_TO_MS, aLeadK=0.08) + + relaxed = get_slc_lead_drop_relaxed_target( + raw_target, + previous_target, + v_ego, + tracking_lead=True, + lead=lead, + override_active=True, + source="Map Data", + ) + + assert relaxed == pytest.approx(raw_target) + + +def test_slc_lead_drop_relaxed_target_bails_out_for_threatening_lead(): + raw_target = 55.0 * CV.MPH_TO_MS + previous_target = 65.0 * CV.MPH_TO_MS + v_ego = 65.0 * CV.MPH_TO_MS + lead = SimpleNamespace(status=True, dRel=20.0, vLead=52.0 * CV.MPH_TO_MS, aLeadK=-0.5) + + relaxed = get_slc_lead_drop_relaxed_target( + raw_target, + previous_target, + v_ego, + tracking_lead=True, + lead=lead, + override_active=False, + source="Map Data", + ) + + assert relaxed == pytest.approx(raw_target) + + +def test_slc_lead_drop_relaxed_target_bails_out_without_tracking_lead(): + raw_target = 55.0 * CV.MPH_TO_MS + previous_target = 65.0 * CV.MPH_TO_MS + v_ego = 65.0 * CV.MPH_TO_MS + lead = SimpleNamespace(status=True, dRel=46.0, vLead=71.0 * CV.MPH_TO_MS, aLeadK=0.08) + + relaxed = get_slc_lead_drop_relaxed_target( + raw_target, + previous_target, + v_ego, + tracking_lead=False, + lead=lead, + override_active=False, + source="Map Data", + ) + + assert relaxed == pytest.approx(raw_target) + + def test_force_stop_clears_at_standstill_once_scene_opens(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index a344d29d1..b892d62cb 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -13,6 +13,13 @@ CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS OVERRIDE_FORCE_STOP_TIMER = 10 STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75 STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0 +SLC_LEAD_DROP_RELAXATION_MIN_SPEED = 20.0 * CV.MPH_TO_MS +SLC_LEAD_DROP_RELAXATION_MIN_DISTANCE = 30.0 +SLC_LEAD_DROP_RELAXATION_MIN_HEADWAY = 1.2 +SLC_LEAD_DROP_RELAXATION_MAX_CLOSING_SPEED = 0.35 +SLC_LEAD_DROP_RELAXATION_MAX_LEAD_BRAKE = 0.25 +SLC_LEAD_DROP_RELAXATION_OVERSPEED_BP = [0.0, 5.0 * CV.MPH_TO_MS, 10.0 * CV.MPH_TO_MS, 15.0 * CV.MPH_TO_MS] +SLC_LEAD_DROP_RELAXATION_DECEL_V = [0.7, 0.9, 1.15, 1.35] NAV_TURN_COMFORT_DECEL = 1.25 NAV_TURN_DISTANCE_BUFFER = 8.0 NAV_TURN_MIN_TARGET_DELTA = 0.25 @@ -56,6 +63,59 @@ def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_t return max(0.0, base_target - float(v_ego_diff)) +def _interp_linear(x, xp, fp): + if x <= xp[0]: + return fp[0] + if x >= xp[-1]: + return fp[-1] + + for i in range(1, len(xp)): + if x <= xp[i]: + span = xp[i] - xp[i - 1] + if span <= 0.0: + return fp[i] + ratio = (x - xp[i - 1]) / span + return fp[i - 1] + ratio * (fp[i] - fp[i - 1]) + + return fp[-1] + + +def get_slc_lead_drop_relaxed_target(raw_target, previous_target, v_ego, tracking_lead, lead, override_active, source): + if ( + raw_target <= 0.0 or + previous_target <= 0.0 or + override_active or + source == "None" or + not tracking_lead or + lead is None or + not getattr(lead, "status", False) + ): + return raw_target + + if ( + raw_target >= previous_target - 1e-3 or + v_ego < SLC_LEAD_DROP_RELAXATION_MIN_SPEED or + raw_target >= v_ego - 0.05 + ): + return raw_target + + d_rel = float(getattr(lead, "dRel", 0.0)) + if d_rel < max(SLC_LEAD_DROP_RELAXATION_MIN_DISTANCE, float(v_ego) * SLC_LEAD_DROP_RELAXATION_MIN_HEADWAY): + return raw_target + + v_lead = float(getattr(lead, "vLead", 0.0)) + if v_lead < float(v_ego) - SLC_LEAD_DROP_RELAXATION_MAX_CLOSING_SPEED: + return raw_target + + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > SLC_LEAD_DROP_RELAXATION_MAX_LEAD_BRAKE: + return raw_target + + overspeed = max(0.0, float(v_ego) - float(raw_target)) + comfort_decel = _interp_linear(overspeed, SLC_LEAD_DROP_RELAXATION_OVERSPEED_BP, SLC_LEAD_DROP_RELAXATION_DECEL_V) + return max(float(raw_target), float(previous_target) - comfort_decel * DT_MDL) + + class StarPilotVCruise: def __init__(self, StarPilotPlanner): self.starpilot_planner = StarPilotPlanner @@ -82,6 +142,7 @@ class StarPilotVCruise: self.nav_turn_target = 0.0 self._nav_instruction_state_raw = None self._nav_instruction_state = {} + self._applied_slc_control_target = 0.0 def _update_nav_instruction_state(self): raw = self.starpilot_planner.params_memory.get("NavInstructionState") or {} @@ -192,6 +253,9 @@ class StarPilotVCruise: # ===== Main update ===== def update(self, controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles): + if not controls_enabled or not getattr(starpilot_toggles, "speed_limit_controller", False): + self._applied_slc_control_target = 0.0 + long_control_active = sm["carControl"].longActive turn_scene_active = bool( v_ego <= FORCE_STOP_TURN_VETO_MAX_SPEED and @@ -415,6 +479,16 @@ class StarPilotVCruise: self.slc.overridden_speed, v_ego_diff, ) + slc_control_target = get_slc_lead_drop_relaxed_target( + slc_control_target, + self._applied_slc_control_target, + v_ego, + bool(getattr(self.starpilot_planner, "tracking_lead", False)), + getattr(self.starpilot_planner, "lead_one", None), + self.slc.overridden_speed > 0.0, + getattr(self.slc, "source", "None"), + ) + self._applied_slc_control_target = slc_control_target if slc_control_target > 0.0 else 0.0 if slc_control_target >= CSC_MIN_SPEED: targets.append(slc_control_target) if self.nav_turn_target > 0.0: