From a22e8c963ab195c75702df0078546dea831c84c1 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 8 Apr 2026 11:02:31 -0500 Subject: [PATCH] 2022 tune --- selfdrive/controls/lib/latcontrol_torque.py | 104 +++++++++++++++++++- selfdrive/controls/tests/test_latcontrol.py | 34 +++++++ starpilot/common/testing_grounds.py | 8 +- 3 files changed, 141 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 101a1bc8aa..7a583d92d6 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -97,6 +97,31 @@ BOLT_2018_2021_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_LEFT = 0.14 BOLT_2018_2021_UNWIND_FRICTION_REDUCTION_RIGHT = 0.19 +BOLT_2022_2023_LATERAL_TESTING_GROUND_ID = testing_ground.id_5 +BOLT_2022_2023_FF_GAIN_LEFT = 0.11 +BOLT_2022_2023_FF_GAIN_RIGHT = 0.08 +BOLT_2022_2023_FF_ONSET = 0.12 +BOLT_2022_2023_FF_ONSET_WIDTH = 0.07 +BOLT_2022_2023_FF_CUTOFF = 1.35 +BOLT_2022_2023_FF_CUTOFF_WIDTH = 0.28 +BOLT_2022_2023_TRANSITION_SPEED = 9.0 +BOLT_2022_2023_PHASE_SCALE = 0.12 +BOLT_2022_2023_TURN_IN_BOOST_LEFT = 0.05 +BOLT_2022_2023_TURN_IN_BOOST_RIGHT = 0.10 +BOLT_2022_2023_UNWIND_TAPER_LEFT = 0.24 +BOLT_2022_2023_UNWIND_TAPER_RIGHT = 0.20 +BOLT_2022_2023_FRICTION_MULT = 1.15 +BOLT_2022_2023_FRICTION_LAT_RISE = 0.22 +BOLT_2022_2023_FRICTION_JERK_RISE = 0.26 +BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.08 +BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.11 +BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.16 +BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.13 +BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT = 0.04 +BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 +BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT = 0.17 +BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT = 0.14 + def get_friction_threshold(v_ego: float) -> float: # Keep the speed-scaled friction threshold behavior. @@ -221,6 +246,78 @@ def get_bolt_2018_2021_friction_scale(v_ego: float, desired_lateral_accel: float return min(max(friction_scale, 0.88), 1.10) +def bolt_2022_2023_lateral_testing_ground_active() -> bool: + return testing_ground.use(BOLT_2022_2023_LATERAL_TESTING_GROUND_ID) + + +def _bolt_2022_2023_sigmoid(x: float) -> float: + return 1.0 / (1.0 + math.exp(-x)) + + +def _bolt_2022_2023_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2022_2023_TRANSITION_SPEED) ** 2) + + +def _bolt_2022_2023_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2022_2023_PHASE_SCALE) + + +def _bolt_2022_2023_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _bolt_2022_2023_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / BOLT_2022_2023_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2022_2023_FRICTION_JERK_RISE) + return _bolt_2022_2023_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_FF_GAIN_LEFT, BOLT_2022_2023_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _bolt_2022_2023_sigmoid((abs_lateral_accel - BOLT_2022_2023_FF_ONSET) / BOLT_2022_2023_FF_ONSET_WIDTH) + cutoff = _bolt_2022_2023_sigmoid((BOLT_2022_2023_FF_CUTOFF - abs_lateral_accel) / BOLT_2022_2023_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + low_speed_factor = _bolt_2022_2023_low_speed_factor(v_ego) + phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + turn_in_boost = 1.0 + (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_BOOST_LEFT, BOLT_2022_2023_TURN_IN_BOOST_RIGHT) * + turn_in_weight * low_speed_factor) + unwind_taper = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_TAPER_LEFT, BOLT_2022_2023_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.55 + 0.45 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT, BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT, BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.84), 1.14) + + +def get_bolt_2022_2023_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = BOLT_2022_2023_FRICTION_MULT + friction_scale += (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_LEFT, BOLT_2022_2023_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_bolt_2022_2023_side_value(desired_lateral_accel, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_LEFT, BOLT_2022_2023_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.92), 1.22) + + class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) @@ -326,10 +423,15 @@ class LatControlTorque(LatControl): ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL], [self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos]) ff *= ff_scale + bolt_2022_2023_test_active = self.is_bolt_2022_2023 and bolt_2022_2023_lateral_testing_ground_active() bolt_2018_2021_test_active = self.is_bolt_2018_2021 and bolt_2018_2021_lateral_testing_ground_active() friction_threshold = get_friction_threshold(CS.vEgo) friction_scale = 1.0 - if bolt_2018_2021_test_active: + if bolt_2022_2023_test_active: + ff *= get_bolt_2022_2023_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_bolt_2022_2023_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = get_bolt_2022_2023_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + elif bolt_2018_2021_test_active: friction_threshold = get_bolt_2018_2021_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_bolt_2018_2021_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 2fffdbe3e5..e1eda53cff 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -17,6 +17,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_friction_threshold, get_bolt_2017_base_torque_scale, get_bolt_2017_torque_scale, + get_bolt_2022_2023_ff_scale, + get_bolt_2022_2023_friction_scale, + get_bolt_2022_2023_friction_threshold, get_bolt_2018_2021_dynamic_torque_scale, get_bolt_2018_2021_friction_scale, get_bolt_2018_2021_friction_threshold, @@ -85,6 +88,29 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind + def test_bolt_2022_2023_ff_scale_curve(self): + assert get_bolt_2022_2023_ff_scale(0.0, 0.0, 20.0) == 1.0 + assert get_bolt_2022_2023_ff_scale(0.5, 0.0, 20.0) > get_bolt_2022_2023_ff_scale(-0.5, 0.0, 20.0) + assert get_bolt_2022_2023_ff_scale(-0.6, -0.7, 8.0) > get_bolt_2022_2023_ff_scale(-0.6, 0.0, 8.0) + assert get_bolt_2022_2023_ff_scale(0.6, -0.7, 8.0) < get_bolt_2022_2023_ff_scale(0.6, 0.0, 8.0) + + def test_bolt_2022_2023_friction_threshold_curve(self): + base = get_friction_threshold(6.0) + left_turn_in = get_bolt_2022_2023_friction_threshold(6.0, 0.7, 0.8) + right_turn_in = get_bolt_2022_2023_friction_threshold(6.0, -0.7, -0.8) + left_unwind = get_bolt_2022_2023_friction_threshold(6.0, 0.7, -0.8) + right_unwind = get_bolt_2022_2023_friction_threshold(6.0, -0.7, 0.8) + assert right_turn_in < left_turn_in < base < right_unwind < left_unwind + + def test_bolt_2022_2023_friction_scale_curve(self): + base = get_bolt_2022_2023_friction_scale(25.0, 0.7, 0.8) + left_turn_in = get_bolt_2022_2023_friction_scale(6.0, 0.7, 0.8) + right_turn_in = get_bolt_2022_2023_friction_scale(6.0, -0.7, -0.8) + left_unwind = get_bolt_2022_2023_friction_scale(6.0, 0.7, -0.8) + right_unwind = get_bolt_2022_2023_friction_scale(6.0, -0.7, 0.8) + assert right_turn_in > left_turn_in > base + assert base > right_unwind > left_unwind + def test_bolt_2017_testing_ground_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2017) monkeypatch.setattr(latcontrol_torque, "bolt_2017_lateral_testing_ground_active", lambda: True) @@ -101,6 +127,14 @@ class TestLatControl: assert lac_log.active + def test_bolt_2022_2023_testing_ground_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_ACC_2022_2023) + monkeypatch.setattr(latcontrol_torque, "bolt_2022_2023_lateral_testing_ground_active", lambda: True) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_ACC_2022_2023, LatControlTorque)]) def test_saturation(self, car_name, controller): diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py index e4594679fa..698e185e24 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -61,10 +61,10 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( }, { "id": TESTING_GROUND_5, - "name": "Unused", - "description": "", - "aLabel": "A", - "bLabel": "B", + "name": "Bolt 22-23 Lat Tune", + "description": "Bolt 2022-2023 lateral torque A/B sandbox.", + "aLabel": "A - Installed tune", + "bLabel": "B - 2022 lateral test", }, { "id": TESTING_GROUND_6,