diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 8b195cbed..43bf780fa 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -35,6 +35,12 @@ def calculate_canfd_speed_limit(CP, FPCP, cp, cp_cam, speed_factor): class CarState(CarStateBase): + @staticmethod + def get_canfd_blinker_sig_names(car_fingerprint, use_alt_lamp: bool) -> tuple[str, str]: + if use_alt_lamp or car_fingerprint == CAR.HYUNDAI_KONA_EV_2ND_GEN: + return "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT" + return "LEFT_LAMP", "RIGHT_LAMP" + def __init__(self, CP, FPCP): super().__init__(CP, FPCP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) @@ -272,10 +278,8 @@ class CarState(CarStateBase): ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 - # TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP'] - left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP" - if self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2ND_GEN: - left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT" + left_blinker_sig, right_blinker_sig = self.get_canfd_blinker_sig_names(self.CP.carFingerprint, + cp.vl["BLINKERS"]["USE_ALT_LAMP"] == 1) ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig], cp.vl["BLINKERS"][right_blinker_sig]) if self.CP.enableBsm: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 8a9b5b831..dc8e37a0b 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -5,6 +5,7 @@ import pytest from opendbc.car import gen_empty_fingerprint from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict +from opendbc.car.hyundai.carstate import CarState from opendbc.car.hyundai.interface import CarInterface from opendbc.car.hyundai.hyundaicanfd import CanBus from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR @@ -114,6 +115,11 @@ class TestHyundaiFingerprint: assert len(ecus_not_in_whitelist) == 0, \ f"{car_model}: Car model has unexpected ECUs: {ecu_strings}" + def test_canfd_blinker_signal_selection(self): + assert CarState.get_canfd_blinker_sig_names(CAR.KIA_SPORTAGE_HEV_2026, True) == ("LEFT_LAMP_ALT", "RIGHT_LAMP_ALT") + assert CarState.get_canfd_blinker_sig_names(CAR.HYUNDAI_KONA_EV_2ND_GEN, False) == ("LEFT_LAMP_ALT", "RIGHT_LAMP_ALT") + assert CarState.get_canfd_blinker_sig_names(CAR.KIA_EV6, False) == ("LEFT_LAMP", "RIGHT_LAMP") + def test_blacklisted_parts(self, subtests): # Asserts no ECUs known to be shared across platforms exist in the database. # Tucson having Santa Cruz camera and EPS for example diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 46dbccd5d..16db67279 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -3,7 +3,7 @@ from dataclasses import dataclass, field from enum import IntFlag from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds -from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL +from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, ISO_LATERAL_JERK from opendbc.car.common.conversions import Conversions as CV from opendbc.car.structs import CarParams from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts @@ -21,7 +21,7 @@ class CarControllerParams: ([], []), ([], []), MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), - MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), + MAX_LATERAL_JERK=ISO_LATERAL_JERK + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), MAX_ANGLE_RATE=5, ) ANGLE_MAX_TORQUE_REDUCTION_GAIN = 1.0 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d8830a646..bcb2bdfc3 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -67,6 +67,9 @@ VOLT_STANDARD_CARS = ( GENESIS_G90_CARS = ( HYUNDAI_CAR.GENESIS_G90, ) +IONIQ_6_CARS = ( + HYUNDAI_CAR.HYUNDAI_IONIQ_6, +) KIA_EV6_CARS = ( HYUNDAI_CAR.KIA_EV6, ) @@ -198,6 +201,32 @@ GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT = 0.08 GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT = 0.07 GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT = 0.11 +IONIQ_6_LATERAL_TESTING_GROUND_ID = testing_ground.id_5 +IONIQ_6_LATERAL_TESTING_GROUND_VARIANT = "C" +IONIQ_6_FF_GAIN_LEFT = 0.04 +IONIQ_6_FF_GAIN_RIGHT = 0.015 +IONIQ_6_FF_ONSET = 0.10 +IONIQ_6_FF_ONSET_WIDTH = 0.04 +IONIQ_6_FF_CUTOFF = 0.72 +IONIQ_6_FF_CUTOFF_WIDTH = 0.18 +IONIQ_6_TRANSITION_SPEED = 10.0 +IONIQ_6_PHASE_SCALE = 0.10 +IONIQ_6_TURN_IN_BOOST_LEFT = 0.34 +IONIQ_6_TURN_IN_BOOST_RIGHT = 0.28 +IONIQ_6_UNWIND_TAPER_LEFT = 0.24 +IONIQ_6_UNWIND_TAPER_RIGHT = 0.38 +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.12 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.10 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.16 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.24 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.05 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.04 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 0.12 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 0.18 + KIA_EV6_LATERAL_TESTING_GROUND_ID = testing_ground.id_6 KIA_EV6_LATERAL_TESTING_GROUND_VARIANT = "C" KIA_EV6_FF_GAIN_LEFT = 0.07 @@ -629,6 +658,78 @@ def get_genesis_g90_friction_scale(v_ego: float, desired_lateral_accel: float, d return min(max(friction_scale, 0.92), 1.12) +def ioniq_6_lateral_testing_ground_active() -> bool: + return testing_ground.use(IONIQ_6_LATERAL_TESTING_GROUND_ID, IONIQ_6_LATERAL_TESTING_GROUND_VARIANT) + + +def _ioniq_6_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _ioniq_6_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / IONIQ_6_TRANSITION_SPEED) ** 2) + + +def _ioniq_6_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_PHASE_SCALE) + + +def _ioniq_6_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _ioniq_6_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / IONIQ_6_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / IONIQ_6_FRICTION_JERK_RISE) + return _ioniq_6_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_FF_GAIN_LEFT, IONIQ_6_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_FF_ONSET) / IONIQ_6_FF_ONSET_WIDTH) + cutoff = _ioniq_6_sigmoid((IONIQ_6_FF_CUTOFF - abs_lateral_accel) / IONIQ_6_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _ioniq_6_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_BOOST_LEFT, IONIQ_6_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_TAPER_LEFT, IONIQ_6_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.30 + 0.70 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT, IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT, IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.82), 1.18) + + +def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = IONIQ_6_FRICTION_MULT + friction_scale += (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT, IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_ioniq_6_side_value(desired_lateral_accel, IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT, IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.82), 1.08) + + def kia_ev6_lateral_testing_ground_active() -> bool: return testing_ground.use(KIA_EV6_LATERAL_TESTING_GROUND_ID, KIA_EV6_LATERAL_TESTING_GROUND_VARIANT) @@ -800,6 +901,7 @@ class LatControlTorque(LatControl): self.is_bolt_2017 = CP.carFingerprint in BOLT_2017_CARS self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS + self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_volt_cc = CP.carFingerprint == GM_CAR.CHEVROLET_VOLT_CC self.is_silverado = CP.carFingerprint == GM_CAR.CHEVROLET_SILVERADO @@ -887,6 +989,7 @@ class LatControlTorque(LatControl): bolt_2018_2021_tuned_path_active = self.is_bolt_2018_2021 volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active() genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active() + ioniq_6_test_active = self.is_ioniq_6 and ioniq_6_lateral_testing_ground_active() kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active() volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0 @@ -908,6 +1011,10 @@ class LatControlTorque(LatControl): ff *= get_genesis_g90_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_genesis_g90_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_genesis_g90_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + elif ioniq_6_test_active: + ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif kia_ev6_test_active: ff *= get_kia_ev6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_kia_ev6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index a9b2edeed..1fa175599 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -30,6 +30,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_g90_ff_scale, get_genesis_g90_friction_scale, get_genesis_g90_friction_threshold, + get_ioniq_6_ff_scale, + get_ioniq_6_friction_scale, + get_ioniq_6_friction_threshold, get_kia_ev6_ff_scale, get_kia_ev6_friction_scale, get_kia_ev6_friction_threshold, @@ -202,6 +205,30 @@ class TestLatControl: assert right_turn_in > left_turn_in > base assert base > left_unwind > right_unwind + def test_ioniq_6_ff_scale_curve(self): + assert get_ioniq_6_ff_scale(0.0, 0.0, 20.0) == 1.0 + assert get_ioniq_6_ff_scale(0.4, 0.0, 20.0) > get_ioniq_6_ff_scale(-0.4, 0.0, 20.0) + assert get_ioniq_6_ff_scale(0.4, 0.7, 8.0) > get_ioniq_6_ff_scale(0.4, 0.0, 8.0) > get_ioniq_6_ff_scale(0.4, -0.7, 8.0) + assert get_ioniq_6_ff_scale(-0.4, -0.7, 8.0) > get_ioniq_6_ff_scale(-0.4, 0.0, 8.0) > get_ioniq_6_ff_scale(-0.4, 0.7, 8.0) + assert get_ioniq_6_ff_scale(1.2, 0.0, 20.0) < get_ioniq_6_ff_scale(0.4, 0.0, 20.0) + + def test_ioniq_6_friction_threshold_curve(self): + base = get_friction_threshold(6.0) + left_turn_in = get_ioniq_6_friction_threshold(6.0, 0.5, 0.8) + right_turn_in = get_ioniq_6_friction_threshold(6.0, -0.5, -0.8) + left_unwind = get_ioniq_6_friction_threshold(6.0, 0.5, -0.8) + right_unwind = get_ioniq_6_friction_threshold(6.0, -0.5, 0.8) + assert left_turn_in < right_turn_in < base < left_unwind < right_unwind + + def test_ioniq_6_friction_scale_curve(self): + base = get_ioniq_6_friction_scale(25.0, 0.5, 0.8) + left_turn_in = get_ioniq_6_friction_scale(6.0, 0.5, 0.8) + right_turn_in = get_ioniq_6_friction_scale(6.0, -0.5, -0.8) + left_unwind = get_ioniq_6_friction_scale(6.0, 0.5, -0.8) + right_unwind = get_ioniq_6_friction_scale(6.0, -0.5, 0.8) + assert left_turn_in > right_turn_in > base + assert base > left_unwind > right_unwind + def test_kia_ev6_ff_scale_curve(self): assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0 assert get_kia_ev6_ff_scale(-0.3, 0.0, 20.0) > get_kia_ev6_ff_scale(0.3, 0.0, 20.0) @@ -280,6 +307,14 @@ class TestLatControl: assert lac_log.active + def test_ioniq_6_testing_ground_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6) + monkeypatch.setattr(latcontrol_torque, "ioniq_6_lateral_testing_ground_active", lambda: True) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + def test_kia_ev6_testing_ground_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6) monkeypatch.setattr(latcontrol_torque, "kia_ev6_lateral_testing_ground_active", lambda: True) diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py index 377978f9c..edb7379aa 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -61,9 +61,10 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( }, { "id": TESTING_GROUND_5, - "name": "Unused", - "description": "", - "aLabel": "A", + "name": "Ioniq 6 Lateral", + "description": "Hyundai Ioniq 6 lateral sandbox.", + "aLabel": "A - Installed tune", + "cLabel": "C - Firestar Tune", }, { "id": TESTING_GROUND_6, diff --git a/starpilot/common/tests/test_testing_grounds.py b/starpilot/common/tests/test_testing_grounds.py index 72f028620..86640f22f 100644 --- a/starpilot/common/tests/test_testing_grounds.py +++ b/starpilot/common/tests/test_testing_grounds.py @@ -17,6 +17,8 @@ def test_hidden_testing_ground_selection_is_migrated(tmp_path, monkeypatch, hidd monkeypatch.setattr(tg, "TESTING_GROUNDS_STATE_PATH", state_path) monkeypatch.setattr(tg, "_CACHE_LAST_REFRESH", 0.0) monkeypatch.setattr(tg, "_CACHE_LAST_MTIME_NS", -1) + monkeypatch.setattr(tg, "_VISIBLE_TESTING_GROUND_IDS", tuple(slot_id for slot_id in tg.TESTING_GROUND_IDS if slot_id != hidden_slot_id)) + monkeypatch.setattr(tg, "_DEFAULT_ACTIVE_SLOT", tg.TESTING_GROUND_1) monkeypatch.setattr(tg, "_CACHE_ACTIVE_SLOT", tg._DEFAULT_ACTIVE_SLOT) monkeypatch.setattr(tg, "_CACHE_ACTIVE_VARIANT", tg.DEFAULT_TESTING_GROUND_VARIANT)