From 7b4643208e1af2beaa478dbafd4376adf11a2216 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 27 Sep 2026 16:14:22 -0500 Subject: [PATCH] EV9 --- .../controls/lib/longitudinal_planner.py | 9 +--- .../lib/longitudinal_vehicle_tunes.py | 46 +++++++++++++++++++ .../tests/test_longitudinal_planner.py | 23 ++++++++++ 3 files changed, 71 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c96528ff00..88c696a4a3 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -38,8 +38,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( is_toyota_rav4_tss2_radar_follow_lead, get_toyota_sienna_post_departure_restop_cap, get_untracked_slow_lead_decel_scale, - get_toyota_prius_stopped_lead_obstacle_bias, - get_honda_crv_5g_stopped_lead_obstacle_bias, + get_stopped_lead_obstacle_bias, get_honda_crv_5g_low_speed_stopped_lead_cap, allow_honda_crv_5g_vision_gap_settle, get_honda_crv_5g_early_radar_follow_cap, @@ -2363,17 +2362,13 @@ class LongitudinalPlanner: stopped_lead_obstacle_bias = (0.0, 0.0) if ( - self.mode == 'acc' and not bool(getattr(sm['modelV2'].action, 'shouldStop', False)) and not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and not bool(getattr(sm['carState'], 'standstill', False)) ): stopped_lead_obstacle_bias = tuple( - max( - get_toyota_prius_stopped_lead_obstacle_bias(self.CP, lead, scene_v_ego), - get_honda_crv_5g_stopped_lead_obstacle_bias(self.CP, lead, scene_v_ego), - ) + get_stopped_lead_obstacle_bias(self.CP, lead, scene_v_ego, self.mode) for lead in (self.lead_one, self.lead_two) ) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 7e36ff0b3b..45c014e450 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -101,6 +101,13 @@ HONDA_CRV_5G_STOPPED_LEAD_MIN_CLOSING_SPEED = 0.15 HONDA_CRV_5G_STOPPED_LEAD_MAX_DISTANCE = 80.0 HONDA_CRV_5G_STOPPED_LEAD_RAMP_DISTANCE = 10.0 HONDA_CRV_5G_STOPPED_LEAD_MAX_LATERAL_OFFSET = 1.75 +KIA_EV9_STOPPED_LEAD_OBSTACLE_BIAS_M = 1.5 +KIA_EV9_STOPPED_LEAD_MAX_EGO_SPEED = 4.5 +KIA_EV9_STOPPED_LEAD_MAX_SPEED = 0.5 +KIA_EV9_STOPPED_LEAD_MIN_CLOSING_SPEED = 0.15 +KIA_EV9_STOPPED_LEAD_MAX_DISTANCE = 30.0 +KIA_EV9_STOPPED_LEAD_RAMP_DISTANCE = 8.0 +KIA_EV9_STOPPED_LEAD_MAX_LATERAL_OFFSET = 1.75 HONDA_CRV_5G_LOW_SPEED_STOP_MAX_EGO_SPEED = 4.5 HONDA_CRV_5G_LOW_SPEED_STOP_MAX_LEAD_SPEED = 0.5 HONDA_CRV_5G_LOW_SPEED_STOP_MIN_MODEL_PROB = 0.99 @@ -241,6 +248,45 @@ def get_honda_crv_5g_stopped_lead_obstacle_bias(CP, lead, v_ego): return float(min(bias, max(distance - 0.5, 0.0))) +def get_kia_ev9_stopped_lead_obstacle_bias(CP, lead, v_ego): + if ( + getattr(CP, "brand", "") != "hyundai" or + str(getattr(CP, "carFingerprint", "")) != "KIA_EV9" or + lead is None or not bool(getattr(lead, "status", False)) or + float(v_ego) <= 0.0 or float(v_ego) > KIA_EV9_STOPPED_LEAD_MAX_EGO_SPEED or + float(getattr(lead, "vLead", 0.0)) > KIA_EV9_STOPPED_LEAD_MAX_SPEED or + abs(float(getattr(lead, "yRel", 0.0))) > KIA_EV9_STOPPED_LEAD_MAX_LATERAL_OFFSET + ): + return 0.0 + + distance = float(getattr(lead, "dRel", float("inf"))) + closing_speed = float(v_ego) - float(getattr(lead, "vLead", 0.0)) + if ( + distance <= 0.0 or distance > KIA_EV9_STOPPED_LEAD_MAX_DISTANCE or + closing_speed < KIA_EV9_STOPPED_LEAD_MIN_CLOSING_SPEED + ): + return 0.0 + + strength = np.clip( + (KIA_EV9_STOPPED_LEAD_MAX_DISTANCE - distance) / + (KIA_EV9_STOPPED_LEAD_MAX_DISTANCE - KIA_EV9_STOPPED_LEAD_RAMP_DISTANCE), + 0.0, 1.0, + ) + bias = KIA_EV9_STOPPED_LEAD_OBSTACLE_BIAS_M * strength + return float(min(bias, max(distance - 0.5, 0.0))) + + +def get_stopped_lead_obstacle_bias(CP, lead, v_ego, mode): + """Dispatch vehicle-specific stopped-lead spacing tweaks for the active mode.""" + mode_bias = 0.0 + if mode == "acc": + mode_bias = max( + get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego), + get_honda_crv_5g_stopped_lead_obstacle_bias(CP, lead, v_ego), + ) + return max(mode_bias, get_kia_ev9_stopped_lead_obstacle_bias(CP, lead, v_ego)) + + def get_honda_crv_5g_low_speed_stopped_lead_cap(CP, lead, v_ego, accel_min): """Bleed a CR-V crawl into the normal standstill gap without a hard jab.""" if ( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 7e364964ba..d6942bc880 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -11,6 +11,8 @@ from openpilot.common.constants import CV from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR from opendbc.car.gm.values import CAR as GM_CAR, GMFlags +from opendbc.car.hyundai.interface import CarInterface as HyundaiCarInterface +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR from opendbc.car.ford.interface import CarInterface as FordCarInterface from opendbc.car.ford.values import CAR as FORD_CAR from opendbc.car.toyota.interface import CarInterface as ToyotaCarInterface @@ -39,6 +41,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_honda_accord_lead_departure_tune, get_honda_accord_stop_go_accel_cap, get_honda_crv_5g_stopped_lead_obstacle_bias, + get_kia_ev9_stopped_lead_obstacle_bias, + get_stopped_lead_obstacle_bias, get_honda_crv_5g_low_speed_stopped_lead_cap, allow_honda_crv_5g_vision_gap_settle, is_honda_crv_5g_early_radar_follow_lead, @@ -162,6 +166,25 @@ def test_honda_crv_5g_stopped_lead_tune_is_vehicle_specific(): assert get_standstill_gap_settle_max_extra_gap(crv) > get_standstill_gap_settle_max_extra_gap(civic) +def test_kia_ev9_stopped_lead_buffer_is_low_speed_and_vehicle_specific(): + ev9 = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_EV9) + other = HyundaiCarInterface.get_non_essential_params(HYUNDAI_CAR.KIA_EV6) + prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS) + stopped_lead = make_lead(status=True, d_rel=6.8, v_lead=0.1, model_prob=0.99) + + bias = get_kia_ev9_stopped_lead_obstacle_bias(ev9, stopped_lead, v_ego=3.0) + assert bias == pytest.approx(1.5) + assert get_kia_ev9_stopped_lead_obstacle_bias(other, stopped_lead, v_ego=3.0) == pytest.approx(0.0) + assert get_kia_ev9_stopped_lead_obstacle_bias(ev9, stopped_lead, v_ego=8.0) == pytest.approx(0.0) + assert get_stopped_lead_obstacle_bias(ev9, stopped_lead, v_ego=3.0, mode="blended") == pytest.approx(1.5) + assert get_stopped_lead_obstacle_bias(other, stopped_lead, v_ego=3.0, mode="blended") == pytest.approx(0.0) + assert get_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=3.0, mode="blended") == pytest.approx(0.0) + assert get_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=3.0, mode="acc") > 0.0 + assert get_kia_ev9_stopped_lead_obstacle_bias( + ev9, make_lead(status=True, d_rel=6.8, v_lead=2.0), v_ego=3.0, + ) == pytest.approx(0.0) + + def test_honda_crv_5g_early_radar_follow_admits_high_closing_centered_lead(): crv = CarInterface.get_non_essential_params(CAR.HONDA_CRV_5G) lead = make_lead(