From a143e6ee93fa005e427904e86b23ebb37184642d Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 16 Aug 2026 10:45:56 -0500 Subject: [PATCH] Squilliam Fancyson --- common/params_keys.h | 1 + opendbc_repo/opendbc/car/hyundai/values.py | 1 + .../opendbc/safety/modes/hyundai_common.h | 5 ++++ .../opendbc/safety/tests/test_hyundai.py | 15 ++++++++++ selfdrive/car/card.py | 28 +++++++++++++++++++ selfdrive/controls/lib/latcontrol_angle.py | 25 +++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 8 +++++- selfdrive/pandad/panda_safety.cc | 13 +++++++++ 8 files changed, 95 insertions(+), 1 deletion(-) diff --git a/common/params_keys.h b/common/params_keys.h index 0f91b612d..d2adbc03b 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -62,6 +62,7 @@ inline static std::unordered_map keys = { {"GsmRoaming", {PERSISTENT, BOOL}}, {"HardwareSerial", {PERSISTENT, STRING}}, {"HasAcceptedTerms", {PERSISTENT, STRING, "0"}}, + {"HyundaiLkasAolPending", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL, "0", "0"}}, {"HondaGasFactorParams", {PERSISTENT, FLOAT}}, {"HondaLateralPidKiScale", {PERSISTENT, FLOAT, "1.0", "1.0", 3}}, {"HondaLateralPidKpScale", {PERSISTENT, FLOAT, "1.0", "1.0", 3}}, diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 57456969e..b27acf48f 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -111,6 +111,7 @@ class HyundaiSafetyFlags(IntFlag): class HyundaiStarPilotSafetyFlags(IntFlag): + AOL_LKAS_ON_INIT = 128 AOL_MAIN_LKAS_SYNC = 32 HAS_LDA_BUTTON = 1024 AOL_LKAS_ON_ENGAGE = 2048 diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index 5487a9765..081e7ef86 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_common.h @@ -77,6 +77,7 @@ void hyundai_common_init(uint16_t param) { const uint16_t HYUNDAI_PARAM_FCEV_GAS = 256; const uint16_t HYUNDAI_PARAM_ALT_LIMITS_2 = 512; + const uint16_t HYUNDAI_PARAM_AOL_LKAS_ON_INIT = 128; const int HYUNDAI_PARAM_HAS_LDA_BUTTON = 1024; const uint16_t HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE = 2048; const uint16_t HYUNDAI_PARAM_NON_SCC = 4096; @@ -100,6 +101,10 @@ void hyundai_common_init(uint16_t param) { hyundai_can_refresh_msgs = GET_FLAG(param, HYUNDAI_PARAM_CAN_REFRESH_MSGS); hyundai_aol_main_lkas_sync = false; + if (GET_FLAG(param, HYUNDAI_PARAM_AOL_LKAS_ON_INIT)) { + lkas_on = true; + } + hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; acc_main_on_prev = false; acc_main_on_tx = false; diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index a565bff49..bad73bdbf 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -572,6 +572,21 @@ class TestHyundaiAolLkasOnEngageStockSafety(HyundaiAolLkasOnEngageStockBase, Tes self.safety.init_tests() +class TestHyundaiAolLkasOnInitSafety(TestHyundaiSafety): + def setUp(self): + self.packer = CANPackerSafety("hyundai_kia_generic") + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_INIT) + self.safety.init_tests() + + def test_pending_lkas_press_allows_aol_after_safety_init(self): + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + self._rx(self._button_msg(Buttons.NONE)) + + self.assertTrue(self.safety.get_lkas_on()) + self.assertTrue(self.safety.get_aol_allowed()) + + class TestHyundaiAolMainLkasSyncSafety(TestHyundaiSafety): def setUp(self): self.packer = CANPackerSafety("hyundai_kia_generic") diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 2fa7fc388..bec0ed642 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -39,6 +39,11 @@ from openpilot.starpilot.controls.starpilot_card import StarPilotCard REPLAY = "REPLAY" in os.environ OPENPILOT_LEAD_MIN_DISTANCE = 0.1 REDNECK_DECREASE_LOOKAHEAD_POINTS = 10 +HYUNDAI_SAFETY_MODELS = { + int(structs.CarParams.SafetyModel.hyundai), + int(structs.CarParams.SafetyModel.hyundaiCanfd), + int(structs.CarParams.SafetyModel.hyundaiLegacy), +} EventName = log.OnroadEvent.EventName @@ -200,6 +205,7 @@ class Car: self.rk = Ratekeeper(100, print_delay_threshold=None) self.resume_prev_button = False + self.hyundai_lkas_aol_pending = False self.starpilot_toggles = get_starpilot_toggles(read_persisted_force_params=True) @@ -320,9 +326,31 @@ class Car: self.resume_prev_button = False FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles) + self._latch_pre_safety_hyundai_lkas_button(CS) return CS, RD, FPCS + def _hyundai_safety_active(self) -> bool: + if not self.sm.seen['pandaStates'] or not self.sm.valid['pandaStates']: + return False + + panda_states = self.sm['pandaStates'].pandaStates + return any( + int(getattr(state.safetyModel, "raw", state.safetyModel)) in HYUNDAI_SAFETY_MODELS + for state in panda_states + ) + + def _latch_pre_safety_hyundai_lkas_button(self, CS: car.CarState) -> None: + if self.CP.brand != "hyundai" or not getattr(self.starpilot_toggles, "always_on_lateral_lkas", False): + return + if self.hyundai_lkas_aol_pending or self._hyundai_safety_active() or not any( + button.type == ButtonType.lkas and button.pressed for button in CS.buttonEvents + ): + return + + self.hyundai_lkas_aol_pending = True + self.params.put_bool_nonblocking("HyundaiLkasAolPending", True) + def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.StarPilotCarState): """carState and carParams publish loop""" diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index a84f3a3a7..02c9ffeab 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,17 +1,34 @@ import math from cereal import log +from opendbc.car.subaru.values import CAR as SUBARU_CAR from openpilot.selfdrive.controls.lib.latcontrol import LatControl # TODO This is speed dependent STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees +_ASCENT_ANGLE_TRACKING_GAIN = 0.25 +_ASCENT_ANGLE_TRACKING_MAX_CORRECTION = 8.0 +_ASCENT_ANGLE_TRACKING_MIN_SPEED = 5.0 + + +def _ascent_angle_tracking_target(target_angle: float, steering_angle: float, + v_ego: float, steering_pressed: bool) -> float: + if steering_pressed or v_ego < _ASCENT_ANGLE_TRACKING_MIN_SPEED: + return target_angle + + correction = (target_angle - steering_angle) * _ASCENT_ANGLE_TRACKING_GAIN + correction = max(-_ASCENT_ANGLE_TRACKING_MAX_CORRECTION, + min(_ASCENT_ANGLE_TRACKING_MAX_CORRECTION, correction)) + return target_angle + correction + class LatControlAngle(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) self.sat_check_min_speed = 5. self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai") + self.is_ascent = CP.carFingerprint == SUBARU_CAR.SUBARU_ASCENT_2023 def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, calibrated_pose, model_data, starpilot_toggles): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -24,6 +41,14 @@ class LatControlAngle(LatControl): angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg + if self.is_ascent: + angle_steers_des = _ascent_angle_tracking_target( + angle_steers_des, + CS.steeringAngleDeg, + CS.vEgo, + bool(getattr(CS, "steeringPressed", False)), + ) + if self.use_steer_limited_by_safety: # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation angle_control_saturated = steer_limited_by_safety diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 0a417ad26..00df097ff 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -17,7 +17,7 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI from opendbc.car.subaru.values import CAR as SUBARU from opendbc.car.vehicle_model import VehicleModel from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, _ascent_angle_tracking_target from openpilot.selfdrive.controls.lib.latcontrol_pid import ( LatControlPID, get_civic_bosch_modified_pid_output_alpha, @@ -169,6 +169,12 @@ class TestLatControl: def test_center_chatter_friction_jerk_deadzone_preserves_vehicle_override(self): assert get_center_chatter_friction_jerk_deadzone(25.0, 0.6, 0.30) == pytest.approx(0.30) + def test_ascent_angle_tracking_correction_is_bounded_and_handoff_safe(self): + assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, False) == pytest.approx(12.5) + assert _ascent_angle_tracking_target(40.0, 0.0, 20.0, False) == pytest.approx(48.0) + assert _ascent_angle_tracking_target(10.0, 0.0, 4.0, False) == pytest.approx(10.0) + assert _ascent_angle_tracking_target(10.0, 0.0, 20.0, True) == pytest.approx(10.0) + def test_torque_log_exposes_friction_controller_state(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc index b3fe74a3e..4d74f752d 100644 --- a/selfdrive/pandad/panda_safety.cc +++ b/selfdrive/pandad/panda_safety.cc @@ -2,6 +2,8 @@ #include "cereal/messaging/messaging.h" #include "common/swaglog.h" +static constexpr uint16_t HYUNDAI_AOL_LKAS_ON_INIT = 128U; + void PandaSafety::configureSafetyMode(bool is_onroad) { if (is_onroad && !safety_configured_) { updateMultiplexingMode(); @@ -73,6 +75,10 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { auto starpilot_safety_configs = starpilot_car_params.getSafetyConfigs(); alternative_experience |= starpilot_car_params.getAlternativeExperience(); + const bool hyundai_lkas_aol_pending = params_.getBool("HyundaiLkasAolPending"); + if (hyundai_lkas_aol_pending) { + params_.putBool("HyundaiLkasAolPending", false); + } for (int i = 0; i < pandas_.size(); ++i) { // Default to SILENT safety model if not specified @@ -87,6 +93,13 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { safety_param |= starpilot_safety_configs[i].getSafetyParam(); } + const bool hyundai_safety = safety_model == cereal::CarParams::SafetyModel::HYUNDAI || + safety_model == cereal::CarParams::SafetyModel::HYUNDAI_CANFD || + safety_model == cereal::CarParams::SafetyModel::HYUNDAI_LEGACY; + if (hyundai_lkas_aol_pending && hyundai_safety) { + safety_param |= HYUNDAI_AOL_LKAS_ON_INIT; + } + LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); pandas_[i]->set_alternative_experience(alternative_experience); pandas_[i]->set_safety_model(safety_model, safety_param);