From 4d059357ee79f3ae23b3b6601fcb3b519fd93541 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Fri, 26 Jun 2026 18:06:21 +0900 Subject: [PATCH 01/13] Add Hyundai angle steering trim governor --- .../opendbc/car/hyundai/carcontroller.py | 29 ++++++++++++++++++- selfdrive/controls/lib/latcontrol_angle.py | 28 +++++++++++++++++- 2 files changed, 55 insertions(+), 2 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 9cbc78d1595..1a5c129caed 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -252,6 +252,33 @@ def update(self, CC, CS, now_nanos): if angle_control: apply_steer_req = CC.latActive + angle_torque_cap = self.angle_max_torque + if angle_control and CC.latActive: + desired_clip_error = abs(float(actuators.steeringAngleDeg) - apply_angle) + tracking_error = abs(apply_angle - CS.out.steeringAngleDeg) + near_angle_cap = abs(apply_angle) > self.params.ANGLE_LIMITS.STEER_ANGLE_MAX - 2.0 + low_speed_large_angle = CS.out.vEgo < 7.0 and abs(CS.out.steeringAngleDeg) > 70.0 + stalled_tracking = abs(CS.out.steeringRateDeg) < 8.0 and tracking_error > 8.0 + eps_loaded = abs(CS.out.steeringTorqueEps) > 18.0 + + if low_speed_large_angle and (near_angle_cap or stalled_tracking or eps_loaded): + clip_factor = float(np.interp(desired_clip_error, [0.0, 5.0, 20.0, 60.0], + [1.0, 0.75, 0.45, 0.25])) + tracking_factor = float(np.interp(tracking_error, [3.0, 8.0, 15.0, 30.0], + [1.0, 0.75, 0.55, 0.35])) + eps_factor = float(np.interp(abs(CS.out.steeringTorqueEps), [10.0, 18.0, 28.0], + [1.0, 0.75, 0.5])) + speed_factor = float(np.interp(CS.out.vEgo, [3.0, 7.0], + [0.65, 1.0])) + authority_factor = min(clip_factor, tracking_factor, eps_factor, speed_factor) + angle_torque_cap = max(self.params.ANGLE_MIN_TORQUE, + self.angle_max_torque * authority_factor) + + # If the applied angle is pinned while the model asks for much more, cut + # authority immediately instead of letting the EPS grind against the stop. + if near_angle_cap and desired_clip_error > 20.0: + angle_torque_cap = min(angle_torque_cap, self.params.ANGLE_MIN_TORQUE) + steering_pressed_rising = CS.out.steeringPressed and not self.steering_pressed_prev if steering_pressed_rising: if 0 < self.full_recovery_frames < int(5.0 / DT_CTRL): @@ -302,7 +329,7 @@ def update(self, CC, CS, now_nanos): # During recovery, taper the rate to zero. Only steeringPressed can reduce authority. torque_delta = base_rate_up * float(np.interp(torque_ratio, [0.6, 0.8], [1.0, 0.0])) self.lkas_max_torque = float(np.clip(self.lkas_max_torque + torque_delta, - self.params.ANGLE_MIN_TORQUE, self.angle_max_torque)) + self.params.ANGLE_MIN_TORQUE, angle_torque_cap)) if not CS.out.steeringPressed and self.recovering_from_override and self.lkas_max_torque >= self.angle_max_torque: self.recovering_from_override = False diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 3278867bb62..baa5e59b1f5 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -5,12 +5,22 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees +ANGLE_TRIM_MAX_CURVATURE = 4.0e-4 # about one steering-wheel degree on HKG LFA2 +ANGLE_TRIM_I = 0.10 +ANGLE_TRIM_BUILDUP_BOOST = 2.5 +ANGLE_TRIM_LEAK_RATE = 0.995 class LatControlAngle(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.sat_check_min_speed = 5. + self.curvature_trim = 0.0 + self.curvature_trim_enabled = CP.carName == "hyundai" + + def reset(self): + super().reset() + self.curvature_trim = 0.0 def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -18,10 +28,26 @@ def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curv if not active: angle_log.active = False angle_steers_des = float(CS.steeringAngleDeg) + self.curvature_trim = 0.0 else: angle_log.active = True - angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + corrected_curvature = float(desired_curvature) + if not self.curvature_trim_enabled or CS.steeringPressed or CS.vEgo < 0.3: + self.curvature_trim = 0.0 + else: + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + curvature_error = float(desired_curvature) - actual_curvature + buildup = curvature_error * float(desired_curvature) > 0.0 and abs(CS.steeringRateDeg) > 5.0 + trim_i = ANGLE_TRIM_I * (ANGLE_TRIM_BUILDUP_BOOST if buildup else 1.0) + self.curvature_trim += curvature_error * trim_i * 0.01 + self.curvature_trim *= ANGLE_TRIM_LEAK_RATE + self.curvature_trim = float(np.clip(self.curvature_trim, -ANGLE_TRIM_MAX_CURVATURE, ANGLE_TRIM_MAX_CURVATURE)) + corrected_curvature += self.curvature_trim + + angle_steers_des = math.degrees(VM.get_steer_from_curvature(-corrected_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg + if CS.steeringPressed: + angle_steers_des = float(CS.steeringAngleDeg) angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) From 5fe8f48e48baa89bcfcd940c28e64b7884805907 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Fri, 26 Jun 2026 18:19:55 +0900 Subject: [PATCH 02/13] Update Hyundai angle control flag logic for curvature trim enablement --- selfdrive/controls/lib/latcontrol_angle.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index baa5e59b1f5..3fd81871989 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -9,6 +9,7 @@ ANGLE_TRIM_I = 0.10 ANGLE_TRIM_BUILDUP_BOOST = 2.5 ANGLE_TRIM_LEAK_RATE = 0.995 +HYUNDAI_ANGLE_CONTROL_FLAG = 1 << 24 class LatControlAngle(LatControl): @@ -16,7 +17,7 @@ def __init__(self, CP, CI): super().__init__(CP, CI) self.sat_check_min_speed = 5. self.curvature_trim = 0.0 - self.curvature_trim_enabled = CP.carName == "hyundai" + self.curvature_trim_enabled = bool(CP.flags & HYUNDAI_ANGLE_CONTROL_FLAG) def reset(self): super().reset() From ac32527b67c73a90f48ee28dc6b1f784b8d03356 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Fri, 26 Jun 2026 18:40:15 +0900 Subject: [PATCH 03/13] Limit LFA2 torque on large-angle launches --- opendbc_repo/opendbc/car/hyundai/carcontroller.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 1a5c129caed..a900f73849a 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -260,6 +260,15 @@ def update(self, CC, CS, now_nanos): low_speed_large_angle = CS.out.vEgo < 7.0 and abs(CS.out.steeringAngleDeg) > 70.0 stalled_tracking = abs(CS.out.steeringRateDeg) < 8.0 and tracking_error > 8.0 eps_loaded = abs(CS.out.steeringTorqueEps) > 18.0 + launch_large_angle = CS.out.vEgo < 6.0 and abs(CS.out.steeringAngleDeg) > 30.0 + + if launch_large_angle: + launch_angle_cap = float(np.interp(abs(CS.out.steeringAngleDeg), [30.0, 70.0, 120.0], + [160.0, 100.0, 60.0])) + launch_speed_cap = float(np.interp(CS.out.vEgo, [0.0, 1.0, 3.0, 6.0], + [30.0, 50.0, 100.0, self.angle_max_torque])) + angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, + min(launch_angle_cap, launch_speed_cap))) if low_speed_large_angle and (near_angle_cap or stalled_tracking or eps_loaded): clip_factor = float(np.interp(desired_clip_error, [0.0, 5.0, 20.0, 60.0], From ba9225572219d5ec5713eacb7966e9829a620834 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Fri, 26 Jun 2026 18:53:18 +0900 Subject: [PATCH 04/13] Limit LFA2 torque during fast low-speed steering --- opendbc_repo/opendbc/car/hyundai/carcontroller.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index a900f73849a..edd6b74cccf 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -261,6 +261,7 @@ def update(self, CC, CS, now_nanos): stalled_tracking = abs(CS.out.steeringRateDeg) < 8.0 and tracking_error > 8.0 eps_loaded = abs(CS.out.steeringTorqueEps) > 18.0 launch_large_angle = CS.out.vEgo < 6.0 and abs(CS.out.steeringAngleDeg) > 30.0 + low_speed_fast_steer = CS.out.vEgo < 7.0 and abs(CS.out.steeringRateDeg) > 45.0 if launch_large_angle: launch_angle_cap = float(np.interp(abs(CS.out.steeringAngleDeg), [30.0, 70.0, 120.0], @@ -270,6 +271,14 @@ def update(self, CC, CS, now_nanos): angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, min(launch_angle_cap, launch_speed_cap))) + if low_speed_fast_steer: + rate_cap = float(np.interp(abs(CS.out.steeringRateDeg), [45.0, 90.0, 140.0], + [150.0, 90.0, 55.0])) + rate_speed_cap = float(np.interp(CS.out.vEgo, [0.0, 2.0, 7.0], + [45.0, 80.0, self.angle_max_torque])) + angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, + min(rate_cap, rate_speed_cap))) + if low_speed_large_angle and (near_angle_cap or stalled_tracking or eps_loaded): clip_factor = float(np.interp(desired_clip_error, [0.0, 5.0, 20.0, 60.0], [1.0, 0.75, 0.45, 0.25])) From 34bafb229661248f1b52e27afa70fa44443717d6 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Fri, 26 Jun 2026 19:59:56 +0900 Subject: [PATCH 05/13] Yield LFA2 torque after driver low-speed turns --- .../opendbc/car/hyundai/carcontroller.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index edd6b74cccf..24bc5a96e0d 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -153,6 +153,7 @@ def __init__(self, dbc_names, CP): self.repeated_override_count = 0 self.override_latched = False self.override_release_frames = 0 + self.driver_unwind_frames = 0 self.lkas11_active = False @@ -262,6 +263,7 @@ def update(self, CC, CS, now_nanos): eps_loaded = abs(CS.out.steeringTorqueEps) > 18.0 launch_large_angle = CS.out.vEgo < 6.0 and abs(CS.out.steeringAngleDeg) > 30.0 low_speed_fast_steer = CS.out.vEgo < 7.0 and abs(CS.out.steeringRateDeg) > 45.0 + post_driver_unwind = self.driver_unwind_frames > 0 and CS.out.vEgo < 10.0 and abs(CS.out.steeringAngleDeg) > 25.0 if launch_large_angle: launch_angle_cap = float(np.interp(abs(CS.out.steeringAngleDeg), [30.0, 70.0, 120.0], @@ -279,6 +281,14 @@ def update(self, CC, CS, now_nanos): angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, min(rate_cap, rate_speed_cap))) + if post_driver_unwind: + unwind_angle_cap = float(np.interp(abs(CS.out.steeringAngleDeg), [25.0, 70.0, 120.0], + [80.0, 50.0, 35.0])) + unwind_speed_cap = float(np.interp(CS.out.vEgo, [0.0, 4.0, 10.0], + [35.0, 55.0, 120.0])) + angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, + min(unwind_angle_cap, unwind_speed_cap))) + if low_speed_large_angle and (near_angle_cap or stalled_tracking or eps_loaded): clip_factor = float(np.interp(desired_clip_error, [0.0, 5.0, 20.0, 60.0], [1.0, 0.75, 0.45, 0.25])) @@ -297,6 +307,11 @@ def update(self, CC, CS, now_nanos): if near_angle_cap and desired_clip_error > 20.0: angle_torque_cap = min(angle_torque_cap, self.params.ANGLE_MIN_TORQUE) + if CS.out.steeringPressed and angle_control and CC.latActive: + self.driver_unwind_frames = int(1.5 / DT_CTRL) + elif self.driver_unwind_frames > 0: + self.driver_unwind_frames -= 1 + steering_pressed_rising = CS.out.steeringPressed and not self.steering_pressed_prev if steering_pressed_rising: if 0 < self.full_recovery_frames < int(5.0 / DT_CTRL): @@ -366,6 +381,7 @@ def update(self, CC, CS, now_nanos): self.repeated_override_count = 0 self.override_latched = False self.override_release_frames = 0 + self.driver_unwind_frames = 0 self.steering_pressed_prev = CS.out.steeringPressed if CC.latActive else False From 5f5ca95d68d6ca70510d1745718b625d63a429a7 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Sun, 28 Jun 2026 12:23:46 +0900 Subject: [PATCH 06/13] Smooth Hyundai angle torque limit transitions --- .../opendbc/car/hyundai/carcontroller.py | 46 +++++++++++++------ 1 file changed, 33 insertions(+), 13 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 24bc5a96e0d..a931b14262e 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -261,33 +261,54 @@ def update(self, CC, CS, now_nanos): low_speed_large_angle = CS.out.vEgo < 7.0 and abs(CS.out.steeringAngleDeg) > 70.0 stalled_tracking = abs(CS.out.steeringRateDeg) < 8.0 and tracking_error > 8.0 eps_loaded = abs(CS.out.steeringTorqueEps) > 18.0 - launch_large_angle = CS.out.vEgo < 6.0 and abs(CS.out.steeringAngleDeg) > 30.0 - low_speed_fast_steer = CS.out.vEgo < 7.0 and abs(CS.out.steeringRateDeg) > 45.0 - post_driver_unwind = self.driver_unwind_frames > 0 and CS.out.vEgo < 10.0 and abs(CS.out.steeringAngleDeg) > 25.0 + launch_large_angle = CS.out.vEgo < 7.0 and abs(CS.out.steeringAngleDeg) > 20.0 + low_speed_fast_steer = CS.out.vEgo < 8.0 and abs(CS.out.steeringRateDeg) > 35.0 + post_driver_unwind = self.driver_unwind_frames > 0 and CS.out.vEgo < 12.0 and abs(CS.out.steeringAngleDeg) > 15.0 if launch_large_angle: - launch_angle_cap = float(np.interp(abs(CS.out.steeringAngleDeg), [30.0, 70.0, 120.0], - [160.0, 100.0, 60.0])) + launch_angle = abs(CS.out.steeringAngleDeg) + launch_angle_cap = float(np.interp(launch_angle, [20.0, 30.0, 70.0, 120.0], + [self.angle_max_torque, 160.0, 100.0, 60.0])) launch_speed_cap = float(np.interp(CS.out.vEgo, [0.0, 1.0, 3.0, 6.0], [30.0, 50.0, 100.0, self.angle_max_torque])) + launch_target_cap = min(launch_angle_cap, launch_speed_cap) + launch_angle_blend = float(np.interp(launch_angle, [20.0, 30.0], [0.0, 1.0])) + launch_cap = float(np.interp(launch_angle_blend, [0.0, 1.0], + [self.angle_max_torque, launch_target_cap])) + launch_cap = float(np.interp(CS.out.vEgo, [6.0, 7.0], + [launch_cap, self.angle_max_torque])) angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, - min(launch_angle_cap, launch_speed_cap))) + launch_cap)) if low_speed_fast_steer: - rate_cap = float(np.interp(abs(CS.out.steeringRateDeg), [45.0, 90.0, 140.0], - [150.0, 90.0, 55.0])) + steering_rate = abs(CS.out.steeringRateDeg) + rate_cap = float(np.interp(steering_rate, [35.0, 45.0, 90.0, 140.0], + [self.angle_max_torque, 150.0, 90.0, 55.0])) rate_speed_cap = float(np.interp(CS.out.vEgo, [0.0, 2.0, 7.0], [45.0, 80.0, self.angle_max_torque])) + rate_target_cap = min(rate_cap, rate_speed_cap) + rate_blend = float(np.interp(steering_rate, [35.0, 45.0], [0.0, 1.0])) + fast_steer_cap = float(np.interp(rate_blend, [0.0, 1.0], + [self.angle_max_torque, rate_target_cap])) + fast_steer_cap = float(np.interp(CS.out.vEgo, [7.0, 8.0], + [fast_steer_cap, self.angle_max_torque])) angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, - min(rate_cap, rate_speed_cap))) + fast_steer_cap)) if post_driver_unwind: - unwind_angle_cap = float(np.interp(abs(CS.out.steeringAngleDeg), [25.0, 70.0, 120.0], - [80.0, 50.0, 35.0])) + unwind_angle = abs(CS.out.steeringAngleDeg) + unwind_angle_cap = float(np.interp(unwind_angle, [15.0, 25.0, 70.0, 120.0], + [self.angle_max_torque, 80.0, 50.0, 35.0])) unwind_speed_cap = float(np.interp(CS.out.vEgo, [0.0, 4.0, 10.0], [35.0, 55.0, 120.0])) + unwind_target_cap = min(unwind_angle_cap, unwind_speed_cap) + unwind_angle_blend = float(np.interp(unwind_angle, [15.0, 25.0], [0.0, 1.0])) + unwind_cap = float(np.interp(unwind_angle_blend, [0.0, 1.0], + [self.angle_max_torque, unwind_target_cap])) + unwind_cap = float(np.interp(CS.out.vEgo, [10.0, 12.0], + [unwind_cap, self.angle_max_torque])) angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, - min(unwind_angle_cap, unwind_speed_cap))) + unwind_cap)) if low_speed_large_angle and (near_angle_cap or stalled_tracking or eps_loaded): clip_factor = float(np.interp(desired_clip_error, [0.0, 5.0, 20.0, 60.0], @@ -768,4 +789,3 @@ def make_jerk(self, CP, CS, accel, actuators, hud_control): self.jerk_l = min(max(1.0, -self.jerk * 4.0), jerk_max_l) self.cb_upper = np.clip(0.9 + accel * 0.2, 0, 1.2) self.cb_lower = np.clip(0.8 + accel * 0.2, 0, 1.2) - From 28cf1db83f3f7ca5ec46e53bdf8e8cb33f8741e2 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Mon, 29 Jun 2026 08:02:54 +0900 Subject: [PATCH 07/13] Balance low-speed angle authority and EPS load --- .../opendbc/car/hyundai/carcontroller.py | 34 ++++++++++++++----- 1 file changed, 26 insertions(+), 8 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index a931b14262e..7ac63ce78dc 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -258,9 +258,12 @@ def update(self, CC, CS, now_nanos): desired_clip_error = abs(float(actuators.steeringAngleDeg) - apply_angle) tracking_error = abs(apply_angle - CS.out.steeringAngleDeg) near_angle_cap = abs(apply_angle) > self.params.ANGLE_LIMITS.STEER_ANGLE_MAX - 2.0 - low_speed_large_angle = CS.out.vEgo < 7.0 and abs(CS.out.steeringAngleDeg) > 70.0 + steering_angle_abs = abs(CS.out.steeringAngleDeg) + low_speed_large_angle = CS.out.vEgo < 8.0 and steering_angle_abs > 60.0 stalled_tracking = abs(CS.out.steeringRateDeg) < 8.0 and tracking_error > 8.0 - eps_loaded = abs(CS.out.steeringTorqueEps) > 18.0 + eps_torque_abs = abs(CS.out.steeringTorqueEps) + eps_elevated = eps_torque_abs > 14.0 + eps_loaded = eps_torque_abs > 18.0 launch_large_angle = CS.out.vEgo < 7.0 and abs(CS.out.steeringAngleDeg) > 20.0 low_speed_fast_steer = CS.out.vEgo < 8.0 and abs(CS.out.steeringRateDeg) > 35.0 post_driver_unwind = self.driver_unwind_frames > 0 and CS.out.vEgo < 12.0 and abs(CS.out.steeringAngleDeg) > 15.0 @@ -310,18 +313,33 @@ def update(self, CC, CS, now_nanos): angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, unwind_cap)) - if low_speed_large_angle and (near_angle_cap or stalled_tracking or eps_loaded): + if low_speed_large_angle and (near_angle_cap or eps_elevated): clip_factor = float(np.interp(desired_clip_error, [0.0, 5.0, 20.0, 60.0], [1.0, 0.75, 0.45, 0.25])) - tracking_factor = float(np.interp(tracking_error, [3.0, 8.0, 15.0, 30.0], - [1.0, 0.75, 0.55, 0.35])) - eps_factor = float(np.interp(abs(CS.out.steeringTorqueEps), [10.0, 18.0, 28.0], + tracking_factor = 1.0 + if eps_loaded and stalled_tracking: + tracking_factor = float(np.interp(tracking_error, [3.0, 8.0, 15.0, 30.0], + [1.0, 0.75, 0.55, 0.35])) + eps_factor = float(np.interp(eps_torque_abs, [14.0, 18.0, 28.0], [1.0, 0.75, 0.5])) speed_factor = float(np.interp(CS.out.vEgo, [3.0, 7.0], [0.65, 1.0])) authority_factor = min(clip_factor, tracking_factor, eps_factor, speed_factor) - angle_torque_cap = max(self.params.ANGLE_MIN_TORQUE, - self.angle_max_torque * authority_factor) + angle_protection_blend = float(np.interp(steering_angle_abs, [60.0, 70.0], [0.0, 1.0])) + speed_protection_blend = float(np.interp(CS.out.vEgo, [7.0, 8.0], [1.0, 0.0])) + protection_blend = angle_protection_blend * speed_protection_blend + authority_factor = 1.0 - (1.0 - authority_factor) * protection_blend + angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, + self.angle_max_torque * authority_factor)) + + driver_torque_quiet = abs(CS.out.steeringTorque) < self.params.STEER_THRESHOLD * 0.6 + settled_tracking_stall = (self.driver_unwind_frames == 0 and not CS.out.steeringPressed and driver_torque_quiet and + CS.out.vEgo < 7.0 and abs(apply_angle) > 70.0 and stalled_tracking and not near_angle_cap) + if settled_tracking_stall and not eps_loaded: + tracking_need = float(np.interp(tracking_error, [20.0, 60.0], [0.0, 1.0])) + eps_headroom = float(np.interp(eps_torque_abs, [14.0, 18.0], [1.0, 0.0])) + angle_torque_cap = min(self.angle_max_torque, + angle_torque_cap + 30.0 * tracking_need * eps_headroom) # If the applied angle is pinned while the model asks for much more, cut # authority immediately instead of letting the EPS grind against the stop. From d5a419dd187f0626a361268f7068bf4b981fb4d9 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Mon, 29 Jun 2026 14:26:53 +0900 Subject: [PATCH 08/13] Add AngleControlBlend parameter for low-speed EPS protection and tracking correction --- common/params_keys.h | 1 + .../opendbc/car/hyundai/carcontroller.py | 41 +++++++++++-------- selfdrive/carrot_settings.json | 19 ++++++++- selfdrive/controls/lib/latcontrol_angle.py | 12 ++++-- 4 files changed, 52 insertions(+), 21 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index bc4bd8ffe17..09733f70835 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -275,6 +275,7 @@ inline static std::unordered_map keys = { {"SoftwareMenu", {PERSISTENT, INT, "1"}}, {"HyundaiCameraSCC", {PERSISTENT, INT, "0"}}, + {"AngleControlBlend", {PERSISTENT, INT, "0"}}, {"FingerPrints", {PERSISTENT | CLEAR_ON_MANAGER_START, STRING}}, {"IsLdwsCar", {PERSISTENT, INT, "0"}}, {"CanfdHDA2", {PERSISTENT, INT, "0"}}, diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 7ac63ce78dc..4e20bbe9671 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -164,6 +164,7 @@ def __init__(self, dbc_names, CP): self.activeCarrot = 0 self.camera_scc_params = Params().get_int("HyundaiCameraSCC") self.is_ldws_car = Params().get_bool("IsLdwsCar") + self.angle_control_level = int(np.clip(Params().get_int("AngleControlBlend"), 0, 100)) self.enable_corner_radar = 0 self.steerDeltaUpOrg = self.steerDeltaUp = self.steerDeltaUpLC = self.params.STEER_DELTA_UP @@ -253,21 +254,22 @@ def update(self, CC, CS, now_nanos): if angle_control: apply_steer_req = CC.latActive + angle_policy_level = self.angle_control_level if angle_control else 0 angle_torque_cap = self.angle_max_torque - if angle_control and CC.latActive: + if angle_control and CC.latActive and angle_policy_level > 0: desired_clip_error = abs(float(actuators.steeringAngleDeg) - apply_angle) tracking_error = abs(apply_angle - CS.out.steeringAngleDeg) near_angle_cap = abs(apply_angle) > self.params.ANGLE_LIMITS.STEER_ANGLE_MAX - 2.0 steering_angle_abs = abs(CS.out.steeringAngleDeg) - low_speed_large_angle = CS.out.vEgo < 8.0 and steering_angle_abs > 60.0 stalled_tracking = abs(CS.out.steeringRateDeg) < 8.0 and tracking_error > 8.0 eps_torque_abs = abs(CS.out.steeringTorqueEps) - eps_elevated = eps_torque_abs > 14.0 eps_loaded = eps_torque_abs > 18.0 launch_large_angle = CS.out.vEgo < 7.0 and abs(CS.out.steeringAngleDeg) > 20.0 low_speed_fast_steer = CS.out.vEgo < 8.0 and abs(CS.out.steeringRateDeg) > 35.0 post_driver_unwind = self.driver_unwind_frames > 0 and CS.out.vEgo < 12.0 and abs(CS.out.steeringAngleDeg) > 15.0 + common_torque_cap = self.angle_max_torque + if launch_large_angle: launch_angle = abs(CS.out.steeringAngleDeg) launch_angle_cap = float(np.interp(launch_angle, [20.0, 30.0, 70.0, 120.0], @@ -280,8 +282,8 @@ def update(self, CC, CS, now_nanos): [self.angle_max_torque, launch_target_cap])) launch_cap = float(np.interp(CS.out.vEgo, [6.0, 7.0], [launch_cap, self.angle_max_torque])) - angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, - launch_cap)) + common_torque_cap = min(common_torque_cap, max(self.params.ANGLE_MIN_TORQUE, + launch_cap)) if low_speed_fast_steer: steering_rate = abs(CS.out.steeringRateDeg) @@ -295,8 +297,8 @@ def update(self, CC, CS, now_nanos): [self.angle_max_torque, rate_target_cap])) fast_steer_cap = float(np.interp(CS.out.vEgo, [7.0, 8.0], [fast_steer_cap, self.angle_max_torque])) - angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, - fast_steer_cap)) + common_torque_cap = min(common_torque_cap, max(self.params.ANGLE_MIN_TORQUE, + fast_steer_cap)) if post_driver_unwind: unwind_angle = abs(CS.out.steeringAngleDeg) @@ -310,10 +312,13 @@ def update(self, CC, CS, now_nanos): [self.angle_max_torque, unwind_target_cap])) unwind_cap = float(np.interp(CS.out.vEgo, [10.0, 12.0], [unwind_cap, self.angle_max_torque])) - angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, - unwind_cap)) + common_torque_cap = min(common_torque_cap, max(self.params.ANGLE_MIN_TORQUE, + unwind_cap)) - if low_speed_large_angle and (near_angle_cap or eps_elevated): + angle_torque_cap_100 = common_torque_cap + low_speed_large_angle_100 = CS.out.vEgo < 8.0 and steering_angle_abs > 60.0 + eps_elevated = eps_torque_abs > 14.0 + if low_speed_large_angle_100 and (near_angle_cap or eps_elevated): clip_factor = float(np.interp(desired_clip_error, [0.0, 5.0, 20.0, 60.0], [1.0, 0.75, 0.45, 0.25])) tracking_factor = 1.0 @@ -329,8 +334,8 @@ def update(self, CC, CS, now_nanos): speed_protection_blend = float(np.interp(CS.out.vEgo, [7.0, 8.0], [1.0, 0.0])) protection_blend = angle_protection_blend * speed_protection_blend authority_factor = 1.0 - (1.0 - authority_factor) * protection_blend - angle_torque_cap = min(angle_torque_cap, max(self.params.ANGLE_MIN_TORQUE, - self.angle_max_torque * authority_factor)) + angle_torque_cap_100 = min(angle_torque_cap_100, max(self.params.ANGLE_MIN_TORQUE, + self.angle_max_torque * authority_factor)) driver_torque_quiet = abs(CS.out.steeringTorque) < self.params.STEER_THRESHOLD * 0.6 settled_tracking_stall = (self.driver_unwind_frames == 0 and not CS.out.steeringPressed and driver_torque_quiet and @@ -338,15 +343,19 @@ def update(self, CC, CS, now_nanos): if settled_tracking_stall and not eps_loaded: tracking_need = float(np.interp(tracking_error, [20.0, 60.0], [0.0, 1.0])) eps_headroom = float(np.interp(eps_torque_abs, [14.0, 18.0], [1.0, 0.0])) - angle_torque_cap = min(self.angle_max_torque, - angle_torque_cap + 30.0 * tracking_need * eps_headroom) + angle_torque_cap_100 = min(self.angle_max_torque, + angle_torque_cap_100 + 30.0 * tracking_need * eps_headroom) # If the applied angle is pinned while the model asks for much more, cut # authority immediately instead of letting the EPS grind against the stop. if near_angle_cap and desired_clip_error > 20.0: - angle_torque_cap = min(angle_torque_cap, self.params.ANGLE_MIN_TORQUE) + angle_torque_cap_100 = min(angle_torque_cap_100, self.params.ANGLE_MIN_TORQUE) + + angle_policy_blend = angle_policy_level / 100.0 + angle_torque_cap = float(np.interp(angle_policy_blend, [0.0, 1.0], + [self.angle_max_torque, angle_torque_cap_100])) - if CS.out.steeringPressed and angle_control and CC.latActive: + if CS.out.steeringPressed and angle_control and CC.latActive and angle_policy_level > 0: self.driver_unwind_frames = int(1.5 / DT_CTRL) elif self.driver_unwind_frames > 0: self.driver_unwind_frames -= 1 diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index dd43f399604..0576a8a2dfd 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -187,7 +187,8 @@ "CustomSteerDeltaUp", "CustomSteerDeltaDown", "CustomSteerDeltaUpLC", - "CustomSteerDeltaDownLC" + "CustomSteerDeltaDownLC", + "AngleControlBlend" ] } ] @@ -897,6 +898,22 @@ "default": 0, "unit": 1 }, + { + "group": "조향튜닝", + "name": "AngleControlBlend", + "title": "앵글조향 보정 적용률(0%)", + "descr": "angle steering 차량의 저속 EPS 보호와 추종 보정 적용률\n0: 최소\n100: 최대\n재시작 적용", + "egroup": "LAT", + "etitle": "Angle Control Blend(0%)", + "edescr": "Low-speed EPS protection and tracking correction for angle steering\n0: Min\n100: Max\nChanges apply on the next controls process start", + "cgroup": "转向", + "ctitle": "角度转向校正比例(0%)", + "cdescr": "角度转向车辆的低速 EPS 保护和跟踪校正比例\n0: Min\n100: Max\n中间值按比例混合两种状态\n更改将在下次控制进程启动时生效", + "min": 0, + "max": 100, + "default": 0, + "unit": 5 + }, { "group": "조향튜닝", "name": "SteerActuatorDelay", diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 3fd81871989..e63e56eca3c 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -2,6 +2,7 @@ import numpy as np from cereal import log +from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.latcontrol import LatControl STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees @@ -18,6 +19,8 @@ def __init__(self, CP, CI): self.sat_check_min_speed = 5. self.curvature_trim = 0.0 self.curvature_trim_enabled = bool(CP.flags & HYUNDAI_ANGLE_CONTROL_FLAG) + angle_control_level = int(np.clip(Params().get_int("AngleControlBlend"), 0, 100)) + self.angle_policy_blend = angle_control_level / 100.0 if self.curvature_trim_enabled else 0.0 def reset(self): super().reset() @@ -33,7 +36,7 @@ def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curv else: angle_log.active = True corrected_curvature = float(desired_curvature) - if not self.curvature_trim_enabled or CS.steeringPressed or CS.vEgo < 0.3: + if not self.curvature_trim_enabled or self.angle_policy_blend <= 0.0 or CS.steeringPressed or CS.vEgo < 0.3: self.curvature_trim = 0.0 else: actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) @@ -43,12 +46,13 @@ def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curv self.curvature_trim += curvature_error * trim_i * 0.01 self.curvature_trim *= ANGLE_TRIM_LEAK_RATE self.curvature_trim = float(np.clip(self.curvature_trim, -ANGLE_TRIM_MAX_CURVATURE, ANGLE_TRIM_MAX_CURVATURE)) - corrected_curvature += self.curvature_trim + corrected_curvature += self.curvature_trim * self.angle_policy_blend angle_steers_des = math.degrees(VM.get_steer_from_curvature(-corrected_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - if CS.steeringPressed: - angle_steers_des = float(CS.steeringAngleDeg) + if CS.steeringPressed and self.angle_policy_blend > 0.0: + angle_steers_des = float(np.interp(self.angle_policy_blend, [0.0, 1.0], + [angle_steers_des, CS.steeringAngleDeg])) angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) From 02c660fa08433ee57b43e807cd5fba4f64a31523 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Wed, 1 Jul 2026 07:52:56 +0900 Subject: [PATCH 09/13] Move Hyundai angle tracking trim to car controller --- .../opendbc/car/hyundai/carcontroller.py | 42 ++++++++++++++++--- selfdrive/controls/lib/latcontrol_angle.py | 33 +-------------- 2 files changed, 38 insertions(+), 37 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index e18bbad4c48..c34e12e7d41 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -1,3 +1,4 @@ +import math import numpy as np from opendbc.can import CANPacker from opendbc.car import Bus, DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg, structs, apply_std_steer_angle_limits @@ -7,7 +8,6 @@ from opendbc.car.hyundai.hyundaicanfd import CanBus from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CAR, CAN_GEARS, HyundaiExtFlags from opendbc.car.interfaces import CarControllerBase -from opendbc.car.vehicle_model import VehicleModel VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState @@ -29,6 +29,12 @@ PRE_OVERRIDE_MIN_RATE_RATIO = 0.50 PRE_OVERRIDE_CONFIRM_FRAMES = 2 PRE_OVERRIDE_MAX_TORQUE_DELTA = -10.0 +ANGLE_TRACKING_TRIM_MAX_CURVATURE = 4.0e-4 +# CarController has the nominal ratio only; retain headroom for the higher live ratio used by lateral control. +ANGLE_TRACKING_TRIM_STEER_RATIO_MARGIN = 1.15 +ANGLE_TRACKING_TRIM_I = 0.001 +ANGLE_TRACKING_TRIM_BUILDUP_BOOST = 2.5 +ANGLE_TRACKING_TRIM_LEAK_RATE = 0.995 vibrate_intervals = [ (0.0, 0.5), @@ -166,6 +172,11 @@ def __init__(self, dbc_names, CP): self.driver_torque_filtered = 0.0 self.driver_torque_filtered_prev = 0.0 self.pre_override_frames = 0 + self.angle_tracking_trim = 0.0 + self.angle_tracking_trim_max = float(np.clip( + math.degrees(CP.wheelbase * CP.steerRatio * ANGLE_TRACKING_TRIM_MAX_CURVATURE) * ANGLE_TRACKING_TRIM_STEER_RATIO_MARGIN, + 0.8, 1.5, + )) self.lkas11_active = False @@ -237,6 +248,8 @@ def update(self, CC, CS, now_nanos): self.params.STEER_DELTA_DOWN = self.steerDeltaDown angle_control = self.CP.flags & HyundaiFlags.ANGLE_CONTROL + angle_policy_level = self.angle_control_level if angle_control else 0 + angle_policy_blend = angle_policy_level / 100.0 # steering torque new_torque = int(round(actuators.torque * self.params.STEER_MAX)) @@ -250,8 +263,29 @@ def update(self, CC, CS, now_nanos): #apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, # CS.out.steeringAngleDeg, CC.latActive, self.params.ANGLE_LIMITS) + desired_angle = float(actuators.steeringAngleDeg) + if angle_control and CC.latActive and angle_policy_level > 0: + if CS.out.steeringPressed: + self.angle_tracking_trim = 0.0 + desired_angle = float(np.interp(angle_policy_blend, [0.0, 1.0], + [desired_angle, CS.out.steeringAngleDeg])) + elif CS.out.vEgo >= 0.3: + tracking_error_signed = desired_angle - CS.out.steeringAngleDeg + buildup = tracking_error_signed * desired_angle > 0.0 and abs(CS.out.steeringRateDeg) > 5.0 + trim_i = ANGLE_TRACKING_TRIM_I * (ANGLE_TRACKING_TRIM_BUILDUP_BOOST if buildup else 1.0) + self.angle_tracking_trim += tracking_error_signed * trim_i + self.angle_tracking_trim *= ANGLE_TRACKING_TRIM_LEAK_RATE + self.angle_tracking_trim = float(np.clip(self.angle_tracking_trim, + -self.angle_tracking_trim_max, + self.angle_tracking_trim_max)) + desired_angle += self.angle_tracking_trim * angle_policy_blend + else: + self.angle_tracking_trim = 0.0 + else: + self.angle_tracking_trim = 0.0 + apply_angle = apply_steer_angle_limits_physics( - actuators.steeringAngleDeg, + desired_angle, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg, @@ -266,10 +300,9 @@ def update(self, CC, CS, now_nanos): if angle_control: apply_steer_req = CC.latActive - angle_policy_level = self.angle_control_level if angle_control else 0 angle_torque_cap = self.angle_max_torque if angle_control and CC.latActive and angle_policy_level > 0: - desired_clip_error = abs(float(actuators.steeringAngleDeg) - apply_angle) + desired_clip_error = abs(desired_angle - apply_angle) tracking_error = abs(apply_angle - CS.out.steeringAngleDeg) near_angle_cap = abs(apply_angle) > self.params.ANGLE_LIMITS.STEER_ANGLE_MAX - 2.0 steering_angle_abs = abs(CS.out.steeringAngleDeg) @@ -363,7 +396,6 @@ def update(self, CC, CS, now_nanos): if near_angle_cap and desired_clip_error > 20.0: angle_torque_cap_100 = min(angle_torque_cap_100, self.params.ANGLE_MIN_TORQUE) - angle_policy_blend = angle_policy_level / 100.0 angle_torque_cap = float(np.interp(angle_policy_blend, [0.0, 1.0], [self.angle_max_torque, angle_torque_cap_100])) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index a30985d227e..ba566047c56 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -2,29 +2,15 @@ import numpy as np from cereal import log -from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.latcontrol import LatControl STEER_ANGLE_SATURATION_THRESHOLD = 3.0 # Degrees -ANGLE_TRIM_MAX_CURVATURE = 4.0e-4 # about one steering-wheel degree on HKG LFA2 -ANGLE_TRIM_I = 0.10 -ANGLE_TRIM_BUILDUP_BOOST = 2.5 -ANGLE_TRIM_LEAK_RATE = 0.995 -HYUNDAI_ANGLE_CONTROL_FLAG = 1 << 24 class LatControlAngle(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.sat_check_min_speed = 5. - self.curvature_trim = 0.0 - self.curvature_trim_enabled = bool(CP.flags & HYUNDAI_ANGLE_CONTROL_FLAG) - angle_control_level = int(np.clip(Params().get_int("AngleControlBlend"), 0, 100)) - self.angle_policy_blend = angle_control_level / 100.0 if self.curvature_trim_enabled else 0.0 - - def reset(self): - super().reset() - self.curvature_trim = 0.0 def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -32,27 +18,10 @@ def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curv if not active: angle_log.active = False angle_steers_des = float(CS.steeringAngleDeg) - self.curvature_trim = 0.0 else: angle_log.active = True - corrected_curvature = float(desired_curvature) - if not self.curvature_trim_enabled or self.angle_policy_blend <= 0.0 or CS.steeringPressed or CS.vEgo < 0.3: - self.curvature_trim = 0.0 - else: - actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) - curvature_error = float(desired_curvature) - actual_curvature - buildup = curvature_error * float(desired_curvature) > 0.0 and abs(CS.steeringRateDeg) > 5.0 - trim_i = ANGLE_TRIM_I * (ANGLE_TRIM_BUILDUP_BOOST if buildup else 1.0) - self.curvature_trim += curvature_error * trim_i * 0.01 - self.curvature_trim *= ANGLE_TRIM_LEAK_RATE - self.curvature_trim = float(np.clip(self.curvature_trim, -ANGLE_TRIM_MAX_CURVATURE, ANGLE_TRIM_MAX_CURVATURE)) - corrected_curvature += self.curvature_trim * self.angle_policy_blend - - angle_steers_des = math.degrees(VM.get_steer_from_curvature(-corrected_curvature, CS.vEgo, params.roll)) + angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - if CS.steeringPressed and self.angle_policy_blend > 0.0: - angle_steers_des = float(np.interp(self.angle_policy_blend, [0.0, 1.0], - [angle_steers_des, CS.steeringAngleDeg])) angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) From 3fb37da86ff8cdf1cb3e42e20738f0f319451512 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Wed, 1 Jul 2026 08:17:24 +0900 Subject: [PATCH 10/13] Apply angle blend live and align LCA icons --- .../opendbc/car/hyundai/carcontroller.py | 21 ++++++++++--------- .../opendbc/car/hyundai/hyundaicanfd.py | 4 ++-- selfdrive/carrot_settings.json | 6 +++--- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index c34e12e7d41..79b612cc856 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -127,7 +127,7 @@ def apply_steer_angle_limits_physics(desired_sw_deg: float, cmd_sw = cmd_rw * steer_ratio return float(np.clip(cmd_sw, -steer_sw_max_deg, steer_sw_max_deg)) - + class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) @@ -187,7 +187,7 @@ def __init__(self, dbc_names, CP): self.activeCarrot = 0 self.camera_scc_params = Params().get_int("HyundaiCameraSCC") self.is_ldws_car = Params().get_bool("IsLdwsCar") - self.angle_control_level = int(np.clip(Params().get_int("AngleControlBlend"), 0, 100)) + self.angle_control_level = 0 self.enable_corner_radar = 0 self.steerDeltaUpOrg = self.steerDeltaUp = self.steerDeltaUpLC = self.params.STEER_DELTA_UP @@ -197,6 +197,7 @@ def update(self, CC, CS, now_nanos): if self.frame % 50 == 0: params = Params() + self.angle_control_level = int(np.clip(params.get_int("AngleControlBlend"), 0, 100)) self.max_angle_frames = params.get_int("MaxAngleFrames") steerMax = params.get_int("CustomSteerMax") steerDeltaUp = params.get_int("CustomSteerDeltaUp") @@ -224,7 +225,7 @@ def update(self, CC, CS, now_nanos): self.steerDeltaDownLC = steerDeltaDownLC else: self.steerDeltaDownLC = self.steerDeltaDown - + self.soft_hold_mode = 1 if params.get_int("AutoCruiseControl") > 1 else 2 self.hapticFeedbackWhenSpeedCamera = int(params.get_int("HapticFeedbackWhenSpeedCamera")) @@ -246,7 +247,7 @@ def update(self, CC, CS, now_nanos): else: self.params.STEER_DELTA_UP = self.steerDeltaUp self.params.STEER_DELTA_DOWN = self.steerDeltaDown - + angle_control = self.CP.flags & HyundaiFlags.ANGLE_CONTROL angle_policy_level = self.angle_control_level if angle_control else 0 angle_policy_blend = angle_policy_level / 100.0 @@ -260,7 +261,7 @@ def update(self, CC, CS, now_nanos): self.angle_limit_counter, self.max_angle_frames, MAX_ANGLE_CONSECUTIVE_FRAMES) - #apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, + #apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, # CS.out.steeringAngleDeg, CC.latActive, self.params.ANGLE_LIMITS) desired_angle = float(actuators.steeringAngleDeg) @@ -296,7 +297,7 @@ def update(self, CC, CS, now_nanos): CS.modelV2, ) - + if angle_control: apply_steer_req = CC.latActive @@ -538,7 +539,7 @@ def update(self, CC, CS, now_nanos): active_speed_decel = hud_control.activeCarrot == 3 and self.activeCarrot != 3 # 3: Speed Decel self.activeCarrot = hud_control.activeCarrot - if active_speed_decel and self.speedCameraHapticEndFrame < 0: # 과속카메라 감속시작 + if active_speed_decel and self.speedCameraHapticEndFrame < 0: # 과속카메라 감속시작 self.speedCameraHapticEndFrame = self.frame + (8.0 / DT_CTRL) #8초간 켜줌. elif not active_speed_decel: self.speedCameraHapticEndFrame = -1 @@ -587,7 +588,7 @@ def update(self, CC, CS, now_nanos): can_sends.extend(hyundaicanfd.create_steering_messages_camera_scc(self.frame, self.packer, self.CP, self.CAN, CC, apply_steer_req, apply_torque, CS, apply_angle, self.lkas_max_torque, angle_control)) else: can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, apply_angle, self.lkas_max_torque, angle_control)) - + # prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU if self.frame % 5 == 0 and hda2 and not camera_scc: can_sends.extend(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS)) @@ -653,7 +654,7 @@ def update(self, CC, CS, now_nanos): #jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value if camera_scc: - + can_sends.extend(hyundaican.create_acc_commands_scc(self.packer, CC.enabled, accel, self.hyundai_jerk, int(self.frame / 2), hud_control, set_speed_in_units, stopping, CC.cruiseControl.override, casper_ev, CS, self.soft_hold_mode)) @@ -881,7 +882,7 @@ def check_carrot_cruise(self, CC, CS, hud_control, stopping, accel, a_target): self.carrot_cruise_accel = max(carrot_cruise, self.carrot_cruise_accel - 1.0 * DT_CTRL) # 점진적으로 줄임. if self.carrot_cruise == 0: self.carrot_cruise_accel = CS.out.aEgo - + def make_jerk(self, CP, CS, accel, actuators, hud_control): if actuators.longControlState == LongCtrlState.stopping: self.jerk = self.jerk_u_min / 2 - CS.out.aEgo diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index d4a1ec0f629..53571db9249 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -833,8 +833,8 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, values["LCA_LEFT_ICON"] = 1 if lat_active else 0 values["LCA_RIGHT_ICON"] = 1 if lat_active else 0 else: - values["LCA_LEFT_ICON"] = (1 if CS.out.leftBlindspot else 2) if lat_active else 0 - values["LCA_RIGHT_ICON"] = (1 if CS.out.rightBlindspot else 2) if lat_active else 0 + values["LCA_LEFT_ICON"] = (2 if desire in (1, 3) else 1) if lat_active else 0 + values["LCA_RIGHT_ICON"] = (2 if desire in (2, 4) else 1) if lat_active else 0 values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if desire in (1, 3) else 0 values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if desire in (2, 4) else 0 diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 0576a8a2dfd..ee520180b2b 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -902,13 +902,13 @@ "group": "조향튜닝", "name": "AngleControlBlend", "title": "앵글조향 보정 적용률(0%)", - "descr": "angle steering 차량의 저속 EPS 보호와 추종 보정 적용률\n0: 최소\n100: 최대\n재시작 적용", + "descr": "angle steering 차량의 저속 EPS 보호와 추종 보정 적용률\n0: 최소\n100: 최대\n변경 즉시 적용", "egroup": "LAT", "etitle": "Angle Control Blend(0%)", - "edescr": "Low-speed EPS protection and tracking correction for angle steering\n0: Min\n100: Max\nChanges apply on the next controls process start", + "edescr": "Low-speed EPS protection and tracking correction for angle steering\n0: Min\n100: Max\nChanges apply immediately", "cgroup": "转向", "ctitle": "角度转向校正比例(0%)", - "cdescr": "角度转向车辆的低速 EPS 保护和跟踪校正比例\n0: Min\n100: Max\n中间值按比例混合两种状态\n更改将在下次控制进程启动时生效", + "cdescr": "角度转向车辆的低速 EPS 保护和跟踪校正比例\n0: Min\n100: Max\n中间值按比例混合两种状态\n更改后立即生效", "min": 0, "max": 100, "default": 0, From 23c291848257f9d3c52207e3d55cc787a3eb1f07 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Wed, 1 Jul 2026 08:32:48 +0900 Subject: [PATCH 11/13] Keep CCNC lane change indicators aligned --- .../opendbc/car/hyundai/hyundaicanfd.py | 29 ++++++++++++------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 53571db9249..e1599cd159c 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -598,12 +598,18 @@ def _get_desire_and_lane_changing(md): lane_changing = 0 if md is not None: desire = md.meta.desire.raw - ds = md.meta.desireState - if len(ds) > 4: - if ds[1] > 0.9: lane_changing = 1 - if ds[2] > 0.9: lane_changing = 2 - if ds[3] > 0.9: lane_changing = 3 - if ds[4] > 0.9: lane_changing = 4 + if md.meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing): + if md.meta.laneChangeDirection == LaneChangeDirection.left: + lane_changing = 3 + elif md.meta.laneChangeDirection == LaneChangeDirection.right: + lane_changing = 4 + else: + ds = md.meta.desireState + if len(ds) > 4: + if ds[1] > 0.9: lane_changing = 1 + if ds[2] > 0.9: lane_changing = 2 + if ds[3] > 0.9: lane_changing = 3 + if ds[4] > 0.9: lane_changing = 4 return desire, lane_changing def _apply_lane_desire(values, desire): @@ -690,6 +696,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, create_ccnc_messages._lane_line_check = Params().get_int("LaneLineCheck") lane_line_check = create_ccnc_messages._lane_line_check desire, lane_changing = _get_desire_and_lane_changing(md) + display_desire = lane_changing if lane_changing in (3, 4) else desire if CP.flags & HyundaiFlags.CAMERA_SCC.value: HDA_CntrlModSta = 0 @@ -833,11 +840,11 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, values["LCA_LEFT_ICON"] = 1 if lat_active else 0 values["LCA_RIGHT_ICON"] = 1 if lat_active else 0 else: - values["LCA_LEFT_ICON"] = (2 if desire in (1, 3) else 1) if lat_active else 0 - values["LCA_RIGHT_ICON"] = (2 if desire in (2, 4) else 1) if lat_active else 0 + values["LCA_LEFT_ICON"] = (2 if values["LANELINE_LEFT"] in (2, 6) else 1) if lat_active else 0 + values["LCA_RIGHT_ICON"] = (2 if values["LANELINE_RIGHT"] in (2, 6) else 1) if lat_active else 0 - values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if desire in (1, 3) else 0 - values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if desire in (2, 4) else 0 + values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if display_desire in (1, 3) else 0 + values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if display_desire in (2, 4) else 0 ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values, rx_counter = rx_counter)) @@ -858,7 +865,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, values, CS, lat_active, frame, hud_control, lane_line=True, corner_radar=True, - desire=desire, + desire=display_desire, # 기존대로 LR/RR만 깜빡임 blink_pairs=[('LR_DETECT', 'LR_DETECT_DISTANCE'), ('RR_DETECT', 'RR_DETECT_DISTANCE')], From a6d5b527a871afe16069ffe95141cb7bc93edc14 Mon Sep 17 00:00:00 2001 From: janpoo6427 <62945300+janpoo6427@users.noreply.github.com> Date: Wed, 1 Jul 2026 08:47:39 +0900 Subject: [PATCH 12/13] Refine LCA icon logic for lane line conditions --- opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index e1599cd159c..92dc940108e 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -840,8 +840,8 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, values["LCA_LEFT_ICON"] = 1 if lat_active else 0 values["LCA_RIGHT_ICON"] = 1 if lat_active else 0 else: - values["LCA_LEFT_ICON"] = (2 if values["LANELINE_LEFT"] in (2, 6) else 1) if lat_active else 0 - values["LCA_RIGHT_ICON"] = (2 if values["LANELINE_RIGHT"] in (2, 6) else 1) if lat_active else 0 + values["LCA_LEFT_ICON"] = (2 if values["LANELINE_LEFT"] == 6 else 1) if lat_active else 0 + values["LCA_RIGHT_ICON"] = (2 if values["LANELINE_RIGHT"] == 6 else 1) if lat_active else 0 values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if display_desire in (1, 3) else 0 values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if display_desire in (2, 4) else 0 From 426dda281d874e241aea27e48c807f4f7a32ac69 Mon Sep 17 00:00:00 2001 From: janpoo6427 Date: Wed, 1 Jul 2026 15:42:57 +0900 Subject: [PATCH 13/13] Keep lane highlight centered through lane change --- .../opendbc/car/hyundai/hyundaicanfd.py | 29 +++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 92dc940108e..f229a3bd19e 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -697,6 +697,10 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, lane_line_check = create_ccnc_messages._lane_line_check desire, lane_changing = _get_desire_and_lane_changing(md) display_desire = lane_changing if lane_changing in (3, 4) else desire + lane_change_starting = md is not None and md.meta.laneChangeState == LaneChangeState.laneChangeStarting + lane_change_finishing = md is not None and md.meta.laneChangeState == LaneChangeState.laneChangeFinishing + lane_change_left = md is not None and md.meta.laneChangeDirection == LaneChangeDirection.left + lane_change_right = md is not None and md.meta.laneChangeDirection == LaneChangeDirection.right if CP.flags & HyundaiFlags.CAMERA_SCC.value: HDA_CntrlModSta = 0 @@ -843,8 +847,22 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, values["LCA_LEFT_ICON"] = (2 if values["LANELINE_LEFT"] == 6 else 1) if lat_active else 0 values["LCA_RIGHT_ICON"] = (2 if values["LANELINE_RIGHT"] == 6 else 1) if lat_active else 0 - values["LANE_LEFT"] = 0 if trailer_lane_change_blocked else 1 if display_desire in (1, 3) else 0 - values["LANE_RIGHT"] = 0 if trailer_lane_change_blocked else 1 if display_desire in (2, 4) else 0 + if trailer_lane_change_blocked: + values["LANE_LEFT"] = 0 + values["LANE_RIGHT"] = 0 + elif lane_change_starting: + values["LANE_LEFT"] = 1 if lane_change_left else 0 + values["LANE_RIGHT"] = 1 if lane_change_right else 0 + values["LANE_HIGHLIGHT"] = 0 + values["LANE_HIGHLIGHT_DISTANCE"] = 0.0 + elif lane_change_finishing: + values["LANE_LEFT"] = 0 + values["LANE_RIGHT"] = 0 + values["LANE_HIGHLIGHT"] = 1 + values["LANE_HIGHLIGHT_DISTANCE"] = 150.0 + else: + values["LANE_LEFT"] = 1 if display_desire in (1, 3) else 0 + values["LANE_RIGHT"] = 1 if display_desire in (2, 4) else 0 ret.append(packer.make_can_msg("ADRV_0x161", CAN.ECAN, values, rx_counter = rx_counter)) @@ -872,6 +890,13 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, blink_t=1.0 ) + if lane_change_starting: + values["LANE_LEFT"] = 2 if lane_change_left else 0 + values["LANE_RIGHT"] = 2 if lane_change_right else 0 + elif lane_change_finishing: + values["LANE_LEFT"] = 0 + values["LANE_RIGHT"] = 0 + ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values, rx_counter = rx_counter)) if CS.ccnc_0x162 is not None: