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

ev9

Browse Source
This commit is contained in:
firestar5683
2026-07-21 13:39:36 -05:00
parent 2141e5a5b1
commit 5b41566e4e
3 changed files with 14 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+2 -91
View File
@@ -67,19 +67,6 @@ REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40]
ANGLE_SAFETY_BASELINE_MODEL = str(CAR.KIA_SPORTAGE_HEV_2026)
DEFAULT_ANGLE_SMOOTHING_VEGO_BP = [5.0, 10.0, 20.0]
DEFAULT_ANGLE_SMOOTHING_ALPHA_V = [0.2, 0.1, 0.0]
EV9_HIGH_ANGLE_GAIN_BP = [0.0, 70.0, 120.0, 220.0, 320.0]
EV9_HIGH_ANGLE_GAIN_CAP_V = [1.0, 0.85, 0.55, 0.30, 0.16]
EV9_HIGH_ANGLE_GAIN_MIN = 0.004
EV9_TRACKING_GAIN_FULL_TORQUE = 75.0
EV9_TRACKING_GAIN_RELEASE_TORQUE = 125.0
EV9_DRIVER_OVERRIDE_TORQUE_THRESHOLD = 175.0
EV9_DRIVER_OVERRIDE_GAIN_BP = [0.0, 175.0, 350.0, 525.0]
EV9_DRIVER_OVERRIDE_GAIN_CAP_V = [0.08, 0.08, 0.04, 0.004]
EV9_DRIVER_OVERRIDE_RECOVERY_FRAMES = int(0.8 / DT_CTRL)
EV9_DRIVER_OVERRIDE_RECOVERY_ANGLE_BP = [0, EV9_DRIVER_OVERRIDE_RECOVERY_FRAMES // 2, EV9_DRIVER_OVERRIDE_RECOVERY_FRAMES]
EV9_DRIVER_OVERRIDE_RECOVERY_ANGLE_V = [0.75, 2.0, 5.0]
EV9_DRIVER_OVERRIDE_RECOVERY_GAIN_V = [0.08, 0.20, 0.45]
EV9_DRIVER_OVERRIDE_RECOVERY_ALPHA = 0.02
EV9_STOP_REQUEST_SPEED = 0.47
EV9_STANDSTILL_DELAY_FRAMES = 178
EV9_STOP_RELEASE_DELAY_FRAMES = 6
@@ -373,53 +360,6 @@ def compute_torque_reduction_gain(steering_torque, v_ego, lat_active, last_gain)
return round(gain / 0.004) * 0.004
def apply_ev9_tracking_gain(CP, gain: float, steering_torque: float, steering_pressed: bool, lat_active: bool) -> float:
if CP.carFingerprint != CAR.KIA_EV9 or not CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING or not lat_active or steering_pressed:
return gain
torque = abs(steering_torque)
if torque >= EV9_TRACKING_GAIN_RELEASE_TORQUE:
return gain
tracking_floor = float(np.interp(torque,
[EV9_TRACKING_GAIN_FULL_TORQUE, EV9_TRACKING_GAIN_RELEASE_TORQUE],
[1.0, 0.85]))
return max(gain, tracking_floor)
def apply_ev9_high_angle_gain_cap(CP, gain: float, steering_angle_deg: float, lat_active: bool,
steering_torque: float = 0.0, steering_pressed: bool = False) -> float:
if CP.carFingerprint != CAR.KIA_EV9 or not CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING or not lat_active:
return gain
cap = float(np.interp(abs(steering_angle_deg), EV9_HIGH_ANGLE_GAIN_BP, EV9_HIGH_ANGLE_GAIN_CAP_V))
gain = max(EV9_HIGH_ANGLE_GAIN_MIN, min(gain, cap))
if steering_pressed or abs(steering_torque) >= EV9_DRIVER_OVERRIDE_TORQUE_THRESHOLD:
driver_override_cap = float(np.interp(abs(steering_torque), EV9_DRIVER_OVERRIDE_GAIN_BP,
EV9_DRIVER_OVERRIDE_GAIN_CAP_V))
gain = min(gain, driver_override_cap)
return gain
def get_ev9_driver_override_recovery_limits(CP, recovery_frames: int) -> tuple[float | None, float | None]:
if CP.carFingerprint != CAR.KIA_EV9 or not CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING or recovery_frames <= 0:
return None, None
elapsed_frames = EV9_DRIVER_OVERRIDE_RECOVERY_FRAMES - min(recovery_frames, EV9_DRIVER_OVERRIDE_RECOVERY_FRAMES)
angle_error_limit = float(np.interp(elapsed_frames, EV9_DRIVER_OVERRIDE_RECOVERY_ANGLE_BP,
EV9_DRIVER_OVERRIDE_RECOVERY_ANGLE_V))
gain_cap = float(np.interp(elapsed_frames, EV9_DRIVER_OVERRIDE_RECOVERY_ANGLE_BP,
EV9_DRIVER_OVERRIDE_RECOVERY_GAIN_V))
return angle_error_limit, gain_cap
def ev9_driver_override_active(CP, steering_torque: float, steering_pressed: bool, lat_active: bool) -> bool:
return CP.carFingerprint == CAR.KIA_EV9 and CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING and lat_active and \
(steering_pressed or abs(steering_torque) >= EV9_DRIVER_OVERRIDE_TORQUE_THRESHOLD)
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
@@ -476,7 +416,6 @@ class CarController(CarControllerBase):
self._dash_lat_disengage_blink_frame = 0
self._dash_lat_disengage_init = False
self._dash_prev_lat_active = False
self._ev9_driver_override_recovery_frames = 0
def _update_dash_icon_state(self, CC):
if CC.latActive:
@@ -569,29 +508,8 @@ class CarController(CarControllerBase):
desired_angle = float(np.clip(actuators.steeringAngleDeg,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
ev9_driver_override = ev9_driver_override_active(self.CP, CS.out.steeringTorque, CS.out.steeringPressed, CC.latActive)
if ev9_driver_override:
self._ev9_driver_override_recovery_frames = EV9_DRIVER_OVERRIDE_RECOVERY_FRAMES
elif self._ev9_driver_override_recovery_frames > 0:
self._ev9_driver_override_recovery_frames -= 1
if ev9_driver_override:
desired_angle = float(np.clip(CS.out.steeringAngleDeg,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
self.angle_filter.x = desired_angle
else:
angle_alpha = get_angle_smoothing_alpha(self.CP, CS.out.vEgo)
if self._ev9_driver_override_recovery_frames > 0 and self.CP.carFingerprint == CAR.KIA_EV9:
angle_alpha = min(angle_alpha, EV9_DRIVER_OVERRIDE_RECOVERY_ALPHA)
self.angle_filter.update_alpha(angle_alpha)
desired_angle = self.angle_filter.update(desired_angle)
recovery_angle_error, _ = get_ev9_driver_override_recovery_limits(self.CP, self._ev9_driver_override_recovery_frames)
if recovery_angle_error is not None:
desired_angle = float(np.clip(desired_angle,
CS.out.steeringAngleDeg - recovery_angle_error,
CS.out.steeringAngleDeg + recovery_angle_error))
self.angle_filter.x = desired_angle
self.angle_filter.update_alpha(get_angle_smoothing_alpha(self.CP, CS.out.vEgo))
desired_angle = self.angle_filter.update(desired_angle)
apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, v_ego_raw,
CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM)
@@ -601,12 +519,6 @@ class CarController(CarControllerBase):
CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM)
apply_torque = compute_torque_reduction_gain(CS.out.steeringTorque, v_ego_raw, CC.latActive, self.apply_torque_last)
apply_torque = apply_ev9_tracking_gain(self.CP, apply_torque, CS.out.steeringTorque, CS.out.steeringPressed, CC.latActive)
apply_torque = apply_ev9_high_angle_gain_cap(self.CP, apply_torque, CS.out.steeringAngleDeg, CC.latActive,
CS.out.steeringTorque, CS.out.steeringPressed)
_, recovery_gain_cap = get_ev9_driver_override_recovery_limits(self.CP, self._ev9_driver_override_recovery_frames)
if recovery_gain_cap is not None:
apply_torque = min(apply_torque, recovery_gain_cap)
apply_steer_req = CC.latActive and apply_torque != 0.0
torque_fault = False
@@ -621,7 +533,6 @@ class CarController(CarControllerBase):
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
self.angle_filter.x = self.apply_angle_last
self._ev9_driver_override_recovery_frames = 0
else:
# steering torque
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
opendbc_repo/opendbc/car/hyundai/interface.py
+1 -2
View File
@@ -50,6 +50,7 @@ def apply_kia_ev6_gt_line_longitudinal_params(ret: structs.CarParams) -> None:
def apply_kia_ev9_longitudinal_params(ret: structs.CarParams) -> None:
ret.startAccel = 0.2
ret.longitudinalActuatorDelay = 0.3
ret.vEgoStarting = 0.5
@@ -157,8 +158,6 @@ class CarInterface(CarInterfaceBase):
if ret.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANFD_ANGLE_STEERING.value
if candidate == CAR.KIA_EV9:
ret.steerAtStandstill = True
if candidate == CAR.HYUNDAI_IONIQ_6:
# Keep lateral active through stops: zeroing torque at standstill dropped the
# stop-turn hold and forced a rate-limit re-ramp from zero on every pull-away
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+11 -52
View File
@@ -14,9 +14,7 @@ from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalT
update_ioniq_6_longitudinal_tuning, \
update_genesis_g90_longitudinal_tuning, egmp_dynamic_longitudinal_tuning, \
should_reset_ev6_gt_line_longitudinal_tuning, reset_ev6_gt_line_longitudinal_tuning, \
get_angle_smoothing_alpha, apply_ev9_tracking_gain, apply_ev9_high_angle_gain_cap, \
ev9_driver_override_active, \
get_ev9_driver_override_recovery_limits, should_use_ev6_gt_line_stop_direct_tracking
get_angle_smoothing_alpha, should_use_ev6_gt_line_stop_direct_tracking
from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai import hyundaican, hyundaicanfd
@@ -301,58 +299,11 @@ class TestHyundaiFingerprint:
assert get_angle_smoothing_alpha(ev9_cp, 20.0) == pytest.approx(get_angle_smoothing_alpha(other_cp, 20.0))
assert get_angle_smoothing_alpha(other_cp, 20.0) == pytest.approx(0.0)
def test_ev9_high_angle_gain_cap_is_ev9_only_and_nonzero(self):
ev9_cp = SimpleNamespace(carFingerprint=CAR.KIA_EV9, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
sportage_cp = SimpleNamespace(carFingerprint=CAR.KIA_SPORTAGE_HEV_2026, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 60.0, True) == pytest.approx(0.70)
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 120.0, True) == pytest.approx(0.55)
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 320.0, True) == pytest.approx(0.16)
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.0, 320.0, True) > 0.0
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 320.0, False) == pytest.approx(0.70)
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 30.0, True, 150.0, True) == pytest.approx(0.08)
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 30.0, True, 350.0, True) == pytest.approx(0.04)
assert apply_ev9_high_angle_gain_cap(ev9_cp, 0.70, 30.0, True, 600.0, True) == pytest.approx(0.004)
assert apply_ev9_high_angle_gain_cap(sportage_cp, 0.70, 320.0, True) == pytest.approx(0.70)
assert apply_ev9_high_angle_gain_cap(sportage_cp, 0.70, 30.0, True, 400.0, True) == pytest.approx(0.70)
def test_ev9_tracking_gain_is_ev9_only_and_preserves_driver_override(self):
ev9_cp = SimpleNamespace(carFingerprint=CAR.KIA_EV9, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
sportage_cp = SimpleNamespace(carFingerprint=CAR.KIA_SPORTAGE_HEV_2026, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
assert apply_ev9_tracking_gain(ev9_cp, 0.848, 0.0, False, True) == pytest.approx(1.0)
assert apply_ev9_tracking_gain(ev9_cp, 0.848, 100.0, False, True) == pytest.approx(0.925)
assert apply_ev9_tracking_gain(ev9_cp, 0.60, 125.0, False, True) == pytest.approx(0.60)
assert apply_ev9_tracking_gain(ev9_cp, 0.60, 0.0, True, True) == pytest.approx(0.60)
assert apply_ev9_tracking_gain(ev9_cp, 0.60, 0.0, False, False) == pytest.approx(0.60)
assert apply_ev9_tracking_gain(sportage_cp, 0.60, 0.0, False, True) == pytest.approx(0.60)
def test_ev9_driver_override_recovery_limits_are_ev9_only(self):
ev9_cp = SimpleNamespace(carFingerprint=CAR.KIA_EV9, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
sportage_cp = SimpleNamespace(carFingerprint=CAR.KIA_SPORTAGE_HEV_2026, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
assert get_ev9_driver_override_recovery_limits(sportage_cp, 80) == (None, None)
assert get_ev9_driver_override_recovery_limits(ev9_cp, 0) == (None, None)
angle_limit_start, gain_cap_start = get_ev9_driver_override_recovery_limits(ev9_cp, 80)
angle_limit_end, gain_cap_end = get_ev9_driver_override_recovery_limits(ev9_cp, 1)
assert angle_limit_start < angle_limit_end
assert gain_cap_start < gain_cap_end
def test_ev9_driver_override_detection_is_ev9_only(self):
ev9_cp = SimpleNamespace(carFingerprint=CAR.KIA_EV9, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
sportage_cp = SimpleNamespace(carFingerprint=CAR.KIA_SPORTAGE_HEV_2026, flags=int(HyundaiFlags.CANFD_ANGLE_STEERING))
assert ev9_driver_override_active(ev9_cp, 0.0, True, True)
assert ev9_driver_override_active(ev9_cp, 200.0, False, True)
assert not ev9_driver_override_active(ev9_cp, 200.0, False, False)
assert not ev9_driver_override_active(sportage_cp, 400.0, True, True)
def test_ev9_allows_lateral_at_standstill_without_changing_other_angle_platforms(self):
def test_ev9_matches_other_angle_platform_standstill_behavior(self):
ev9_cp = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], False, False, False, None)
sportage_cp = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, gen_empty_fingerprint(), [], False, False, False, None)
assert ev9_cp.steerAtStandstill
assert not ev9_cp.steerAtStandstill
assert not sportage_cp.steerAtStandstill
@pytest.mark.parametrize("candidate", (CAR.KIA_K4_2025, CAR.KIA_CARNIVAL_2025, CAR.KIA_CARNIVAL_HEV_4TH_GEN))
@@ -1142,6 +1093,14 @@ class TestHyundaiFingerprint:
assert CP.longitudinalActuatorDelay == pytest.approx(0.6)
assert CP.startingState
def test_ev9_longitudinal_params_match_observed_response(self):
toggles = get_test_toggles()
CP = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], True, False, False, toggles)
assert CP.startAccel == pytest.approx(0.2)
assert CP.vEgoStarting == pytest.approx(0.5)
assert CP.longitudinalActuatorDelay == pytest.approx(0.3)
def test_ioniq_6_longitudinal_tuning_helper_matches_dynamic_profile(self):
state = Ioniq6LongitudinalTuningState()