Janpoo6427/carrot wip angle2

common/params_keys.h

@@ -276,6 +276,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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),
@@ -121,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)
@@ -162,9 +168,15 @@ 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.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
 
@@ -175,6 +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 = 0
     self.enable_corner_radar = 0
 
     self.steerDeltaUpOrg = self.steerDeltaUp = self.steerDeltaUpLC = self.params.STEER_DELTA_UP
@@ -184,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")
@@ -211,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"))
 
@@ -233,8 +247,10 @@ 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
 
     # steering torque
     new_torque = int(round(actuators.torque * self.params.STEER_MAX))
@@ -245,11 +261,32 @@ 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)
+    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,
@@ -260,10 +297,114 @@ def update(self, CC, CS, now_nanos):
       CS.modelV2,
     )
 
-    
+
     if angle_control:
       apply_steer_req = CC.latActive
 
+    angle_torque_cap = self.angle_max_torque
+    if angle_control and CC.latActive and angle_policy_level > 0:
+      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)
+      stalled_tracking = abs(CS.out.steeringRateDeg) < 8.0 and tracking_error > 8.0
+      eps_torque_abs = abs(CS.out.steeringTorqueEps)
+      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],
+                                           [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]))
+        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)
+        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]))
+        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)
+        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]))
+        common_torque_cap = min(common_torque_cap, max(self.params.ANGLE_MIN_TORQUE,
+                                                       unwind_cap))
+
+      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
+        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_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_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
+                                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_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_100 = min(angle_torque_cap_100, self.params.ANGLE_MIN_TORQUE)
+
+      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 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
+
     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):
@@ -354,7 +495,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
@@ -373,6 +514,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.driver_torque_filtered = 0.0
       self.driver_torque_filtered_prev = 0.0
       self.pre_override_frames = 0
@@ -397,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
@@ -446,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))
@@ -512,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))
@@ -740,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
@@ -765,4 +907,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)
-