angle

2026-08-05 08:16:06 +08:00 · 2026-05-08 11:01:30 -05:00
parent fbba97f9a6
commit 7dc26d1a68
3 changed files with 137 additions and 8 deletions
@@ -63,6 +63,80 @@ IONIQ_6_IPEDAL_REGEN_STATE = 0x50
 IONIQ_6_IPEDAL_REGEN_STATE_2_PENDING = 0x01
 IONIQ_6_IPEDAL_PROGRESS_RETRY_WAIT_FRAMES = 10
 IONIQ_6_IPEDAL_RETRY_WAIT_FRAMES = 30
+ANGLE_SAFETY_BASELINE_MODEL = "KIA_SPORTAGE_HEV_2026"
+
+
+def get_baseline_safety_cp():
+  from opendbc.car.hyundai.interface import CarInterface
+  return CarInterface.get_non_essential_params(ANGLE_SAFETY_BASELINE_MODEL)
+
+
+def calculate_angle_torque_reduction_gain(params, CS, apply_torque_last, target_torque_reduction_gain):
+  target_gain = max(target_torque_reduction_gain, params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN)
+
+  driver_torque = abs(CS.out.steeringTorque)
+  alpha = np.interp(driver_torque, [params.STEER_THRESHOLD * 0.8, params.STEER_THRESHOLD * 2.0], [0.02, 0.1])
+
+  if CS.out.steeringPressed:
+    scale = 100.0
+    clamped_torque_gain = max(apply_torque_last, params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN)
+    target_gain = params.ANGLE_MIN_TORQUE_REDUCTION_GAIN + (clamped_torque_gain - params.ANGLE_MIN_TORQUE_REDUCTION_GAIN) * \
+                  np.exp(-(driver_torque - params.STEER_THRESHOLD) / scale)
+
+  new_gain = apply_torque_last + alpha * (target_gain - apply_torque_last)
+  return float(np.clip(new_gain, params.ANGLE_MIN_TORQUE_REDUCTION_GAIN, params.ANGLE_MAX_TORQUE_REDUCTION_GAIN))
+
+
+def sp_smooth_angle(v_ego_raw: float, apply_angle: float, apply_angle_last: float) -> float:
+  if abs(apply_angle - apply_angle_last) <= 0.1:
+    return apply_angle
+
+  adjusted_alpha = np.interp(v_ego_raw, CarControllerParams.SMOOTHING_ANGLE_VEGO_MATRIX,
+                             CarControllerParams.SMOOTHING_ANGLE_ALPHA_MATRIX)
+  adjusted_alpha_limited = float(min(float(adjusted_alpha), 1.0))
+  return (apply_angle * adjusted_alpha_limited) + (apply_angle_last * (1.0 - adjusted_alpha_limited))
+
+
+class TorqueReductionGainController:
+  def __init__(self, angle_threshold=3.0, debounce_time=0.5, min_gain=0.0, max_gain=1.0,
+               ramp_up_rate=0.1, ramp_down_rate=0.05):
+    self.angle_threshold = angle_threshold
+    self.debounce_time = debounce_time
+    self.min_gain = min_gain
+    self.max_gain = max_gain
+    self.ramp_up_rate = ramp_up_rate
+    self.ramp_down_rate = ramp_down_rate
+    self.saturated_since = None
+    self.gain = min_gain
+    self.last_update_time = 0.0
+
+  def update(self, last_requested_angle, actual_angle, lat_active):
+    now = self.last_update_time + DT_CTRL if self.last_update_time > 0.0 else DT_CTRL
+    dt = now - self.last_update_time if self.last_update_time > 0.0 else DT_CTRL
+    self.last_update_time = now
+
+    angle_error = abs(last_requested_angle - actual_angle)
+    saturated = lat_active and angle_error > self.angle_threshold
+
+    if saturated:
+      if self.saturated_since is None:
+        self.saturated_since = now
+      elif (now - self.saturated_since) > self.debounce_time:
+        self.gain = min(self.gain + self.ramp_up_rate * dt, self.max_gain)
+    else:
+      self.saturated_since = None
+      self.gain = max(self.gain - self.ramp_down_rate * dt, self.min_gain)
+
+    if not lat_active:
+      self.gain = self.min_gain
+      self.saturated_since = None
+
+    return self.gain
+
+  def reset(self):
+    self.gain = self.min_gain
+    self.saturated_since = None
+    self.last_update_time = 0.0


@dataclass
@@ -226,10 +300,12 @@ 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())

    self.accel_last = 0
    self.apply_torque_last = 0
    self.apply_angle_last = 0.0
+    self.angle_enable_smoothing_factor = True
    self.car_fingerprint = CP.carFingerprint
    self.last_button_frame = 0
    self.ecu_disable_failed = False
@@ -250,6 +326,14 @@ class CarController(CarControllerBase):
    self._ioniq_6_regen_request_sent = False
    self._ioniq_6_last_gear = structs.CarState.GearShifter.unknown
    self._genesis_g90_long_tuning = GenesisG90LongitudinalTuningState()
+    self.angle_torque_reduction_gain_controller = TorqueReductionGainController(
+      angle_threshold=0.3,
+      debounce_time=0.1,
+      min_gain=self.params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN,
+      max_gain=self.params.ANGLE_MAX_TORQUE_REDUCTION_GAIN,
+      ramp_up_rate=self.params.ANGLE_RAMP_UP_TORQUE_REDUCTION_RATE,
+      ramp_down_rate=self.params.ANGLE_RAMP_DOWN_TORQUE_REDUCTION_RATE,
+    )

  def _reset_ioniq_6_always_ipedal(self) -> None:
    self._ioniq_6_always_ipedal_pending = False
@@ -358,18 +442,30 @@ class CarController(CarControllerBase):
      desired_angle = float(np.clip(actuators.steeringAngleDeg,
                                    -self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
                                    self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
+      if self.angle_enable_smoothing_factor and abs(CS.out.vEgoRaw) < CarControllerParams.SMOOTHING_ANGLE_MAX_VEGO:
+        desired_angle = sp_smooth_angle(CS.out.vEgoRaw, desired_angle, self.apply_angle_last)
+
      apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, CS.out.vEgoRaw,
                                                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 self.CP.carFingerprint != ANGLE_SAFETY_BASELINE_MODEL:
+        apply_angle = apply_steer_angle_limits_vm(apply_angle or desired_angle, self.apply_angle_last, CS.out.vEgoRaw,
+                                                  CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM)

-      apply_steer_req = CC.latActive and apply_torque > 0.0
+      target_torque_reduction_gain = self.angle_torque_reduction_gain_controller.update(
+        last_requested_angle=self.apply_angle_last,
+        actual_angle=CS.out.steeringAngleDeg,
+        lat_active=CC.latActive,
+      )
+      apply_torque = calculate_angle_torque_reduction_gain(self.params, CS, self.apply_torque_last, target_torque_reduction_gain)
+      apply_steer_req = CC.latActive and apply_torque != 0
      torque_fault = False
+
+      if apply_angle is None:
+        apply_torque = 0.0
+        apply_angle = CS.out.steeringAngleDeg
+        apply_steer_req = False
+
      self.apply_angle_last = apply_angle
    else:
      # steering torque
@@ -9,6 +9,7 @@ from opendbc.car.structs import CarControl, CarParams
 from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
 from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \
                                              IONIQ_6_IPEDAL_PADDLE_BURST_COUNT, IONIQ_6_IPEDAL_NEXT_COUNTER_BURST_COUNT, \
+                                              calculate_angle_torque_reduction_gain, sp_smooth_angle, \
                                              update_ioniq_6_longitudinal_tuning, \
                                              update_genesis_g90_longitudinal_tuning
 from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state, decode_ioniq_6_ipedal_intermediate_state, \
@@ -1074,6 +1075,32 @@ class TestHyundaiFingerprint:
    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_angle_smoothing_is_speed_dependent(self):
+    target_angle = 12.0
+    last_angle = 0.0
+
+    low_speed_smoothed = sp_smooth_angle(0.0, target_angle, last_angle)
+    high_speed_smoothed = sp_smooth_angle(25.0, target_angle, last_angle)
+
+    assert last_angle < low_speed_smoothed < target_angle
+    assert high_speed_smoothed == pytest.approx(target_angle)
+    assert low_speed_smoothed < high_speed_smoothed
+
+  def test_angle_torque_reduction_gain_gives_up_on_driver_override(self):
+    sportage = CarParams.new_message()
+    sportage.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
+    sportage.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)
+    params = CarControllerParams(sportage)
+
+    no_override_cs = SimpleNamespace(out=SimpleNamespace(steeringTorque=0.0, steeringPressed=False))
+    override_cs = SimpleNamespace(out=SimpleNamespace(steeringTorque=float(params.STEER_THRESHOLD * 2), steeringPressed=True))
+
+    no_override_gain = calculate_angle_torque_reduction_gain(params, no_override_cs, params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN, 1.0)
+    override_gain = calculate_angle_torque_reduction_gain(params, override_cs, 1.0, 1.0)
+
+    assert params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN <= no_override_gain <= params.ANGLE_MAX_TORQUE_REDUCTION_GAIN
+    assert params.ANGLE_MIN_TORQUE_REDUCTION_GAIN <= override_gain < no_override_gain
+
  def test_ioniq_5_canfd_aux_messages_are_optional(self):
    toggles = get_test_toggles()
    fingerprint = gen_empty_fingerprint()
@@ -32,8 +32,14 @@ class CarControllerParams:
    MAX_ANGLE_RATE=5,
  )
  ANGLE_MAX_TORQUE_REDUCTION_GAIN = 1.0
-  ANGLE_MIN_TORQUE_REDUCTION_GAIN = 0.6
+  ANGLE_MIN_TORQUE_REDUCTION_GAIN = 0.1
  ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN = 0.6
+  ANGLE_RAMP_UP_TORQUE_REDUCTION_RATE = 0.008
+  ANGLE_RAMP_DOWN_TORQUE_REDUCTION_RATE = 0.0012
+  ANGLE_TORQUE_OVERRIDE_CYCLES = 17
+  SMOOTHING_ANGLE_VEGO_MATRIX = [0.0, 8.5, 11.0, 13.8, 22.22]
+  SMOOTHING_ANGLE_ALPHA_MATRIX = [0.05, 0.1, 0.3, 0.6, 1.0]
+  SMOOTHING_ANGLE_MAX_VEGO = SMOOTHING_ANGLE_VEGO_MATRIX[-1]

  def __init__(self, CP, vEgoRaw=100.):
    self.ANGLE_LIMITS = self.ANGLE_LIMITS