From e1ae4cdff362e5cf8903131047b22de0c4d723a8 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Tue, 29 Sep 2026 14:08:28 -0500 Subject: [PATCH] Auto-tune fix --- selfdrive/locationd/test/test_torqued.py | 63 +++++++++++++++++++++++- selfdrive/locationd/torqued.py | 13 ++++- 2 files changed, 74 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/test/test_torqued.py b/selfdrive/locationd/test/test_torqued.py index f85c62f80f..3e399adb23 100644 --- a/selfdrive/locationd/test/test_torqued.py +++ b/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(): diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 1ceba9e6f0..77ff4fb295 100755 --- a/selfdrive/locationd/torqued.py +++ b/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: