diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 6b309702e..824876b5b 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -731,7 +731,7 @@ class TestHyundaiFingerprint: @pytest.mark.parametrize("candidate, tracks_main_cruise", ( (CAR.HYUNDAI_ELANTRA_2021, False), (CAR.HYUNDAI_ELANTRA_HEV_2024, False), - (CAR.HYUNDAI_SONATA_HYBRID, True), + (CAR.HYUNDAI_SONATA_HYBRID, False), )) def test_legacy_hyundai_long_main_cruise_tracking_is_vehicle_specific(self, candidate, tracks_main_cruise): toggles = get_test_toggles() diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index ec833848f..c8f4a5c59 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -245,12 +245,6 @@ class CarInterfaceBase(ABC): fp_ret.pcmCruiseSpeed = False CP.openpilotLongitudinalControl = True - # The Sonata Hybrid needs its stock ACC main state tracked while using OP long. - # The classic Elantra follows the default always-on ACC-main behavior instead. - if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID and \ - CP.openpilotLongitudinalControl: - fp_ret.flags |= HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING.value - hyundai_has_lda_button = not (CP.flags & HyundaiFlags.CANFD) and ( 0x391 in fingerprint[0] or 0x50C in fingerprint[0] or diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index dd46774bd..aefa0568e 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -363,6 +363,8 @@ class LatControlTorque(LatControl): friction_threshold = get_genesis_g70_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) elif self.is_genesis_gv70: friction_threshold = get_genesis_gv70_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + elif self.is_sonata_hybrid: + friction_threshold = get_sonata_hybrid_friction_threshold(CS.vEgo, setpoint) friction_scale = 1.0 if bolt_2022_2023_tuned_path_active: ff *= get_bolt_2022_2023_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 197ce7608..1f94d0270 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -464,6 +464,10 @@ SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT = 0.18 SONATA_HYBRID_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05 SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED = 12.5 SONATA_HYBRID_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5 +SONATA_HYBRID_CHATTER_THRESHOLD_SPEED_BP = [0.0, 4.5, 7.5, 11.0, 20.0] +SONATA_HYBRID_CHATTER_THRESHOLD_BUMP = [0.02, 0.04, 0.04, 0.02, 0.0] +SONATA_HYBRID_CHATTER_THRESHOLD_CENTER = 0.20 +SONATA_HYBRID_CHATTER_THRESHOLD_CENTER_WIDTH = 0.05 SONATA_FF_REDUCTION_LEFT = 0.04 SONATA_FF_REDUCTION_RIGHT = 0.26 @@ -2506,6 +2510,17 @@ def get_sonata_hybrid_center_output_scale(desired_lateral_accel: float, v_ego: f return 1.0 - SONATA_HYBRID_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight +def get_sonata_hybrid_friction_threshold(v_ego: float, desired_lateral_accel: float) -> float: + base_threshold = get_standard_friction_threshold(v_ego) + speed_bump = np.interp(max(v_ego, 0.0), SONATA_HYBRID_CHATTER_THRESHOLD_SPEED_BP, + SONATA_HYBRID_CHATTER_THRESHOLD_BUMP) + center_weight = _sonata_hybrid_sigmoid( + (SONATA_HYBRID_CHATTER_THRESHOLD_CENTER - abs(desired_lateral_accel)) / + SONATA_HYBRID_CHATTER_THRESHOLD_CENTER_WIDTH + ) + return float(base_threshold + speed_bump * center_weight) + + def _sonata_sigmoid(x: float) -> float: return _sigmoid(x) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 9578cd2cb..f7af3fcb8 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -34,6 +34,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( get_kona_non_scc_highway_transition_output_scale, get_kia_ev6_center_output_scale, get_sonata_hybrid_center_output_scale, + get_sonata_hybrid_friction_threshold, get_prius_center_taper_scale, KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT, HONDA_ACCORD_TORQUE_KI, @@ -930,6 +931,13 @@ class TestLatControl: assert turn > 0.99 assert center > 0.85 + def test_sonata_hybrid_chatter_threshold_is_low_mid_speed_and_center_gated(self): + base_low = get_standard_friction_threshold(5.5) + base_high = get_standard_friction_threshold(20.0) + assert get_sonata_hybrid_friction_threshold(5.5, 0.0) > base_low + assert get_sonata_hybrid_friction_threshold(5.5, 0.6) == pytest.approx(base_low, abs=0.0001) + assert get_sonata_hybrid_friction_threshold(20.0, 0.0) == pytest.approx(base_high) + def test_ioniq_5_ff_scale_curve(self): assert get_ioniq_5_ff_scale(0.0, 0.0, 20.0) == 1.0 steady_left = get_ioniq_5_ff_scale(0.7, 0.0, 12.0) diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 0693b30a2..0c861b5b3 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -285,8 +285,8 @@ class StarPilotLateralLayout(_SettingsPage): SettingRow( "SteerDelay", "value", tr_noop("Actuator Delay"), subtitle=tr_noop("Exact full delay between steering command and vehicle response."), - get_value=lambda: f"{p.get_float('SteerDelay'):.2f}s", - on_click=lambda: self._show_slider("SteerDelay", 0.01, 1.0, step=0.01, unit="s", value_type="float"), + get_value=lambda: f"{p.get_float('SteerDelay'):.3f}s", + on_click=lambda: self._show_slider("SteerDelay", 0.01, 1.0, step=0.001, unit="s", value_type="float"), enabled=lambda: not p.get_bool("UseAutoSteerDelay"), disabled_label=tr_noop("Disabled while auto-learned delay is enabled."), visible=lambda: alt_on() and cs.steerActuatorDelay != 0,