diff --git a/selfdrive/locationd/test/test_torqued.py b/selfdrive/locationd/test/test_torqued.py index f85c62f80f..7ed8ffe068 100644 --- a/selfdrive/locationd/test/test_torqued.py +++ b/selfdrive/locationd/test/test_torqued.py @@ -1,6 +1,69 @@ +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): + """Fill every bucket to its minimum (and the total) with points on lat = slope * torque.""" + 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)) + # top up the total round-robin: 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 + + # the old +/-30% window (still used by every other car) pins the same data at 3.9 + _, 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..202c1f3bc8 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,15 @@ STEER_MIN_THRESHOLD = 0.02 MIN_FILTER_DECAY = 50 MAX_FILTER_DECAY = 250 LAT_ACC_THRESHOLD = 1 +# The Ioniq 6 needs very little torque per unit lateral accel (~4.4-4.9 m/s^2 per unit torque after +# the 2026-09-12 tire/alignment change), so |torque| 0.3-0.5 means 1.5-2.5 m/s^2 and the 1 m/s^2 cap +# discarded 909 of 911 points in the [-0.5, -0.3) bucket: calPerc sat at ~50% forever and the learner +# never saw the tire change. The cap also truncated on the y variable, dragging the slope low; replaying +# the post-tire drives, the estimate stops moving between 2.5 and 3.0 m/s^2 (bias gone) and every +# bucket fills. The sanity window is widened to +/-50% (1.5-4.5, the SteerLatAccel slider range): +# the first post-tire raw estimate was 4.43, above the +/-30% ceiling of 3.9. +IONIQ_6_LAT_ACC_THRESHOLD = 2.5 +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 +81,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 +213,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: