mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 19:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

EV9

Browse Source
This commit is contained in:
firestar5683
2026-09-27 16:14:22 -05:00
parent 2dd44a6368
commit 7b4643208e
3 changed files with 71 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+2 -7
View File
@@ -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)
)
selfdrive/controls/lib/longitudinal_vehicle_tunes.py
+46
View File
@@ -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 (
selfdrive/controls/tests/test_longitudinal_planner.py
+23
View File
@@ -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(