From 80f465616a6fd04252e9709e2830a23cd3d4e379 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 21 Jul 2026 15:20:28 -0500 Subject: [PATCH] Tone --- .../opendbc/car/hyundai/carcontroller.py | 51 ++++++++++++++----- opendbc_repo/opendbc/car/hyundai/carstate.py | 2 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 11 +++- .../opendbc/safety/modes/hyundai_canfd.h | 4 +- .../safety/tests/safety_replay/helpers.py | 18 +++++-- .../safety/tests/test_hyundai_canfd.py | 15 ++++++ 6 files changed, 82 insertions(+), 19 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 5a8134576..d6fc68a1f 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 +from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance, get_max_angle_delta_vm, get_max_angle_vm from opendbc.car.common.conversions import Conversions as CV from opendbc.car.hyundai import hyundaicanfd, hyundaican from opendbc.car.hyundai.hyundaicanfd import CanBus @@ -343,6 +343,12 @@ 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 direct_angle_request_allowed(v_ego_raw, measured_angle, last_angle, drive_gear, VM, params): + safety_v_ego = max(v_ego_raw - 1.0, 1.0) + max_safety_angle = get_max_angle_vm(safety_v_ego, VM, params) + return drive_gear and abs(measured_angle) <= max_safety_angle and abs(last_angle) <= max_safety_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]) @@ -394,6 +400,7 @@ class CarController(CarControllerBase): 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.direct_angle_request_allowed = True self.accel_last = 0 self.apply_torque_last = 0 @@ -501,7 +508,15 @@ class CarController(CarControllerBase): if not self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: self.params = CarControllerParams(self.CP, CS.out.vEgoRaw) - apply_angle = CS.out.steeringAngleDeg + direct_angle_control = self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR and self.long_active_ecu + measured_steering_angle = CS.angle_steering_angle if direct_angle_control else CS.out.steeringAngleDeg + angle_lat_active = CC.latActive + if direct_angle_control and CC.latActive: + drive_gear = CS.out.gearShifter == structs.CarState.GearShifter.drive + angle_lat_active = direct_angle_request_allowed(CS.out.vEgoRaw, measured_steering_angle, self.apply_angle_last, + drive_gear, self.BASELINE_VM, self.params) and not CS.angle_steering_fault + self.direct_angle_request_allowed = angle_lat_active + apply_angle = measured_steering_angle if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING: v_ego_raw = CS.out.vEgoRaw @@ -512,24 +527,33 @@ class CarController(CarControllerBase): 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) + measured_steering_angle, angle_lat_active, self.params, self.VM) 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) + measured_steering_angle, angle_lat_active, 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 + if direct_angle_control and angle_lat_active: + # Match Panda's 1 m/s speed tolerance so a shrinking absolute limit stays inside its jerk envelope. + safety_v_ego = max(v_ego_raw - 1.0, 1.0) + max_angle_delta = min(get_max_angle_delta_vm(safety_v_ego, self.BASELINE_VM, self.params), + self.params.ANGLE_LIMITS.MAX_ANGLE_RATE) + apply_angle = float(np.clip(apply_angle, + self.apply_angle_last - max_angle_delta, + self.apply_angle_last + max_angle_delta)) + + apply_torque = compute_torque_reduction_gain(CS.out.steeringTorque, v_ego_raw, angle_lat_active, self.apply_torque_last) + apply_steer_req = angle_lat_active and apply_torque != 0.0 torque_fault = False if apply_angle is None: apply_torque = 0 - apply_angle = CS.out.steeringAngleDeg + apply_angle = measured_steering_angle apply_steer_req = False self.apply_angle_last = apply_angle - if not CC.latActive: - self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg, + if not angle_lat_active: + self.apply_angle_last = float(np.clip(measured_steering_angle, -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) self.angle_filter.x = self.apply_angle_last @@ -756,16 +780,19 @@ class CarController(CarControllerBase): CS.stock_lfa_msg, CS.stock_lkas_msg if preserve_stock_lkas else None, lka_icon=lka_icon)) - direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and not CS.angle_steering_fault + direct_steering_active = ccnc_angle_long and drive_gear and CC.latActive and self.direct_angle_request_allowed and not CS.angle_steering_fault + inactive_steering_angle = float(np.clip(CS.angle_steering_angle, + -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, + self.params.ANGLE_LIMITS.STEER_ANGLE_MAX)) if ccnc_angle_long else 0.0 if ccnc_angle_long and drive_gear: can_sends.append(hyundaicanfd.create_angle_adas_cmd( self.packer, self.CAN, - apply_angle if direct_steering_active else CS.angle_steering_angle, + apply_angle if direct_steering_active else inactive_steering_angle, direct_steering_active, apply_torque if direct_steering_active else 0.0, )) if ccnc_angle_long and not drive_gear: can_sends.extend(hyundaicanfd.create_inactive_angle_steering_messages(self.packer, self.CAN, - CS.angle_steering_angle)) + inactive_steering_angle)) # prevent LFA from activating on LKA steering cars by sending "no lane lines detected" to ADAS ECU suppress_lfa = bool(lka_steering) diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index e2a992746..5263aa9b6 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -461,7 +461,7 @@ class CarState(CarStateBase): ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5) ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0 if self.CP.carFingerprint in CANFD_ANGLE_LONGITUDINAL_CAR: - self.angle_steering_angle = cp.vl["MDPS"]["STEERING_ANGLE"] + self.angle_steering_angle = cp.vl["MDPS"]["STEERING_ANGLE_2"] self.angle_steering_fault = cp.vl["MDPS"]["LKA_ANGLE_FAULT"] != 0 ret.steerFaultTemporary = ret.steerFaultTemporary or self.angle_steering_fault diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index cba2c253f..f94ab23cf 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -14,7 +14,8 @@ 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, should_use_ev6_gt_line_stop_direct_tracking + direct_angle_request_allowed, get_angle_smoothing_alpha, \ + should_use_ev6_gt_line_stop_direct_tracking 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 @@ -299,6 +300,14 @@ 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_ev9_direct_angle_waits_for_safety_envelope(self): + CP = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], True, False, False, None) + controller = CarController(DBC[CP.carFingerprint], CP) + + assert not direct_angle_request_allowed(8.47, 155.5, 155.6, True, controller.BASELINE_VM, controller.params) + assert direct_angle_request_allowed(8.47, 140.0, 140.0, True, controller.BASELINE_VM, controller.params) + assert not direct_angle_request_allowed(8.47, 140.0, 140.0, False, controller.BASELINE_VM, controller.params) + def test_ev9_matches_other_angle_platform_standstill_behavior(self): ev9_cp = CarInterface.get_params(CAR.KIA_EV9, gen_empty_fingerprint(), [], False, False, False, None) sportage_cp = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, gen_empty_fingerprint(), [], False, False, False, None) diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index cb6633b4c..b81acdd8d 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -125,7 +125,9 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) { torque_driver_new -= 4095; update_sample(&torque_driver, torque_driver_new); - int angle_meas_new = (msg->data[13] << 8U) | msg->data[12]; + // CCNC angle-long platforms publish the usable angle in STEERING_ANGLE_2. + const unsigned int angle_offset = hyundai_canfd_ccnc_angle_long ? 16U : 12U; + int angle_meas_new = (msg->data[angle_offset + 1U] << 8U) | msg->data[angle_offset]; angle_meas_new = to_signed(angle_meas_new, 16); update_sample(&angle_meas, angle_meas_new); } diff --git a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py index 17264af0f..ef7d97027 100644 --- a/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py +++ b/opendbc_repo/opendbc/safety/tests/safety_replay/helpers.py @@ -23,9 +23,12 @@ def is_steering_msg(mode, param, addr): elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy): ret = addr == 832 elif mode == CarParams.SafetyModel.hyundaiCanfd: - ret = addr == (0x110 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT else - 0x50 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING else - 0x12A) + if param & HyundaiSafetyFlags.CCNC and param & HyundaiSafetyFlags.LONG and param & HyundaiSafetyFlags.CANFD_ANGLE_STEERING: + ret = addr == 0xCB + else: + ret = addr == (0x110 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT else + 0x50 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING else + 0x12A) elif mode == CarParams.SafetyModel.chrysler: ret = addr == 0x292 elif mode == CarParams.SafetyModel.subaru: @@ -62,7 +65,14 @@ def get_steer_value(mode, param, msg): elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy): torque = (((msg.data[3] & 0x7) << 8) | msg.data[2]) - 1024 elif mode == CarParams.SafetyModel.hyundaiCanfd: - torque = ((msg.data[5] >> 1) | (msg.data[6] & 0xF) << 7) - 1024 + if param & HyundaiSafetyFlags.CANFD_ANGLE_STEERING: + if param & HyundaiSafetyFlags.CCNC and param & HyundaiSafetyFlags.LONG: + angle = ((msg.data[5] & 0x3F) << 8) | msg.data[4] + else: + angle = (msg.data[11] << 6) | (msg.data[10] >> 2) + angle = to_signed(angle, 14) + else: + torque = ((msg.data[5] >> 1) | (msg.data[6] & 0xF) << 7) - 1024 elif mode == CarParams.SafetyModel.chrysler: torque = (((msg.data[0] & 0x7) << 8) | msg.data[1]) - 1024 elif mode == CarParams.SafetyModel.subaru: diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 8c6c31575..2177286a2 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -752,6 +752,21 @@ class TestHyundaiCanfdLKASteeringAltAngleLongEV(HyundaiLongitudinalBase, TestHyu with self.subTest(address=address): self.assertFalse(self._tx(common.make_msg(1 if address != 0x51 else 0, address, length))) + def test_ccnc_angle_long_uses_second_mdps_angle(self): + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, self.SAFETY_PARAM | HyundaiSafetyFlags.CCNC) + self.safety.init_tests() + + angle = -38.5 + for _ in range(common.MAX_SAMPLE_VALS): + self._rx(self.packer.make_can_msg_safety("MDPS", self.PT_BUS, { + "STEERING_ANGLE": 0.0, + "STEERING_ANGLE_2": angle, + })) + + expected = round(angle * self.DEG_TO_CAN) + self.assertEqual(self.safety.get_angle_meas_min(), expected) + self.assertEqual(self.safety.get_angle_meas_max(), expected) + def _accel_msg(self, accel, aeb_req=False, aeb_decel=0): values = { "aReqRaw": accel,