diff --git a/.gitignore b/.gitignore index e09e052c38..3470fc9440 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -venv/ +pvenv/ .venv/ .ci_cache .env diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 5cb39db7b4..8175024092 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -109,7 +109,8 @@ def apply_steer_angle_limits_physics(desired_sw_deg: float, wheelbase_m: float, steer_ratio: float, steer_sw_max_deg: float, - model_v2=None) -> float: + model_v2=None, + driver_override: bool=False) -> float: max_lat_accel = 8.5 # m/s^2 max_lat_jerk = 4.0 # m/s^3 y_std_1s = 0.1 @@ -158,7 +159,7 @@ def apply_steer_angle_limits_physics(desired_sw_deg: float, # --- accel clip --- cmd_rw = float(np.clip(cmd_rw, -rw_max, rw_max)) - if not lat_active: + if not lat_active or driver_override: cmd_rw = float(steering_sw_deg) / steer_ratio cmd_sw = cmd_rw * steer_ratio @@ -218,6 +219,8 @@ def __init__(self, dbc_names, CP): self.camera_scc_params = Params().get_int("HyundaiCameraSCC") self.is_ldws_car = Params().get_bool("IsLdwsCar") self.enable_corner_radar = 0 + self.steer_override_release_sec = 0.20 + self.steer_override_recovery_sec = 0.50 self.steerDeltaUpOrg = self.steerDeltaUp = self.steerDeltaUpLC = self.params.STEER_DELTA_UP self.steerDeltaDownOrg = self.steerDeltaDown = self.steerDeltaDownLC = self.params.STEER_DELTA_DOWN @@ -227,6 +230,8 @@ def update(self, CC, CS, now_nanos): if self.frame % 50 == 0: params = Params() self.max_angle_frames = params.get_int("MaxAngleFrames") + self.steer_override_release_sec = params.get_int("SteerOverrideReleaseSec") * 0.01 + self.steer_override_recovery_sec = params.get_int("SteerOverrideRecoverySec") * 0.01 steerMax = params.get_int("CustomSteerMax") steerDeltaUp = params.get_int("CustomSteerDeltaUp") steerDeltaDown = params.get_int("CustomSteerDeltaDown") @@ -300,6 +305,7 @@ def update(self, CC, CS, now_nanos): self.CP.steerRatio, self.params.ANGLE_LIMITS.STEER_ANGLE_MAX, CS.modelV2, + driver_override=CS.out.steeringPressed, ) @@ -367,9 +373,9 @@ def update(self, CC, CS, now_nanos): # Once fully recovered, hold full authority until the next driver override. torque_delta = 0.0 elif self.override_latched: - # Hold reduced authority until driver torque stays below 60% for 0.2 seconds. + # Hold reduced authority until driver torque stays below 60% for configured release delay. self.override_release_frames = self.override_release_frames + 1 if torque_ratio < 0.6 else 0 - if self.override_release_frames >= int(0.2 / DT_CTRL): + if self.override_release_frames >= int(self.steer_override_release_sec / DT_CTRL): self.override_latched = False self.override_release_frames = 0 recovery_allowed = True @@ -379,21 +385,24 @@ def update(self, CC, CS, now_nanos): recovery_allowed = True if recovery_allowed: - # Use one-second model uncertainty to set the base torque recovery time. - # Missing or invalid model data falls back to a moderate 1.5-second recovery. - y_std_1s = 0.2 - if CS.modelV2 is not None and len(CS.modelV2.position.yStd) > 10: - model_y_std_1s = float(CS.modelV2.position.yStd[10]) - if np.isfinite(model_y_std_1s) and model_y_std_1s >= 0.0: - y_std_1s = model_y_std_1s - - recovery_time = float(np.interp(y_std_1s, [0.1, 0.2, 0.3, 0.4], [0.5, 0.8, 1.5, 3.0])) - recovery_time = max(recovery_time, float(np.interp( - self.repeated_override_count, - [0, 1, 2, 3], - [0.1, 1.0, 2.0, 3.0], - ))) - base_rate_up = (self.angle_max_torque - self.params.ANGLE_MIN_TORQUE) * DT_CTRL / recovery_time + if self.steer_override_recovery_sec > 0: + recovery_time = self.steer_override_recovery_sec + else: + # Use one-second model uncertainty to set the base torque recovery time. + # Missing or invalid model data falls back to a moderate 1.5-second recovery. + y_std_1s = 0.2 + if CS.modelV2 is not None and len(CS.modelV2.position.yStd) > 10: + model_y_std_1s = float(CS.modelV2.position.yStd[10]) + if np.isfinite(model_y_std_1s) and model_y_std_1s >= 0.0: + y_std_1s = model_y_std_1s + + recovery_time = float(np.interp(y_std_1s, [0.1, 0.2, 0.3, 0.4], [0.5, 0.8, 1.5, 3.0])) + recovery_time = max(recovery_time, float(np.interp( + self.repeated_override_count, + [0, 1, 2, 3], + [0.1, 1.0, 2.0, 3.0], + ))) + base_rate_up = (self.angle_max_torque - self.params.ANGLE_MIN_TORQUE) * DT_CTRL / max(0.01, recovery_time) # 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])) diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 123c4140ff..a5e1ee6754 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -273,6 +273,8 @@ inline static std::unordered_map keys = { {"SteerActuatorDelay", {PERSISTENT, INT, "0"}}, {"LatSmoothSec", {PERSISTENT, INT, "13"}}, {"LatSuspendAngleDeg", {PERSISTENT, INT, "300"}}, + {"SteerOverrideReleaseSec", {PERSISTENT, INT, "20"}}, + {"SteerOverrideRecoverySec", {PERSISTENT, INT, "50"}}, {"CruiseOnDist", {PERSISTENT, INT, "400"}}, {"CruiseMaxVals0", {PERSISTENT, INT, "160"}}, diff --git a/openpilot/selfdrive/carrot_settings.json b/openpilot/selfdrive/carrot_settings.json index e436f38431..ab091f87e6 100644 --- a/openpilot/selfdrive/carrot_settings.json +++ b/openpilot/selfdrive/carrot_settings.json @@ -119,6 +119,8 @@ "SteerActuatorDelay", "LatSmoothSec", "LatSuspendAngleDeg", + "SteerOverrideReleaseSec", + "SteerOverrideRecoverySec", "CustomSR", "SteerRatioRate" ] @@ -994,6 +996,38 @@ "default": 300, "unit": 10 }, + { + "group": "조향튜닝", + "name": "SteerOverrideReleaseSec", + "title": "수동조향해제대기시간(20)", + "descr": "수동 조향 개입 후 손을 뗐을 때 자동 조향 토크 복구를 시작하기까지의 대기시간 (단위: 0.01초, 20 = 0.20초). 값을 낮추면 손을 떼자마자 복구가 바로 시작됩니다.", + "egroup": "LAT", + "etitle": "Steer Override Release Delay(20)", + "edescr": "Delay before starting steering torque recovery after releasing the wheel (unit: 0.01s, 20 = 0.20s). Lower values start recovery immediately.", + "cgroup": "转向", + "ctitle": "Steer Override Release Delay(20)", + "cdescr": "Delay before starting steering torque recovery after releasing the wheel (unit: 0.01s, 20 = 0.20s). Lower values start recovery immediately.", + "min": 0, + "max": 100, + "default": 20, + "unit": 1 + }, + { + "group": "조향튜닝", + "name": "SteerOverrideRecoverySec", + "title": "수동조향복구시간(50)", + "descr": "수동 조향 후 손을 뗐을 때 자동 조향 토크가 100%까지 램프업되는 복구시간 (단위: 0.01초, 50 = 0.50초). 낮출수록 커브길에서 손을 뗀 직후 토크가 멍하지 않고 빠르게 회복됩니다.", + "egroup": "LAT", + "etitle": "Steer Override Recovery Time(50)", + "edescr": "Ramp-up time for steering torque to reach 100% after releasing the wheel (unit: 0.01s, 50 = 0.50s). Lower values recover full torque faster in curves.", + "cgroup": "转向", + "ctitle": "Steer Override Recovery Time(50)", + "cdescr": "Ramp-up time for steering torque to reach 100% after releasing the wheel (unit: 0.01s, 50 = 0.50s). Lower values recover full torque faster in curves.", + "min": 0, + "max": 300, + "default": 50, + "unit": 5 + }, { "group": "크루즈", "name": "CruiseOnDist",