mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
3778552160
Interpolate the measured Ioniq 6 steering delay instead of rounding delayed model requests to whole frames. Bound unsaturated friction growth without exceeding the current requested component or retaining its old direction; preserve full breakaway response, saved tunes and native limits. Cover actual Controls, replay state and controller timing.
253 lines
14 KiB
Python
253 lines
14 KiB
Python
"""Replay-only Ioniq 6 FLM controller seam; no production profile selection."""
|
|
|
|
import json
|
|
from pathlib import Path
|
|
from types import SimpleNamespace
|
|
import unittest
|
|
from unittest import mock
|
|
|
|
from opendbc.car import structs, scale_rot_inertia, scale_tire_stiffness
|
|
from opendbc.car.car_helpers import interfaces
|
|
from opendbc.car.hyundai.values import CAR as HYUNDAI
|
|
from opendbc.car.toyota.values import CAR as TOYOTA
|
|
from opendbc.car.vehicle_model import VehicleModel
|
|
from openpilot.common.realtime import DT_CTRL
|
|
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
|
from openpilot.starpilot.lateral.torque_extension import selected_policy
|
|
from openpilot.starpilot.flm.torque_surface import Ioniq6Surface
|
|
from openpilot.starpilot.lateral.ioniq6_policy import Ioniq6TorquePolicy
|
|
|
|
|
|
# Independently generated from frozen 678af783 LatControlTorque and Ioniq helpers.
|
|
# Full controller/PID/filter rows include the recorded symmetric-unwind correction.
|
|
_GOLDEN = json.loads((Path(__file__).parent / "testdata" / "ioniq6_controller_replay.json").read_text())
|
|
_EXTENDED = json.loads((Path(__file__).parent / "testdata" / "ioniq6_controller_extended.json").read_text())
|
|
_LIVE = json.loads((Path(__file__).parent / "testdata" / "ioniq6_controller_live_update.json").read_text())
|
|
_TUNED_KNOBS = {"ff_gain_left": 0.30, "ff_gain_right": 0.12,
|
|
"turn_in_boost_left": 2.20, "unwind_taper_right": 10.0,
|
|
"center_taper_max": 0.16, "center_deadband_low_deg": 0.1,
|
|
"low_speed_angle_assist_max_torque": 0.7}
|
|
|
|
|
|
def _params(*, firmware_2025=False):
|
|
car = HYUNDAI.HYUNDAI_IONIQ_6
|
|
cp = interfaces[car].get_non_essential_params(car)
|
|
for name, value in (("mass", 2084.0), ("wheelbase", 2.97), ("centerToFront", 1.188),
|
|
("steerRatio", 14.26), ("tireStiffnessFactor", 0.65)):
|
|
setattr(cp, name, value)
|
|
cp.rotationalInertia = scale_rot_inertia(cp.mass, cp.wheelbase)
|
|
cp.tireStiffnessFront, cp.tireStiffnessRear = scale_tire_stiffness(
|
|
cp.mass, cp.wheelbase, cp.centerToFront, cp.tireStiffnessFactor)
|
|
cp.lateralTuning.torque.latAccelFactor = 3.0
|
|
cp.lateralTuning.torque.latAccelOffset = 0.0
|
|
cp.lateralTuning.torque.friction = 0.09
|
|
cp.lateralTuning.torque.steeringAngleDeadzoneDeg = 0.0
|
|
if firmware_2025:
|
|
versions = cp.init("carFw", 2)
|
|
versions[0].fwVersion = b"IONIQ6-230915"
|
|
versions[1].fwVersion = b"IONIQ6-240206"
|
|
return cp
|
|
|
|
|
|
def _controller(cp, surface=None, *, original_constructor_context=False):
|
|
controller = LatControlTorque(cp.as_reader(), interfaces[HYUNDAI.HYUNDAI_IONIQ_6](cp), DT_CTRL, turn_assist=True)
|
|
if surface is not None:
|
|
# Replace before the first update. The production constructor always uses None.
|
|
controller.starpilot_extension.policy = Ioniq6TorquePolicy(controller, cp.as_reader(), surface=surface, turn_assist=True)
|
|
if original_constructor_context:
|
|
controller.hkg_canfd_torque = False
|
|
# These frozen vectors invoke the original constructor directly. Dom
|
|
# controlsd replaces this curve with the saved/default flat gain each tick;
|
|
# this is not a drive-loop replay. Preserve its gain curve and raw-CP startup limits.
|
|
controller.pid._k_p = [[1, 1.5, 2., 3., 5, 7.5, 10, 15, 30],
|
|
[250, 120, 65, 30, 11.5, 5.5, 3.5, 2., .6]]
|
|
controller.pid.set_limits(controller.lateral_accel_from_torque(controller.steer_max, cp.lateralTuning.torque),
|
|
controller.lateral_accel_from_torque(-controller.steer_max, cp.lateralTuning.torque))
|
|
return controller
|
|
|
|
|
|
def _assert_rows(test, actual, expected):
|
|
for row, reference in zip(actual, expected, strict=True):
|
|
for value, frozen in zip(row, reference, strict=True):
|
|
test.assertAlmostEqual(value, frozen, places=8)
|
|
|
|
|
|
def _trace(controller, cp, sequence, *, historical_friction=False):
|
|
vm = VehicleModel(cp)
|
|
cs = structs.CarState.new_message()
|
|
cs.gearShifter = structs.CarState.GearShifter.drive
|
|
params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.03)
|
|
rows = []
|
|
for active, speed, angle, curvature, pressed, limited in sequence:
|
|
cs.vEgo = speed
|
|
cs.steeringAngleDeg = angle
|
|
cs.steeringPressed = pressed
|
|
# Historical vectors qualify the unchanged calibration/PID equations before
|
|
# the separately tested friction growth guard, not its new transients.
|
|
if historical_friction:
|
|
with mock.patch.object(selected_policy(controller), 'center_friction',
|
|
side_effect=lambda friction, *args, **kwargs: friction):
|
|
torque, _, state = controller.update(active, cs, vm, params, limited, curvature, limited, 0.1)
|
|
else:
|
|
torque, _, state = controller.update(active, cs, vm, params, limited, curvature, limited, 0.1)
|
|
rows.append((torque, state.desiredLateralAccel, state.desiredLateralJerk,
|
|
state.p, state.i, state.f, selected_policy(controller).directional_taper_filter.x))
|
|
return rows
|
|
|
|
|
|
SEQUENCE = (
|
|
[(False, 0.3, 10.0, -0.001, False, False)] * 2 +
|
|
[(True, 0.3, 10.0, -0.001, False, False)] * 4 +
|
|
[(True, 3.0, 8.0, -0.001, False, False)] * 4 +
|
|
[(True, 14.0, -3.0, 0.0004, False, False)] * 5 +
|
|
[(True, 27.0, -2.0, -0.0003, False, True)] * 4 +
|
|
[(True, 14.0, -3.0, 0.0004, True, False)] * 2 +
|
|
[(True, 14.0, -3.0, 0.0004, False, False)] * 3 +
|
|
[(False, 0.0, 8.0, -0.001, False, False)] * 2 +
|
|
[(True, 3.0, 8.0, -0.001, False, False)] * 4
|
|
)
|
|
|
|
|
|
class TestIoniq6ControllerReplay(unittest.TestCase):
|
|
def test_frozen_stateful_controller_trace(self):
|
|
self.assertEqual(_GOLDEN["sourceCommit"], "678af783")
|
|
self.assertEqual(_GOLDEN["controllerSha256"], "3561d5ac43a72f9b68bc9e44042b8f5cbdca406dec940e7648c64607862a60f7")
|
|
self.assertEqual(_GOLDEN["fields"], ["output", "desiredLateralAccel", "desiredLateralJerk", "p", "i", "f", "directionalFilter"])
|
|
for firmware, fw in (("2023", False), ("2025", True)):
|
|
for mode in ("default", "tuned"):
|
|
with self.subTest(firmware=firmware, mode=mode):
|
|
cp = _params(firmware_2025=fw)
|
|
surface = Ioniq6Surface.validated("firmware_2025" if fw else "standard", _TUNED_KNOBS) if mode == "tuned" else None
|
|
actual = _trace(_controller(cp, surface, original_constructor_context=True), cp, SEQUENCE)
|
|
expected = _GOLDEN["cases"][f"{firmware}_{mode}"]
|
|
_assert_rows(self, actual, expected)
|
|
|
|
def test_extended_frozen_filter_limit_override_and_reset_trace(self):
|
|
self.assertEqual(_EXTENDED["sourceCommit"], "678af783")
|
|
self.assertEqual(_EXTENDED["controllerSha256"], _GOLDEN["controllerSha256"])
|
|
self.assertEqual(_EXTENDED["inputFields"], ["active", "speed", "angle", "curvature", "pressed", "limited"])
|
|
self.assertEqual(_EXTENDED["outputFields"], _GOLDEN["fields"])
|
|
sequence = _EXTENDED["sequence"]
|
|
self.assertEqual(len(sequence), 154)
|
|
self.assertTrue(any(not row[0] for row in sequence))
|
|
self.assertTrue(any(row[4] for row in sequence))
|
|
self.assertTrue(any(row[5] for row in sequence))
|
|
for firmware, fw in (("2023", False), ("2025", True)):
|
|
for mode in ("default", "tuned"):
|
|
with self.subTest(firmware=firmware, mode=mode):
|
|
cp = _params(firmware_2025=fw)
|
|
surface = Ioniq6Surface.validated("firmware_2025" if fw else "standard", _TUNED_KNOBS) if mode == "tuned" else None
|
|
actual = _trace(_controller(cp, surface, original_constructor_context=True), cp, sequence, historical_friction=True)
|
|
expected = _EXTENDED["cases"][f"{firmware}_{mode}"]
|
|
_assert_rows(self, actual, expected)
|
|
|
|
def test_actual_torque_parameter_update_recalculates_frozen_pid_limits(self):
|
|
self.assertEqual(_LIVE["sourceControllerSha256"], _GOLDEN["controllerSha256"])
|
|
self.assertEqual(_LIVE["fields"], _GOLDEN["fields"])
|
|
for firmware, fw in (("2023", False), ("2025", True)):
|
|
for mode in ("default", "tuned"):
|
|
with self.subTest(firmware=firmware, mode=mode):
|
|
cp = _params(firmware_2025=fw)
|
|
surface = Ioniq6Surface.validated("firmware_2025" if fw else "standard", _TUNED_KNOBS) if mode == "tuned" else None
|
|
controller = _controller(cp, surface, original_constructor_context=True)
|
|
expected = _LIVE["cases"][f"{firmware}_{mode}"]
|
|
self.assertAlmostEqual(controller.pid.pos_limit, expected["before"]["posLimit"], places=8)
|
|
self.assertAlmostEqual(controller.pid.neg_limit, expected["before"]["negLimit"], places=8)
|
|
controller.update_torque_parameters(3.0, 0.0, 0.09)
|
|
self.assertAlmostEqual(controller.pid.pos_limit, expected["after"]["posLimit"], places=8)
|
|
self.assertAlmostEqual(controller.pid.neg_limit, expected["after"]["negLimit"], places=8)
|
|
actual = _trace(controller, cp, _EXTENDED["sequence"], historical_friction=True)
|
|
_assert_rows(self, actual, expected["rows"])
|
|
|
|
def test_guarded_extended_trace_preserves_model_pid_inputs_and_manual_surface(self):
|
|
for firmware, fw in (("2023", False), ("2025", True)):
|
|
for mode in ("default", "tuned"):
|
|
with self.subTest(firmware=firmware, mode=mode):
|
|
cp = _params(firmware_2025=fw)
|
|
surface = Ioniq6Surface.validated("firmware_2025" if fw else "standard", _TUNED_KNOBS) if mode == "tuned" else None
|
|
controller = _controller(cp, surface, original_constructor_context=True)
|
|
tune = controller.torque_params.to_dict()
|
|
actual = _trace(controller, cp, _EXTENDED["sequence"])
|
|
expected = _EXTENDED["cases"][f"{firmware}_{mode}"]
|
|
for row, reference in zip(actual, expected, strict=True):
|
|
for index in (1, 2, 3, 6):
|
|
self.assertAlmostEqual(row[index], reference[index], places=8)
|
|
self.assertLessEqual(abs(row[0]), controller.steer_max)
|
|
if mode == "tuned":
|
|
_assert_rows(self, actual, expected)
|
|
else:
|
|
self.assertTrue(any(abs(row[5] - ref[5]) > 1e-8 for row, ref in zip(actual, expected, strict=True)))
|
|
self.assertEqual(controller.torque_params.to_dict(), tune)
|
|
|
|
def test_current_startup_and_equal_live_update_use_compensated_limits(self):
|
|
for firmware, fw in (("2023", False), ("2025", True)):
|
|
for mode in ("default", "tuned"):
|
|
with self.subTest(firmware=firmware, mode=mode):
|
|
cp = _params(firmware_2025=fw)
|
|
surface = Ioniq6Surface.validated("firmware_2025" if fw else "standard", _TUNED_KNOBS) if mode == "tuned" else None
|
|
controller = _controller(cp, surface)
|
|
self.assertAlmostEqual(controller.torque_params.latAccelFactor, 3.0 * 1.22, places=6)
|
|
limits = (controller.pid.pos_limit, controller.pid.neg_limit)
|
|
self.assertEqual(limits, (controller.torque_params.latAccelFactor, -controller.torque_params.latAccelFactor))
|
|
for sign in (-1, 1):
|
|
controller.pid.reset()
|
|
accel = controller.pid.update(0., feedforward=sign * 100., freeze_integrator=True)
|
|
self.assertEqual(accel, sign * controller.torque_params.latAccelFactor)
|
|
controller.update_torque_parameters(3.0, 0.0, 0.09)
|
|
self.assertEqual((controller.pid.pos_limit, controller.pid.neg_limit), limits)
|
|
|
|
def test_curvature_delay_scales_at_current_speed_without_spurious_unwind(self):
|
|
cp = _params()
|
|
controller = _controller(cp)
|
|
sequence = [(True, 10., 0., .001, False, False)] * 30 + [(True, 20., 0., .001, False, False)] * 20
|
|
actual = _trace(controller, cp, sequence)
|
|
alpha = selected_policy(controller).jerk_filter.alpha
|
|
self.assertAlmostEqual(actual[30][2], actual[29][2] * (1.0 - alpha), places=8)
|
|
|
|
def test_replay_surface_is_immutable_session_choice_and_neutral_matches_default(self):
|
|
for variant, fw in (("standard", False), ("firmware_2025", True)):
|
|
with self.subTest(variant=variant):
|
|
cp = _params(firmware_2025=fw)
|
|
original = _controller(cp)
|
|
neutral = _controller(cp, Ioniq6Surface.validated(variant, {}))
|
|
default_rows = _trace(original, cp, SEQUENCE)
|
|
neutral_rows = _trace(neutral, cp, SEQUENCE)
|
|
for row, expected in zip(neutral_rows, default_rows, strict=True):
|
|
for value, reference in zip(row, expected, strict=True):
|
|
self.assertAlmostEqual(value, reference, places=10)
|
|
self.assertEqual(selected_policy(original).surface, None)
|
|
self.assertEqual(selected_policy(neutral).surface.variant, variant)
|
|
|
|
def test_tuned_surface_changes_shaping_but_reset_is_deterministic(self):
|
|
for variant, fw in (("standard", False), ("firmware_2025", True)):
|
|
with self.subTest(variant=variant):
|
|
cp = _params(firmware_2025=fw)
|
|
surface = Ioniq6Surface.validated(variant, _TUNED_KNOBS)
|
|
baseline = _trace(_controller(cp), cp, SEQUENCE)
|
|
tuned = _trace(_controller(cp, surface), cp, SEQUENCE)
|
|
repeated = _trace(_controller(cp, surface), cp, SEQUENCE)
|
|
self.assertEqual(tuned, repeated)
|
|
self.assertTrue(any(abs(row[0] - base[0]) > 1e-7 for row, base in zip(tuned, baseline, strict=True)))
|
|
self.assertEqual(tuned[0][0], 0.0)
|
|
self.assertEqual(tuned[-5][0], 0.0) # inactive reset frame
|
|
|
|
def test_invalid_surface_rejected_before_parent_mutation(self):
|
|
cp = _params()
|
|
parent = _controller(cp)
|
|
original = (parent.pid, parent.torque_params.latAccelFactor, selected_policy(parent))
|
|
with self.assertRaises(ValueError):
|
|
Ioniq6TorquePolicy(parent, cp.as_reader(), surface=Ioniq6Surface.validated("firmware_2025", {}))
|
|
self.assertIs(parent.pid, original[0])
|
|
self.assertEqual(parent.torque_params.latAccelFactor, original[1])
|
|
self.assertIs(selected_policy(parent), original[2])
|
|
with self.assertRaises(ValueError):
|
|
Ioniq6TorquePolicy(parent, cp.as_reader(), surface=object())
|
|
other = interfaces[TOYOTA.TOYOTA_COROLLA_TSS2].get_non_essential_params(TOYOTA.TOYOTA_COROLLA_TSS2)
|
|
with self.assertRaises(ValueError):
|
|
Ioniq6TorquePolicy(parent, other.as_reader(), surface=Ioniq6Surface.validated("standard", {}))
|
|
self.assertIs(parent.pid, original[0])
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|