From 71260b52d10497d5b5909e0bf4b6f8e854b86a8f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 2 Jun 2026 20:27:34 -0500 Subject: [PATCH] angle cleanup --- .../opendbc/car/hyundai/carcontroller.py | 57 +++++++++++++++---- .../opendbc/car/hyundai/hyundaicanfd.py | 3 - .../opendbc/car/hyundai/tests/test_hyundai.py | 33 +---------- opendbc_repo/opendbc/car/hyundai/values.py | 33 ++--------- .../opendbc/safety/modes/hyundai_canfd.h | 11 +++- .../safety/tests/test_hyundai_canfd.py | 19 ++++++- 6 files changed, 77 insertions(+), 79 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 45fb5860c..621d0a0e8 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -2,7 +2,8 @@ from dataclasses import dataclass import numpy as np from opendbc.can import CANPacker -from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs +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 from opendbc.car.common.conversions import Conversions as CV from opendbc.car.hyundai import hyundaicanfd, hyundaican @@ -61,6 +62,7 @@ REDNECK_BUTTON_COPIES = 2 REDNECK_BUTTON_COPIES_TIME = 7 REDNECK_BUTTON_COPIES_TIME_IMPERIAL = [REDNECK_BUTTON_COPIES_TIME + 3, 70] REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40] +ANGLE_SAFETY_BASELINE_MODEL = str(CAR.KIA_SPORTAGE_HEV_2026) @dataclass @@ -195,6 +197,28 @@ def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningSt return state +def get_baseline_safety_cp(): + from opendbc.car.hyundai.interface import CarInterface + return CarInterface.get_non_essential_params(ANGLE_SAFETY_BASELINE_MODEL) + + +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]) + shelf = np.interp(v_ego, [2.0, 11.0], [0.45, 0.6]) + floor = np.interp(v_ego, [2.0, 22.0], [0.1, 0.3]) + bp1 = np.interp(v_ego, [2.0, 11.0], [75.0, 125.0]) + bp2 = np.interp(v_ego, [2.0, 11.0], [125.0, 150.0]) + bp3 = np.interp(v_ego, [2.0, 11.0], [175.0, 275.0]) + bp4 = np.interp(v_ego, [2.0, 22.0], [400.0, 700.0]) + target = np.interp(abs(steering_torque), [bp1, bp2, bp3, bp4], [ceiling, shelf, shelf, floor]) + else: + target = 0.0 + + gain = rate_limit(target, last_gain, -0.014, 0.004) + return round(gain / 0.004) * 0.004 + + def process_hud_alert(enabled, fingerprint, hud_control): sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)) @@ -227,6 +251,8 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_names[Bus.pt]) self.angle_limit_counter = 0 self.VM = VehicleModel(CP) + self.BASELINE_VM = VehicleModel(get_baseline_safety_cp()) if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else self.VM + self.angle_filter = FirstOrderFilter(0.0, 0.2, DT_CTRL) self.accel_last = 0 self.apply_torque_last = 0 @@ -328,32 +354,41 @@ class CarController(CarControllerBase): hud_control = CC.hudControl lka_icon, lfa_icon = self._update_dash_icon_state(CC) - self.params = CarControllerParams(self.CP, CS.out.vEgoRaw) + if not self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + self.params = CarControllerParams(self.CP, CS.out.vEgoRaw) apply_angle = CS.out.steeringAngleDeg if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: + v_ego_raw = CS.out.vEgoRaw desired_angle = float(np.clip(actuators.steeringAngleDeg, -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) - apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, CS.out.vEgoRaw, + + self.angle_filter.update_alpha(float(np.interp(CS.out.vEgo, [5.0, 10.0, 20.0], [0.2, 0.1, 0.0]))) + desired_angle = self.angle_filter.update(desired_angle) + + 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 CS.out.steeringPressed and abs(CS.out.steeringTorque) > self.params.STEER_THRESHOLD: - apply_torque = self.params.ANGLE_MIN_TORQUE_REDUCTION_GAIN - elif CC.latActive and CS.out.vEgoRaw < 0.3: - apply_torque = self.params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN - else: - apply_torque = self.params.ANGLE_MAX_TORQUE_REDUCTION_GAIN if CC.latActive else 0.0 + if str(self.CP.carFingerprint) != ANGLE_SAFETY_BASELINE_MODEL: + 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_steer_req = CC.latActive and apply_torque > 0.0 + 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 torque_fault = False if apply_angle is None: - apply_torque = 0.0 + apply_torque = 0 apply_angle = CS.out.steeringAngleDeg apply_steer_req = False self.apply_angle_last = apply_angle + if not CC.latActive: + self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg, + -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, + self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) + self.angle_filter.x = self.apply_angle_last else: # steering torque new_torque = int(round(actuators.torque * self.params.STEER_MAX)) diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index f1fec6c37..1f6b09231 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -138,9 +138,6 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, else: if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: if CP.flags & HyundaiFlags.SEND_LFA: - # Some CAN-FD angle-steering trims still expect the stock-style LFA status/UI - # message to remain present even though angle actuation comes through ADAS_CMD. - ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values)) ret.append(_create_angle_adas_cmd_msg(packer, CAN, apply_angle, lat_active, apply_torque)) else: ret.append(_create_angle_lfa_msg(packer, CAN, lfa_values, apply_angle, lat_active, apply_torque)) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 0303d1cd3..8d24b0f29 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -1167,11 +1167,11 @@ class TestHyundaiFingerprint: assert parser.vl["FCA12"]["FCA_DrvSetState"] == 2 assert parser.vl["FCA12"]["FCA_USM"] == 2 - def test_sportage_angle_steering_uses_lfa_and_adas_cmd_with_send_lfa(self): + def test_angle_steering_uses_adas_cmd_with_send_lfa(self): fingerprint = gen_empty_fingerprint() cam_can = CanBus(None, fingerprint).CAM fingerprint[cam_can][0xCB] = 24 - CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None) + CP = CarInterface.get_params(CAR.HYUNDAI_SANTA_FE_HEV_5TH_GEN, fingerprint, [], False, False, False, None) assert CP.flags & HyundaiFlags.SEND_LFA assert CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING @@ -1180,7 +1180,6 @@ class TestHyundaiFingerprint: can_bus = CanBus(CP) msgs = hyundaicanfd.create_steering_messages(packer, CP, can_bus, True, True, 1.0, 12.3) assert [(packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs] == [ - ("LFA", can_bus.ECAN), ("ADAS_CMD_35_10ms", can_bus.ECAN), ] @@ -1474,34 +1473,6 @@ class TestHyundaiFingerprint: ] assert hyundaicanfd.create_ioniq_6_cluster_lane_change_messages(can_bus, 5, "none") == [] - def test_sportage_angle_jerk_override_is_scoped(self): - sportage = CarParams.new_message() - sportage.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026 - sportage.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING) - - comparison_angle = CarParams.new_message() - comparison_angle.carFingerprint = CAR.KIA_EV6 - comparison_angle.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING) - - ioniq6 = CarParams.new_message() - ioniq6.carFingerprint = CAR.HYUNDAI_IONIQ_6 - ioniq6.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT) - - sportage_params = CarControllerParams(sportage) - sportage_low_speed_params = CarControllerParams(sportage, vEgoRaw=5.0) - sportage_high_speed_params = CarControllerParams(sportage, vEgoRaw=20.0) - comparison_params = CarControllerParams(comparison_angle) - ioniq6_params = CarControllerParams(ioniq6) - - assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK - assert sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK == sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK - assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK > sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK - assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK - assert sportage_params.ANGLE_LIMITS.STEER_ANGLE_MAX > comparison_params.ANGLE_LIMITS.STEER_ANGLE_MAX - assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL > comparison_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL - assert sportage_params.ANGLE_LIMITS.MAX_ANGLE_RATE > comparison_params.ANGLE_LIMITS.MAX_ANGLE_RATE - assert comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK == ioniq6_params.ANGLE_LIMITS.MAX_LATERAL_JERK - def test_ioniq_5_canfd_aux_messages_are_optional(self): toggles = get_test_toggles() fingerprint = gen_empty_fingerprint() diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 0a439161b..8446b8827 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -1,9 +1,9 @@ import re -from dataclasses import dataclass, field, replace +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, ISO_LATERAL_JERK +from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL from opendbc.car.common.conversions import Conversions as CV from opendbc.car.structs import CarParams from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, SupportType @@ -11,13 +11,6 @@ from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = CarParams.Ecu AVERAGE_ROAD_ROLL = 0.06 # conservative roll margin used by Hyundai CAN-FD angle steering safety -SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL = 3.6 -SPORTAGE_HEV_2026_BASE_LATERAL_JERK = 3.25 -SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST = 0.55 -SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED = 11.0 -SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH = 5.0 -SPORTAGE_HEV_2026_MAX_ANGLE_RATE = 6.5 -SPORTAGE_HEV_2026_STEER_ANGLE_MAX = 220.0 HYUNDAI_MANDO_FRONT_RADAR_DBC = "hyundai_kia_mando_front_radar_generated" HYUNDAI_MRREVO14F_RADAR_DBC = "hyundai_mrrevo14f_radar_generated" HYUNDAI_MRR30_RADAR_DBC = "hyundai_mrr30_radar_generated" @@ -28,16 +21,13 @@ class CarControllerParams: ACCEL_MIN = -3.5 # m/s ACCEL_MAX = 3.5 # m/s ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( - 180, + 360, ([], []), ([], []), MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), - MAX_LATERAL_JERK=ISO_LATERAL_JERK + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), + MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), MAX_ANGLE_RATE=5, ) - ANGLE_MAX_TORQUE_REDUCTION_GAIN = 1.0 - ANGLE_MIN_TORQUE_REDUCTION_GAIN = 0.6 - ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN = 0.6 def __init__(self, CP, vEgoRaw=100.): self.ANGLE_LIMITS = self.ANGLE_LIMITS @@ -64,21 +54,6 @@ class CarControllerParams: if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: self.STEER_THRESHOLD = 175 - # The Sportage angle port still needs more authority in real turns than the - # fully calmed branch-wide ceiling allows, but the old low-speed jerk boost - # made the 3-20 degree band angry and ping-pongy as the car slowed down. - # Split the difference: - # - keep a calmer low-speed boost that fades out earlier - # - give the car a little more true turn headroom through accel/rate limits - if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026: - sportage_low_speed_weight = min(max((SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED - vEgoRaw) / SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH, 0.0), 1.0) - sportage_lateral_jerk = SPORTAGE_HEV_2026_BASE_LATERAL_JERK + (SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST * sportage_low_speed_weight) - self.ANGLE_LIMITS = replace(self.ANGLE_LIMITS, - STEER_ANGLE_MAX=SPORTAGE_HEV_2026_STEER_ANGLE_MAX, - MAX_LATERAL_ACCEL=SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), - MAX_LATERAL_JERK=sportage_lateral_jerk + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), - MAX_ANGLE_RATE=SPORTAGE_HEV_2026_MAX_ANGLE_RATE) - # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ, diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index eeadf2178..f589cbf5a 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -190,7 +190,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { .has_steer_req_tolerance = true, }; const AngleSteeringLimits HYUNDAI_CANFD_ANGLE_STEERING_LIMITS = { - .max_angle = 1800, + .max_angle = 3600, .angle_deg_to_can = 10, .frequency = 100U, }; @@ -230,9 +230,16 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { int desired_angle = (msg->data[11] << 6U) | (msg->data[10] >> 2U); desired_angle = to_signed(desired_angle, 14); + // ADAS_ACIAnglTqRedcGainVal: bit 96, 8 bits, unsigned. Raw 0-250 valid, 251-255 reserved. + const uint8_t gain_raw = msg->data[12]; + bool gain_violation = gain_raw > 250U; + if (!steer_angle_req && (gain_raw != 0U)) { + gain_violation = true; + } + if (steer_angle_cmd_checks_vm(desired_angle, steer_angle_req, HYUNDAI_CANFD_ANGLE_STEERING_LIMITS, - HYUNDAI_CANFD_ANGLE_STEERING_PARAMS)) { + HYUNDAI_CANFD_ANGLE_STEERING_PARAMS) || gain_violation) { tx = false; } } else { diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 247936a0e..77d367d0a 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -174,7 +174,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): BUTTONS_TX_BUS = 2 LATERAL_FREQUENCY = 100 STANDSTILL_THRESHOLD = 12 - STEER_ANGLE_MAX = 180 + STEER_ANGLE_MAX = 360 DEG_TO_CAN = 10 GAS_MSG = ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL") SAFETY_PARAM = HyundaiSafetyFlags.CANFD_ANGLE_STEERING | HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.HYBRID_GAS @@ -248,7 +248,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): checksum = sig_checksum.calc_checksum(addr, sig_checksum, dat) _set_value(dat, sig_checksum, checksum) - def _angle_cmd_msg(self, angle, enabled, increment_timer=True): + def _angle_cmd_msg(self, angle, enabled, increment_timer=True, gain_raw=250): if increment_timer: self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY)) self.angle_cmd_cnt += 1 @@ -272,7 +272,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): dat[9] = (dat[9] & ~0x30) | (((2 if enabled else 1) & 0x3) << 4) dat[10] = (dat[10] & 0x03) | ((desired_angle & 0x3F) << 2) dat[11] = (desired_angle >> 6) & 0xFF - dat[12] = 250 if enabled else 0 + dat[12] = gain_raw if enabled or gain_raw != 250 else 0 self._update_checksum(addr, dat) return libsafety_py.make_CANPacket(addr, 0, bytes(dat)) @@ -335,6 +335,19 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest): self.assertFalse(self._tx(self._angle_cmd_msg(0, True, increment_timer=False))) self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False))) + def test_angle_torque_reduction_gain_limits(self): + if self.__class__.__name__ != "TestHyundaiCanfdAngleSteering": + return + + self.safety.set_controls_allowed(True) + self._reset_speed_measurement(1) + self._set_prev_desired_angle(0) + self.assertTrue(self._tx(self._angle_cmd_msg(0, True, gain_raw=250))) + self._set_prev_desired_angle(0) + self.assertFalse(self._tx(self._angle_cmd_msg(0, True, gain_raw=251))) + self._set_prev_desired_angle(0) + self.assertFalse(self._tx(self._angle_cmd_msg(0, False, gain_raw=1))) + class TestHyundaiCanfdAngleSteeringLfaAlt(TestHyundaiCanfdAngleSteering):