diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index fd3439dde..d3bde23bf 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -4,7 +4,7 @@ import numpy as np from opendbc.can import CANPacker from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs from opendbc.car.common.filter_simple import FirstOrderFilter -from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance, get_max_angle_delta_vm +from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance from opendbc.car.common.conversions import Conversions as CV from opendbc.car.hyundai import hyundaicanfd, hyundaican from opendbc.car.hyundai.hyundaicanfd import CanBus @@ -66,7 +66,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_CONTROL_LIMIT = MAX_ANGLE def egmp_dynamic_longitudinal_tuning(CP) -> bool: @@ -227,16 +226,6 @@ def get_angle_smoothing_alpha(CP, v_ego: float) -> float: return float(np.interp(v_ego, DEFAULT_ANGLE_SMOOTHING_VEGO_BP, DEFAULT_ANGLE_SMOOTHING_ALPHA_V)) -def apply_steer_angle_limits_vm_checked(apply_angle: float, apply_angle_last: float, v_ego_raw: float, - steering_angle: float, lat_active: bool, limits, VM: VehicleModel) -> float | None: - new_apply_angle = apply_steer_angle_limits_vm(apply_angle, apply_angle_last, v_ego_raw, steering_angle, lat_active, limits, VM) - v_ego_raw = max(v_ego_raw, 1.0) - max_angle_delta = min(get_max_angle_delta_vm(v_ego_raw, VM, limits), limits.ANGLE_LIMITS.MAX_ANGLE_RATE) - safety_violation = lat_active and not np.isclose(new_apply_angle, - rate_limit(new_apply_angle, apply_angle_last, -max_angle_delta, max_angle_delta)) - return None if safety_violation else new_apply_angle - - def compute_torque_reduction_gain(steering_torque, v_ego, lat_active, last_gain): if lat_active: ceiling = np.interp(v_ego, [0.5, 1.5], [1.0, 0.85]) @@ -395,7 +384,6 @@ class CarController(CarControllerBase): if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: v_ego_raw = CS.out.vEgoRaw - ev9_high_angle_inhibit = self.CP.carFingerprint == CAR.KIA_EV9 and abs(CS.out.steeringAngleDeg) >= EV9_HIGH_ANGLE_CONTROL_LIMIT desired_angle = float(np.clip(actuators.steeringAngleDeg, -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) @@ -403,13 +391,12 @@ class CarController(CarControllerBase): self.angle_filter.update_alpha(get_angle_smoothing_alpha(self.CP, CS.out.vEgo)) desired_angle = self.angle_filter.update(desired_angle) - angle_limit_fn = apply_steer_angle_limits_vm_checked if self.CP.carFingerprint == CAR.KIA_EV9 else apply_steer_angle_limits_vm - apply_angle = angle_limit_fn(desired_angle, self.apply_angle_last, v_ego_raw, - CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM) + 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) if str(self.CP.carFingerprint) != ANGLE_SAFETY_BASELINE_MODEL: - apply_angle = angle_limit_fn(apply_angle or desired_angle, self.apply_angle_last, v_ego_raw, - CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM) + apply_angle = apply_steer_angle_limits_vm(apply_angle or desired_angle, self.apply_angle_last, v_ego_raw, + 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_steer_req = CC.latActive and apply_torque != 0.0 @@ -420,14 +407,6 @@ class CarController(CarControllerBase): apply_angle = CS.out.steeringAngleDeg apply_steer_req = False - if ev9_high_angle_inhibit: - apply_torque = 0 - apply_angle = float(np.clip(CS.out.steeringAngleDeg, - -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, - self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) - apply_steer_req = False - self.angle_filter.x = apply_angle - self.apply_angle_last = apply_angle if not CC.latActive: self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg, @@ -627,7 +606,7 @@ class CarController(CarControllerBase): steering_msg_active = apply_steer_req if self.CP.carFingerprint == CAR.KIA_EV9 and self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: # EV9 faults if the angle-steering status drops inactive during torque limiting. - # Hold the angle status active while lateral is active; high-angle inhibit only zeros gain. + # Hold the angle status active while lateral is active; VM/safety limits handle actuation. steering_msg_active = CC.latActive can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 4e4e81b5c..ce8bd7932 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -154,6 +154,20 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, "TORQUE_REQUEST": 0, "STEER_REQ": 0, }) + else: + lkas_values.update({ + "LKA_MODE": 0, + "LKA_AVAILABLE": 0, + "LKA_WARNING": 0, + "LKA_ICON": lka_icon, + "FCA_SYSWARN": 0, + "TORQUE_REQUEST": 0, + "STEER_REQ": 0, + "LFA_BUTTON": 0, + "LKA_ASSIST": 0, + "DAMP_FACTOR": 100, + "HAS_LANE_SAFETY": 0, + }) # These signals overlap DAMP_FACTOR in the local DBC naming; omitting them # preserves the stock angle-steering damping byte expected by the ADAS ECU. lkas_values.pop("STEER_MODE", None) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 4517346a4..3d89273b8 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -11,7 +11,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_steer_angle_limits_vm_checked + get_angle_smoothing_alpha 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 @@ -21,9 +21,8 @@ from opendbc.car.hyundai.radar_interface import MRREVO14F_RADAR_START_ADDR, MRR3 from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \ HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \ UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \ - LEGACY_LONGITUDINAL_CAR, CarControllerParams, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \ + LEGACY_LONGITUDINAL_CAR, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, \ HyundaiStarPilotSafetyFlags, Buttons, kia_ev6_gt_line_longitudinal_tuning -from opendbc.car.vehicle_model import VehicleModel LongCtrlState = CarControl.Actuators.LongControlState from opendbc.car.hyundai.fingerprints import FW_VERSIONS @@ -288,13 +287,6 @@ 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_checked_angle_limiter_blocks_rate_accel_conflict(self): - CP = CarInterface.get_non_essential_params(CAR.KIA_EV9) - params = CarControllerParams(CP) - - assert apply_steer_angle_limits_vm_checked(0.0, 100.0, 20.0, 100.0, True, - params, VehicleModel(CP)) is None - def test_ccnc_hda2_lka_layout_does_not_set_ccnc_safety_param(self): fingerprint = gen_empty_fingerprint() cam_can = CanBus(None, fingerprint).CAM @@ -1405,7 +1397,37 @@ class TestHyundaiFingerprint: assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0) assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(8.5) - def test_ev9_high_steering_angle_keeps_active_status_with_zero_gain(self): + def test_ev9_inactive_angle_status_keeps_standby_damping_without_stock_lkas(self): + CP = CarParams.new_message() + CP.carFingerprint = CAR.KIA_EV9 + CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING | + HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT) + CP.openpilotLongitudinalControl = False + + controller = CarController(DBC[CP.carFingerprint], CP) + can_bus = CanBus(CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS_ALT", 0)], can_bus.ACAN) + cc = SimpleNamespace(enabled=False, latActive=False, actuators=SimpleNamespace(longControlState=LongCtrlState.off), + leftBlinker=False, rightBlinker=False, hudControl=SimpleNamespace()) + cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg={}, + out=SimpleNamespace(steeringAngleDeg=-201.0)) + + msgs = controller.create_canfd_msgs(0, False, 0.0, -201.0, 0.0, 0.0, False, cc.hudControl, cs, cc, + get_test_toggles(), lka_icon=1, lfa_icon=1) + lkas_msgs = [msg for msg in msgs if msg[0] == 0x110] + assert len(lkas_msgs) == 1 + + parser.update([(1, lkas_msgs)]) + + assert parser.can_valid + assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 1 + assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0) + assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(-201.0) + assert parser.vl["LKAS_ALT"]["DAMP_FACTOR"] == pytest.approx(100.0) + assert parser.vl["LKAS_ALT"]["STEER_MODE"] == 2 + assert parser.vl["LKAS_ALT"]["NEW_SIGNAL_2"] == 3 + + def test_ev9_high_steering_angle_keeps_active_status_and_gain(self): CP = CarParams.new_message() CP.carFingerprint = CAR.KIA_EV9 CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_ANGLE_STEERING | @@ -1443,7 +1465,7 @@ class TestHyundaiFingerprint: cs = SimpleNamespace(stock_lfa_msg=None, stock_lkas_msg=stock_lkas, lfa_block_msg=lfa_block_msg, out=SimpleNamespace(steeringAngleDeg=120.0)) - msgs = controller.create_canfd_msgs(0, False, 0.0, 120.0, 0.0, 0.0, False, cc.hudControl, cs, cc, + msgs = controller.create_canfd_msgs(0, True, 0.44, 120.0, 0.0, 0.0, False, cc.hudControl, cs, cc, get_test_toggles(), lka_icon=2, lfa_icon=2) lkas_msgs = [msg for msg in msgs if msg[0] == 0x110] suppress_msgs = [msg for msg in msgs if msg[0] == 0x362] @@ -1454,7 +1476,7 @@ class TestHyundaiFingerprint: assert parser.can_valid assert parser.vl["LKAS_ALT"]["LKAS_ANGLE_ACTIVE"] == 2 - assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.0) + assert parser.vl["LKAS_ALT"]["ADAS_ACIAnglTqRedcGainVal"] == pytest.approx(0.44) assert parser.vl["LKAS_ALT"]["ADAS_StrAnglReqVal"] == pytest.approx(120.0) def test_can_acc_commands_use_default_values(self):