diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 9576da743..0f12f8362 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -31,7 +31,7 @@ def get_non_scc_cruise_signals(CP) -> tuple[str, str, str, str, str, str]: if CP.flags & HyundaiFlags.EV: return "LABEL11", "CC_React", "EMS12", "ACC_ACT", "E_EMS11", "Cruise_Limit_Target" if CP.flags & HyundaiFlags.HYBRID: - return "EMS16", "CRUISE_LAMP_M", "EMS16", "CRUISE_LAMP_S", "ELECT_GEAR", "SLC_SET_SPEED" + return "E_CRUISE_CONTROL", "CRUISE_LAMP_M", "E_CRUISE_CONTROL", "CRUISE_LAMP_S", "ELECT_GEAR", "SLC_SET_SPEED" return "EMS16", "CRUISE_LAMP_M", "EMS16", "CRUISE_LAMP_S", "LVR12", "CF_Lvr_CruiseSet" diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 13bab758a..5db2844cd 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -138,7 +138,7 @@ class TestHyundaiFingerprint: @pytest.mark.parametrize(("candidate", "expected_msgs", "unexpected_msgs"), [ (CAR.HYUNDAI_ELANTRA_2022_NON_SCC, ("EMS16", "LVR12"), ()), - (CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, ("EMS16", "ELECT_GEAR"), ("E_CRUISE_CONTROL",)), + (CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, ("E_CRUISE_CONTROL", "ELECT_GEAR"), ("EMS16",)), (CAR.HYUNDAI_KONA_EV_NON_SCC, ("LABEL11", "EMS12", "E_EMS11"), ()), ]) def test_non_scc_cruise_message_selection(self, candidate, expected_msgs, unexpected_msgs): diff --git a/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc b/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc index f94511ad4..befafd0f1 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc +++ b/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc @@ -1536,6 +1536,10 @@ BO_ 881 E_EMS11: 8 XXX BO_ 1355 EV_PC6: 8 CGW SG_ CF_Vcu_SbwWarnMsg : 16|3@1+ (1,0) [0|7] "" Vector__XXX +BO_ 1429 E_CRUISE_CONTROL: 8 XXX + SG_ CRUISE_LAMP_M : 50|1@0+ (1,0) [0|1] "" XXX + SG_ CRUISE_LAMP_S : 51|1@0+ (1,0) [0|1] "" XXX + BO_ 1430 EV_PC2: 8 CGW SG_ CR_Ldc_ActVol_LS_V : 32|8@1+ (0.1,0) [0|0] "V" Vector__XXX diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 85ca14306..c1e02f382 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -126,6 +126,10 @@ IONIQ_6_CARS = ( SONATA_HYBRID_CARS = ( HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID, ) +ELANTRA_NON_SCC_CARS = ( + HYUNDAI_CAR.HYUNDAI_ELANTRA_2022_NON_SCC, + HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, +) KIA_EV6_CARS = ( HYUNDAI_CAR.KIA_EV6, ) @@ -264,6 +268,19 @@ SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02 SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 6.0 SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0 +ELANTRA_NON_SCC_FF_ADJUST_LEFT = 0.02 +ELANTRA_NON_SCC_FF_ADJUST_RIGHT = -0.02 +ELANTRA_NON_SCC_FF_ONSET = 0.14 +ELANTRA_NON_SCC_FF_ONSET_WIDTH = 0.06 +ELANTRA_NON_SCC_FF_CUTOFF = 1.10 +ELANTRA_NON_SCC_FF_CUTOFF_WIDTH = 0.34 +ELANTRA_NON_SCC_TRANSITION_SPEED = 8.0 +ELANTRA_NON_SCC_PHASE_SCALE = 0.10 +ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT = 0.10 +ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT = 0.12 +ELANTRA_NON_SCC_UNWIND_TAPER_LEFT = 0.22 +ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT = 0.12 + KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.08 KIA_FORTE_FF_REDUCTION_LEFT = 0.08 KIA_FORTE_FF_REDUCTION_RIGHT = 0.20 @@ -966,6 +983,48 @@ def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: fl return 1.0 - reduction +def _elantra_non_scc_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _elantra_non_scc_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / ELANTRA_NON_SCC_TRANSITION_SPEED) ** 2) + + +def _elantra_non_scc_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / ELANTRA_NON_SCC_PHASE_SCALE) + + +def _elantra_non_scc_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 get_elantra_non_scc_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + abs_lateral_accel = abs(desired_lateral_accel) + onset = _elantra_non_scc_sigmoid((abs_lateral_accel - ELANTRA_NON_SCC_FF_ONSET) / ELANTRA_NON_SCC_FF_ONSET_WIDTH) + cutoff = _elantra_non_scc_sigmoid((ELANTRA_NON_SCC_FF_CUTOFF - abs_lateral_accel) / ELANTRA_NON_SCC_FF_CUTOFF_WIDTH) + low_speed_factor = _elantra_non_scc_low_speed_factor(v_ego) + envelope = onset * cutoff * low_speed_factor + base_scale = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel, + ELANTRA_NON_SCC_FF_ADJUST_LEFT, + ELANTRA_NON_SCC_FF_ADJUST_RIGHT) * envelope) + phase = _elantra_non_scc_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + turn_in_boost = 1.0 + (_elantra_non_scc_side_value(desired_lateral_accel, + ELANTRA_NON_SCC_TURN_IN_BOOST_LEFT, + ELANTRA_NON_SCC_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_elantra_non_scc_side_value(desired_lateral_accel, + ELANTRA_NON_SCC_UNWIND_TAPER_LEFT, + ELANTRA_NON_SCC_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return base_scale * turn_in_boost * max(unwind_taper, 0.0) + + def _kia_forte_sigmoid(x: float) -> float: return _sigmoid(x) @@ -1498,6 +1557,7 @@ class LatControlTorque(LatControl): self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS self.is_sonata_hybrid = CP.carFingerprint in SONATA_HYBRID_CARS + self.is_elantra_non_scc = CP.carFingerprint in ELANTRA_NON_SCC_CARS self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) @@ -1623,6 +1683,7 @@ class LatControlTorque(LatControl): ioniq_ev_old_active = self.is_ioniq_ev_old ioniq_6_active = self.is_ioniq_6 sonata_hybrid_active = self.is_sonata_hybrid + elantra_non_scc_active = self.is_elantra_non_scc kia_forte_active = self.is_kia_forte 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() @@ -1666,6 +1727,8 @@ class LatControlTorque(LatControl): friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_6_center_taper) elif sonata_hybrid_active: ff *= get_sonata_hybrid_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_hybrid_center_taper + elif elantra_non_scc_active: + ff *= get_elantra_non_scc_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif kia_forte_active: ff *= get_kia_forte_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_forte_center_taper elif kia_ev6_test_active: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 533c868f7..4c59fc7f8 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -39,6 +39,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_g90_ff_scale, get_genesis_g90_friction_scale, get_genesis_g90_friction_threshold, + get_elantra_non_scc_ff_scale, get_ioniq_5_ff_scale, get_ioniq_5_friction_scale, get_ioniq_5_friction_threshold, @@ -237,6 +238,23 @@ class TestLatControl: assert get_sonata_hybrid_center_taper_scale(0.0, 3.0) < get_sonata_hybrid_center_taper_scale(0.0, 10.0) assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.20, 30.0) <= 1.0 + def test_elantra_non_scc_ff_scale_curve(self): + assert get_elantra_non_scc_ff_scale(0.0, 0.0, 20.0) == 1.0 + steady_left = get_elantra_non_scc_ff_scale(0.45, 0.0, 8.0) + steady_right = get_elantra_non_scc_ff_scale(-0.45, 0.0, 8.0) + turn_in_left = get_elantra_non_scc_ff_scale(0.45, 0.7, 8.0) + turn_in_right = get_elantra_non_scc_ff_scale(-0.45, -0.7, 8.0) + unwind_left = get_elantra_non_scc_ff_scale(0.45, -0.7, 8.0) + unwind_right = get_elantra_non_scc_ff_scale(-0.45, 0.7, 8.0) + assert steady_left < 1.0 + assert steady_right > steady_left + assert turn_in_left > steady_left + assert turn_in_right > steady_right + assert unwind_left < steady_left + assert unwind_right < steady_right + assert unwind_left < unwind_right + assert get_elantra_non_scc_ff_scale(-0.45, 0.0, 25.0) < get_elantra_non_scc_ff_scale(-0.45, 0.0, 8.0) + def test_kia_forte_ff_scale_curve(self): assert get_kia_forte_ff_scale(0.0, 0.0, 20.0) == 1.0 steady_left = get_kia_forte_ff_scale(0.45, 0.0, 25.0) @@ -485,6 +503,16 @@ class TestLatControl: assert lac_log.active assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.22) + def test_elantra_non_scc_default_update_path(self): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_ELANTRA_HEV_2022_NON_SCC) + CarInterface = interfaces[HYUNDAI.HYUNDAI_ELANTRA_HEV_2022_NON_SCC] + CP = CarInterface.get_non_essential_params(HYUNDAI.HYUNDAI_ELANTRA_HEV_2022_NON_SCC) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor) + def test_kia_forte_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_FORTE) CarInterface = interfaces[HYUNDAI.KIA_FORTE]