mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-23 17:23:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

I6 and CEM

Browse Source
This commit is contained in:
firestar5683
2026-04-30 10:18:17 -05:00
parent f58b6fc6c7
commit edc9a9ce60
5 changed files with 131 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+31 -6
View File
@@ -24,12 +24,18 @@ CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000
CANFD_BLINKER_STALKS_STALE_NS = 200_000_000
HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0
HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0
IONIQ_6_CANFD_SCC_ACCEL_STEP = 6.0 / 50.0
IONIQ_6_CANFD_SCC_DECEL_STEP = 15.0 / 50.0
IONIQ_6_LONG_MIN_JERK = 0.5
IONIQ_6_LONG_JERK_LIMIT = 4.0
IONIQ_6_LONG_JERK_LIMIT = 4.8
IONIQ_6_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0]
IONIQ_6_LONG_LOOKAHEAD_JERK_V = [0.3, 0.45, 0.6]
IONIQ_6_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005]
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]
@dataclass
@@ -39,6 +45,7 @@ class Ioniq6LongitudinalTuningState:
accel_last: float = 0.0
jerk_upper: float = 0.0
jerk_lower: float = 0.0
launch_active: bool = False
stopping: bool = False
stopping_count: int = 0
long_control_state_last: LongCtrlState = LongCtrlState.off
@@ -58,6 +65,7 @@ def _calculate_ioniq_6_dynamic_lower_jerk(accel_error: float) -> float:
def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, accel_cmd: float, v_ego: float, a_ego: float,
long_control_state: LongCtrlState, long_active: bool) -> Ioniq6LongitudinalTuningState:
starting = long_control_state == LongCtrlState.starting
stopping = long_control_state == LongCtrlState.stopping
if not long_active or not stopping:
@@ -76,9 +84,16 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
state.accel_last = 0.0
state.jerk_upper = 0.0
state.jerk_lower = 0.0
state.launch_active = False
state.long_control_state_last = long_control_state
return state
if accel_cmd <= 0.0 or v_ego >= IONIQ_6_LAUNCH_HOLD_SPEED_BP[-1]:
state.launch_active = False
elif starting or (state.launch_active and v_ego < IONIQ_6_LAUNCH_HOLD_SPEED_BP[-1]) or \
(state.long_control_state_last == LongCtrlState.starting and long_control_state == LongCtrlState.pid and v_ego < IONIQ_6_LAUNCH_HOLD_SPEED_BP[-1]):
state.launch_active = True
upper_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0])) if long_control_state == LongCtrlState.pid else IONIQ_6_LONG_MIN_JERK
lower_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
@@ -96,9 +111,14 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
state.jerk_lower = min(dynamic_lower_jerk, lower_speed_limit)
if state.stopping:
state.desired_accel = 0.0
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])))
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)
state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower)
state.accel_last = state.actual_accel
@@ -213,25 +233,30 @@ class CarController(CarControllerBase):
self.long_active_ecu = self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
use_ioniq_6_dynamic_long_tuning = self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu and \
actuators.longControlState == LongCtrlState.pid
actuators.longControlState in (LongCtrlState.starting, LongCtrlState.pid, LongCtrlState.stopping)
if use_ioniq_6_dynamic_long_tuning and self.frame % 5 == 0:
self._ioniq_6_long_tuning = update_ioniq_6_longitudinal_tuning(self._ioniq_6_long_tuning, accel_cmd,
CS.out.vEgo, CS.out.aEgo,
actuators.longControlState, self.long_active_ecu)
use_ioniq_6_smoothed_accel = use_ioniq_6_dynamic_long_tuning and accel_cmd >= self._ioniq_6_long_tuning.actual_accel
use_ioniq_6_smoothed_accel = use_ioniq_6_dynamic_long_tuning and (
accel_cmd >= self._ioniq_6_long_tuning.actual_accel or
self._ioniq_6_long_tuning.launch_active or
self._ioniq_6_long_tuning.stopping
)
if self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu:
if use_ioniq_6_smoothed_accel:
accel = self._ioniq_6_long_tuning.actual_accel
stopping = self._ioniq_6_long_tuning.stopping
elif use_ioniq_6_dynamic_long_tuning:
accel = float(np.clip(accel_cmd,
self.accel_last - HYUNDAI_CANFD_SCC_DECEL_STEP,
self.accel_last + HYUNDAI_CANFD_SCC_ACCEL_STEP))
self.accel_last - IONIQ_6_CANFD_SCC_DECEL_STEP,
self.accel_last + IONIQ_6_CANFD_SCC_ACCEL_STEP))
self._ioniq_6_long_tuning.desired_accel = accel_cmd
self._ioniq_6_long_tuning.actual_accel = accel
self._ioniq_6_long_tuning.accel_last = accel
self._ioniq_6_long_tuning.jerk_upper = 3.0
self._ioniq_6_long_tuning.jerk_lower = 5.0 if CC.enabled else 1.0
self._ioniq_6_long_tuning.launch_active = False
self._ioniq_6_long_tuning.stopping = stopping
self._ioniq_6_long_tuning.long_control_state_last = actuators.longControlState
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+16
View File
@@ -275,6 +275,22 @@ class TestHyundaiFingerprint:
assert state.jerk_upper == pytest.approx(0.0)
assert state.jerk_lower == pytest.approx(0.0)
def test_ioniq_6_longitudinal_tuning_helper_holds_launch_through_starting_handoff(self):
state = Ioniq6LongitudinalTuningState()
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.launch_active
assert state.actual_accel == pytest.approx(0.24)
assert state.jerk_upper == pytest.approx(4.8)
assert state.jerk_lower == pytest.approx(1.0)
state = update_ioniq_6_longitudinal_tuning(state, accel_cmd=0.3, v_ego=0.25, a_ego=1.2,
long_control_state=LongCtrlState.pid, long_active=True)
assert state.launch_active
assert state.desired_accel > 0.3
assert state.actual_accel > 0.24
def test_canfd_acc_control_uses_direct_accel(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV6
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.76
IONIQ_6_TURN_IN_BOOST_RIGHT = 0.76
IONIQ_6_UNWIND_TAPER_LEFT = 1.36
IONIQ_6_UNWIND_TAPER_RIGHT = 2.40
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_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.20
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.26
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.20
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 2.45
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.09
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.14
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.02
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 1.96
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_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.60
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.30
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.66
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.44
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
+42
View File
@@ -18,6 +18,7 @@ def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead:
model_stopped=model_stopped,
tracking_lead=tracking_lead,
starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=False),
starpilot_following=SimpleNamespace(slower_lead=False, following_lead=False),
lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel, vLead=lead_v_lead,
modelProb=lead_model_prob, radar=lead_radar),
)
@@ -133,6 +134,47 @@ def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_fl
assert cem.stop_light_detected
def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead():
v_ego = 35 * CV.MPH_TO_MS
cem = make_cem(
model_length=v_ego * 5.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=v_ego * 5.0,
lead_v_lead=8.0 * CV.MPH_TO_MS,
lead_model_prob=0.95,
)
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.tracking_lead = False
cem.starpilot_planner.starpilot_following.slower_lead = False
cem.slow_lead(toggles, v_ego)
assert cem.slow_lead_detected
def test_slow_lead_does_not_linger_at_crawl_when_stopped_lead_disabled():
v_ego = 1.5
cem = make_cem(
model_length=20.0,
tracking_lead=True,
lead_status=True,
lead_d_rel=8.0,
lead_v_lead=1.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
+28 -9
View File
@@ -48,6 +48,9 @@ class ConditionalExperimentalMode:
STOP_APPROACH_LATCH_TIME = 1.0
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_MIN_EGO = 2.5
# ===== END TUNING PARAMETERS =====
@@ -158,19 +161,35 @@ class ConditionalExperimentalMode:
self.curve_detected = bool(self.curvature_filter.x >= THRESHOLD and v_ego > CRUISING_SPEED)
def slow_lead(self, starpilot_toggles, v_ego):
if self.starpilot_planner.tracking_lead:
slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead
stopped_lead = starpilot_toggles.conditional_stopped_lead and self.starpilot_planner.lead_one.vLead < 1
lead_threshold = scale_threshold(v_ego)
lead = self.starpilot_planner.lead_one
lead_status = bool(getattr(lead, "status", False))
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", float("inf")))
lead_prob = float(getattr(lead, "modelProb", 1.0))
# Adjust threshold based on lead probability for vision-only accuracy
lead_prob = getattr(self.starpilot_planner.lead_one, 'modelProb', 1.0)
adjusted_threshold = lead_threshold * (1.0 + 0.2 * (1.0 - lead_prob)) # Higher threshold for lower confidence
if not starpilot_toggles.conditional_stopped_lead and v_ego < self.SLOW_LEAD_CONTINUITY_MIN_EGO:
self.slow_lead_filter.update(False)
self.slow_lead_detected = False
return
self.slow_lead_filter.update(slower_lead or stopped_lead)
slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead
stopped_lead = starpilot_toggles.conditional_stopped_lead and lead_speed < 1
raw_vision_slow_lead = bool(
starpilot_toggles.conditional_slower_lead and
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
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 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)
else:
self.slow_lead_filter.x = 0
self.slow_lead_filter.update(False)
self.slow_lead_detected = False
def stop_sign_and_light(self, v_ego, sm, model_time):