torqued: widen Ioniq 6 lateral accel cap and factor sanity window

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

openpilot/selfdrive/locationd/test/test_torqued.py

@@ -1,6 +1,27 @@
+import numpy as np
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.hyundai.values import CAR as HYUNDAI
 from opendbc.car.structs import car
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.selfdrive.locationd.torqued import TorqueEstimator
+from openpilot.selfdrive.locationd.torqued import (TorqueEstimator, LAT_ACC_THRESHOLD, FACTOR_SANITY,
+                                                   IONIQ_6_LAT_ACC_THRESHOLD, IONIQ_6_FACTOR_SANITY)
+
+
+def _fill_line(est, slope, seed=0):
+  rng = np.random.default_rng(seed)
+  for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
+                                  est.filtered_points.buckets_min_points.values(), strict=True):
+    for _ in range(int(min_pts)):
+      x = rng.uniform(low, high)
+      est.filtered_points.add_point(x, slope * x + rng.normal(0.0, 0.02))
+  # a single bucket caps at POINTS_PER_BUCKET, below min_points_total
+  keys = list(est.filtered_points.buckets)
+  i = 0
+  while len(est.filtered_points) < est.min_points_total:
+    x = rng.uniform(*keys[i % len(keys)])
+    est.filtered_points.add_point(x, slope * x + rng.normal(0.0, 0.02))
+    i += 1
 
 
 class TestTorqued(OpenpilotTestCase):
@@ -25,3 +46,25 @@ class TestTorqued(OpenpilotTestCase):
 
     msg = est.get_msg()
     assert msg.lateralTorqueParameters.calPerc == 100
+
+  def test_ioniq_6_learner_limits(self):
+    cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
+    est = TorqueEstimator(cp)
+    factor = cp.lateralTuning.torque.latAccelFactor
+    assert est.lat_acc_threshold == IONIQ_6_LAT_ACC_THRESHOLD
+    assert np.isclose(est.max_lataccel_factor, factor * (1 + IONIQ_6_FACTOR_SANITY))
+
+    other = TorqueEstimator(interfaces[HYUNDAI.HYUNDAI_IONIQ_5].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_5))
+    assert other.lat_acc_threshold == LAT_ACC_THRESHOLD
+    assert other.factor_sanity == FACTOR_SANITY
+
+  def test_ioniq_6_learned_slope_is_not_clamped(self):
+    cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
+    est = TorqueEstimator(cp)
+    slope = cp.lateralTuning.torque.latAccelFactor * 1.45
+    _fill_line(est, slope)
+    captured = {}
+    est.update_params = lambda params: captured.update(params)
+    msg = est.get_msg()
+    assert msg.lateralTorqueParameters.valid
+    assert abs(captured["latAccelFactor"] - slope) < 0.1

openpilot/selfdrive/locationd/torqued.py

@@ -32,6 +32,11 @@ STEER_MIN_THRESHOLD = 0.02
 MIN_FILTER_DECAY = 50
 MAX_FILTER_DECAY = 250
 LAT_ACC_THRESHOLD = 1
+# Ioniq 6 makes ~4.5 m/s^2 per unit torque, so the 1 m/s^2 cap discards nearly every point in the
+# outer torque buckets (calPerc stuck at 50%) and truncating on lateral accel biases the slope low.
+IONIQ_6_LAT_ACC_THRESHOLD = 2.5
+# Its learned factor (~4.3) sits above the default +/-30% window around the 3.0 base.
+IONIQ_6_FACTOR_SANITY = 0.5
 STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
 MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
 MIN_ENGAGE_BUFFER = 2  # secs
@@ -73,6 +78,11 @@ class TorqueEstimator(ParameterEstimator):
       self.factor_sanity = FACTOR_SANITY
       self.friction_sanity = FRICTION_SANITY
 
+    self.lat_acc_threshold = LAT_ACC_THRESHOLD
+    if str(CP.carFingerprint) == 'HYUNDAI_IONIQ_6':
+      self.lat_acc_threshold = IONIQ_6_LAT_ACC_THRESHOLD
+      self.factor_sanity = max(self.factor_sanity, IONIQ_6_FACTOR_SANITY)
+
     self.offline_friction = 0.0
     self.offline_latAccelFactor = 0.0
     self.resets = 0.0
@@ -211,7 +221,7 @@ class TorqueEstimator(ParameterEstimator):
         steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']).item()
         lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item()
         if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD):
-          if abs(lateral_acc) <= LAT_ACC_THRESHOLD:
+          if abs(lateral_acc) <= self.lat_acc_threshold:
             self.filtered_points.add_point(steer, lateral_acc)
 
           if self.track_all_points: