Auto-tune fix

selfdrive/locationd/test/test_torqued.py

@@ -1,6 +1,67 @@
+import numpy as np
+
 from cereal import car
 from types import SimpleNamespace
-from openpilot.selfdrive.locationd.torqued import TorqueEstimator
+from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
+from openpilot.selfdrive.locationd.torqued import (TorqueEstimator, LAT_ACC_THRESHOLD, FACTOR_SANITY,
+                                                   IONIQ_6_LAT_ACC_THRESHOLD, IONIQ_6_FACTOR_SANITY)
+
+
+def _torque_cp(fingerprint, lat_accel_factor=3.0, friction=0.09):
+  CP = car.CarParams.new_message()
+  CP.carFingerprint = fingerprint
+  CP.brand = "hyundai"
+  CP.lateralTuning.init("torque")
+  CP.lateralTuning.torque.latAccelFactor = lat_accel_factor
+  CP.lateralTuning.torque.friction = friction
+  return CP
+
+
+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
+
+
+def _clipped_factor(fingerprint, slope):
+  est = TorqueEstimator(_torque_cp(fingerprint))
+  est.starpilot_toggles = SimpleNamespace(use_custom_latAccelFactor=False, use_custom_friction=False)
+  _fill_line(est, slope)
+  captured = {}
+  est.update_params = lambda params: captured.update(params)
+  msg = est.get_msg()
+  assert msg.liveTorqueParameters.liveValid
+  return msg.liveTorqueParameters.latAccelFactorRaw, captured["latAccelFactor"]
+
+
+def test_ioniq_6_learner_limits():
+  est = TorqueEstimator(_torque_cp(HYUNDAI_CAR.HYUNDAI_IONIQ_6))
+  assert est.lat_acc_threshold == IONIQ_6_LAT_ACC_THRESHOLD
+  assert np.isclose(est.min_lataccel_factor, 3.0 * (1 - IONIQ_6_FACTOR_SANITY))
+  assert np.isclose(est.max_lataccel_factor, 3.0 * (1 + IONIQ_6_FACTOR_SANITY))
+
+  other = TorqueEstimator(_torque_cp(HYUNDAI_CAR.HYUNDAI_IONIQ_5))
+  assert other.lat_acc_threshold == LAT_ACC_THRESHOLD
+  assert np.isclose(other.max_lataccel_factor, 3.0 * (1 + FACTOR_SANITY))
+
+
+def test_ioniq_6_post_tire_slope_is_not_clamped():
+  raw, used = _clipped_factor(HYUNDAI_CAR.HYUNDAI_IONIQ_6, 4.4)
+  assert abs(raw - 4.4) < 0.1
+  assert abs(used - 4.4) < 0.1
+
+  _, used_other = _clipped_factor(HYUNDAI_CAR.HYUNDAI_IONIQ_5, 4.4)
+  assert np.isclose(used_other, 3.0 * (1 + FACTOR_SANITY))
 
 
 def test_cal_percent():

selfdrive/locationd/torqued.py

@@ -10,6 +10,7 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import IONIQ_6_CARS
 from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
 
 from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles
@@ -30,6 +31,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.4) 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
@@ -71,6 +77,11 @@ class TorqueEstimator(ParameterEstimator):
       self.factor_sanity = FACTOR_SANITY
       self.friction_sanity = FRICTION_SANITY
 
+    self.lat_acc_threshold = LAT_ACC_THRESHOLD
+    if CP.carFingerprint in IONIQ_6_CARS:
+      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
@@ -198,7 +209,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: