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

i6 cem

Browse Source
This commit is contained in:
firestar5683
2026-04-30 17:52:50 -05:00
parent 0041a79823
commit 8b12ed459f
5 changed files with 87 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+8 -2
View File
@@ -41,7 +41,9 @@ IONIQ_6_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5]
IONIQ_6_LAUNCH_HOLD_SPEED_BP = [0.0, 0.6, 1.25, 2.5]
IONIQ_6_LAUNCH_HOLD_SPEED_V = [0.75, 0.6, 0.4, 0.0]
IONIQ_6_STOP_HOLD_SPEED_BP = [0.0, 0.25, 0.6, 1.2]
IONIQ_6_STOP_HOLD_SPEED_V = [-0.18, -0.15, -0.08, 0.0]
IONIQ_6_STOP_HOLD_SPEED_V = [-0.12, -0.10, -0.05, 0.0]
IONIQ_6_STOP_RELEASE_JERK_BP = [0.0, 0.15, 0.5]
IONIQ_6_STOP_RELEASE_JERK_V = [3.6, 4.2, 4.8]
@dataclass
@@ -80,6 +82,8 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
long_control_state: LongCtrlState, long_active: bool) -> Ioniq6LongitudinalTuningState:
starting = long_control_state == LongCtrlState.starting
stopping = long_control_state == LongCtrlState.stopping
restart_from_stop = state.long_control_state_last in (LongCtrlState.stopping, LongCtrlState.starting) and \
long_control_state in (LongCtrlState.starting, LongCtrlState.pid) and accel_cmd > 0.0 and v_ego < 0.5
if not long_active or not stopping:
state.stopping = False
@@ -125,13 +129,15 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
if state.stopping:
state.desired_accel = float(np.interp(v_ego, IONIQ_6_STOP_HOLD_SPEED_BP, IONIQ_6_STOP_HOLD_SPEED_V))
state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, [0.0, 1.2], [0.25, 0.5])))
state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, [0.0, 1.2], [0.45, 0.65])))
else:
state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
if state.launch_active:
state.desired_accel = max(state.desired_accel, float(np.interp(v_ego, IONIQ_6_LAUNCH_HOLD_SPEED_BP, IONIQ_6_LAUNCH_HOLD_SPEED_V)))
state.jerk_upper = max(state.jerk_upper, float(np.interp(v_ego, [0.0, 2.5], [4.8, 3.2])))
state.jerk_lower = max(state.jerk_lower, 1.0)
if restart_from_stop:
state.jerk_upper = min(state.jerk_upper, float(np.interp(v_ego, IONIQ_6_STOP_RELEASE_JERK_BP, IONIQ_6_STOP_RELEASE_JERK_V)))
state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower)
state.accel_last = state.actual_accel
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+13
View File
@@ -292,6 +292,19 @@ class TestHyundaiFingerprint:
assert state.desired_accel > 0.3
assert state.actual_accel > 0.24
def test_ioniq_6_longitudinal_tuning_helper_softens_stop_release_handoff(self):
state = Ioniq6LongitudinalTuningState(actual_accel=-0.12, accel_last=-0.12,
stopping=True, stopping_count=25,
long_control_state_last=LongCtrlState.stopping)
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=0.0, a_ego=0.0,
long_control_state=LongCtrlState.starting, long_active=True)
assert state.actual_accel == pytest.approx(0.06)
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=1.0, v_ego=0.0, a_ego=0.0,
long_control_state=LongCtrlState.starting, long_active=True)
assert state.actual_accel == pytest.approx(0.24)
def test_genesis_g90_longitudinal_tuning_softens_final_stop_hold(self):
state = GenesisG90LongitudinalTuningState()
selfdrive/controls/lib/latcontrol_torque.py
+14 -14
View File
@@ -211,21 +211,21 @@ IONIQ_6_FF_CUTOFF = 0.48
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 0.82
IONIQ_6_TURN_IN_BOOST_RIGHT = 0.84
IONIQ_6_UNWIND_TAPER_LEFT = 1.44
IONIQ_6_UNWIND_TAPER_RIGHT = 2.70
IONIQ_6_TURN_IN_BOOST_LEFT = 0.88
IONIQ_6_TURN_IN_BOOST_RIGHT = 0.92
IONIQ_6_UNWIND_TAPER_LEFT = 1.52
IONIQ_6_UNWIND_TAPER_RIGHT = 2.92
IONIQ_6_FRICTION_MULT = 0.995
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.22
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.30
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.30
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 2.80
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.10
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.16
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.12
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.28
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.34
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.40
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.10
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.11
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.18
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.22
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 2.58
IONIQ_6_CENTER_TAPER_MAX = 0.042
IONIQ_6_CENTER_TAPER_LAT = 0.18
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02
@@ -242,8 +242,8 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.05
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.44
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.66
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.44
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.72
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.58
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
selfdrive/controls/tests/test_conditional_experimental_mode.py
+41 -1
View File
@@ -140,7 +140,7 @@ def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead()
model_length=v_ego * 5.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=v_ego * 5.0,
lead_d_rel=v_ego * 3.5,
lead_v_lead=8.0 * CV.MPH_TO_MS,
lead_model_prob=0.95,
)
@@ -175,6 +175,46 @@ def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled():
assert not cem.slow_lead_detected
def test_far_untracked_slow_lead_does_not_trigger_slow_lead():
v_ego = 50 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=False,
lead_status=True,
lead_d_rel=v_ego * 4.8,
lead_v_lead=v_ego - 2.0,
lead_model_prob=0.97,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
def test_pace_matched_lead_clears_slow_lead_quickly():
v_ego = 30 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 4.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=24.0,
lead_v_lead=v_ego + 0.2,
lead_model_prob=0.99,
)
toggles = SimpleNamespace(conditional_slower_lead=True, conditional_stopped_lead=False)
cem.slow_lead_filter.x = 1.0
cem.slow_lead_detected = True
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert not cem.slow_lead_detected
class DummyThemeManager:
def update_wheel_image(self, *args, **kwargs):
pass
starpilot/controls/lib/conditional_experimental_mode.py
+11 -1
View File
@@ -49,8 +49,10 @@ class ConditionalExperimentalMode:
STOP_APPROACH_MAX_LEAD_SPEED = 4.5
STOP_APPROACH_MIN_MODEL_PROB = 0.9
SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB = 0.85
SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME = 7.0
SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME = 4.0
SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5
SLOW_LEAD_MIN_CLOSING_SPEED = 0.75
SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5
# ===== END TUNING PARAMETERS =====
@@ -166,6 +168,8 @@ class ConditionalExperimentalMode:
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", float("inf")))
lead_prob = float(getattr(lead, "modelProb", 1.0))
closing_speed = max(0.0, v_ego - lead_speed)
min_closing_speed = max(self.SLOW_LEAD_MIN_CLOSING_SPEED, 0.04 * v_ego)
if not starpilot_toggles.conditional_stopped_lead and v_ego < self.SLOW_LEAD_CONTINUITY_MIN_EGO:
self.slow_lead_filter.update(False)
@@ -179,12 +183,18 @@ class ConditionalExperimentalMode:
lead_status and
lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and
lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME) and
closing_speed >= min_closing_speed and
lead_speed < max(v_ego - 0.5, 2.0)
)
lead_threshold = scale_threshold(v_ego)
adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence
if lead_status and not slower_lead and not stopped_lead and closing_speed < (min_closing_speed * self.SLOW_LEAD_CLEAR_FASTER_FACTOR):
self.slow_lead_filter.update(False)
self.slow_lead_detected = False
return
if self.starpilot_planner.tracking_lead or raw_vision_slow_lead or stopped_lead:
self.slow_lead_filter.update(slower_lead or raw_vision_slow_lead or stopped_lead)
self.slow_lead_detected = bool(self.slow_lead_filter.x >= adjusted_threshold)