diff --git a/openpilot/selfdrive/locationd/test/test_torqued.py b/openpilot/selfdrive/locationd/test/test_torqued.py index 3c5cb29bfc..1ca21cc595 100644 --- a/openpilot/selfdrive/locationd/test/test_torqued.py +++ b/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 diff --git a/openpilot/selfdrive/locationd/torqued.py b/openpilot/selfdrive/locationd/torqued.py index 92607225fd..ccd7f98896 100755 --- a/openpilot/selfdrive/locationd/torqued.py +++ b/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: