From 3f2dc76051a245f787425445e831b26b25a3112c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 7 Apr 2026 19:51:25 -0500 Subject: [PATCH] prius / bolt / volt tuning | UI Logging --- opendbc_repo/opendbc/car/gm/carcontroller.py | 17 +++- opendbc_repo/opendbc/car/gm/interface.py | 28 ++++++ opendbc_repo/opendbc/car/gm/tests/test_gm.py | 32 ++++++- .../opendbc/car/tests/test_car_interfaces.py | 37 ++++++-- .../opendbc/car/toyota/carcontroller.py | 13 +-- selfdrive/car/car_specific.py | 3 +- selfdrive/car/tests/test_car_interfaces.py | 8 +- selfdrive/car/tests/test_models.py | 42 ++++++--- selfdrive/controls/lib/latcontrol_torque.py | 82 +++++++++++++---- selfdrive/controls/lib/longcontrol.py | 14 +++ selfdrive/controls/tests/test_latcontrol.py | 23 +++-- starpilot/common/testing_grounds.py | 1 + .../assets/components/tools/testing_ground.js | 57 ++++++++++-- system/manager/process.py | 88 +++++++++++++++++++ tools/longitudinal_maneuvers/maneuversd.py | 11 ++- tools/tuning/analyze_bolt_lateral.py | 4 + 16 files changed, 394 insertions(+), 66 deletions(-) diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index a523062a..0fe8868e 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/opendbc_repo/opendbc/car/gm/carcontroller.py @@ -27,10 +27,13 @@ def get_stock_cc_active_for_cancel(CP, CS): return stock_cc_active -def use_interceptor_sng_launch(CP, CS): +def use_interceptor_sng_launch(CP, CS, maneuver_mode=False): # Restrict the fixed standstill-launch gas to actual near-zero motion # so higher accel requests can take over once the car has started moving. - return CS.out.cruiseState.standstill and (CS.out.standstill or CS.out.vEgo < max(CP.vEgoStarting, 0.3)) + launch_speed = max(CP.vEgoStarting, 0.3) + if maneuver_mode: + launch_speed = max(launch_speed, 2.0) + return CS.out.cruiseState.standstill and (CS.out.standstill or CS.out.vEgo < launch_speed) class CarController(CarControllerBase): @@ -67,6 +70,7 @@ class CarController(CarControllerBase): self.pedal_active_last = False self.aego = 0.0 self.maneuver_paddle_mode = "auto" + self.longitudinal_maneuver_mode = False self.params_ = Params() self.is_volt = self.CP.carFingerprint in { @@ -243,6 +247,10 @@ class CarController(CarControllerBase): mode = mode.decode("utf-8", errors="replace") mode = (mode or "auto").strip().lower() self.maneuver_paddle_mode = mode if mode in ("auto", "off", "force") else "auto" + try: + self.longitudinal_maneuver_mode = self.params_.get_bool("LongitudinalManeuverMode") + except UnknownKeyName: + self.longitudinal_maneuver_mode = False kaofui_cars = SDGM_CAR | ASCM_INT | { CAR.CHEVROLET_VOLT, @@ -445,8 +453,11 @@ class CarController(CarControllerBase): # gas interceptor only used for full long control on cars without ACC interceptor_gas_cmd, press_regen_paddle = self.calc_pedal_command(actuators.accel, CC.longActive, CS.out.vEgo) - if self.CP.enableGasInterceptorDEPRECATED and self.apply_gas > self.params.INACTIVE_REGEN and use_interceptor_sng_launch(self.CP, CS): + maneuver_sng_launch = self.longitudinal_maneuver_mode and self.is_volt + if self.CP.enableGasInterceptorDEPRECATED and self.apply_gas > self.params.INACTIVE_REGEN and use_interceptor_sng_launch(self.CP, CS, maneuver_sng_launch): interceptor_gas_cmd = self.params.SNG_INTERCEPTOR_GAS + if maneuver_sng_launch: + interceptor_gas_cmd = max(interceptor_gas_cmd, float(np.interp(actuators.accel, [0.0, 1.0, 2.0], [self.params.SNG_INTERCEPTOR_GAS, 0.11, 0.16]))) self.apply_brake = 0 self.apply_gas = self.params.INACTIVE_REGEN diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index edeb97c4..d5d68203 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -24,6 +24,7 @@ from opendbc.car.gm.values import ( from opendbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params, UnknownKeyName +from openpilot.starpilot.common.testing_grounds import testing_ground TransmissionType = structs.CarParams.TransmissionType NetworkLocation = structs.CarParams.NetworkLocation @@ -76,6 +77,14 @@ VOLT_LIKE_CARS = { CAR.CHEVROLET_MALIBU_HYBRID_CC, } +VOLT_LONG_TEST_TUNE_CARS = { + CAR.CHEVROLET_VOLT, + CAR.CHEVROLET_VOLT_2019, + CAR.CHEVROLET_VOLT_ASCM, + CAR.CHEVROLET_VOLT_CAMERA, + CAR.CHEVROLET_VOLT_CC, +} + BOLT_PEDAL_LONG_CARS = { CAR.CHEVROLET_BOLT_CC_2017, CAR.CHEVROLET_BOLT_CC_2018_2021, @@ -348,6 +357,7 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerLimitTimer = 0.4 + ret.radarTimeStepDEPRECATED = 0.0667 # GM radar runs at 15Hz instead of the standard 20Hz ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking if candidate in ( @@ -557,6 +567,24 @@ class CarInterface(CarInterfaceBase): ret.pcmCruise = False ret.openpilotLongitudinalControl = not disable_openpilot_long + volt_test_tune_active = ( + testing_ground.use_2 and + ret.enableGasInterceptorDEPRECATED and + ret.openpilotLongitudinalControl and + candidate in VOLT_LONG_TEST_TUNE_CARS + ) + if volt_test_tune_active: + # Volt interceptor-long currently falls back to an all-I tune on this path. + # The test-ground tune adds a modest P term, reduces I memory, and uses a + # dedicated starting state so stop-and-go launches do not wind up the PID. + ret.longitudinalTuning.kpBP = [0.0, 4.0, 12.0, 35.0] + ret.longitudinalTuning.kpV = [0.10, 0.08, 0.06, 0.045] + ret.longitudinalTuning.kiBP = [0.0, 4.0, 12.0, 35.0] + ret.longitudinalTuning.kiV = [0.025, 0.035, 0.055, 0.080] + ret.startingState = True + ret.startAccel = 1.15 + ret.vEgoStarting = max(ret.vEgoStarting, 0.35) + elif candidate in CC_ONLY_CAR and not ret.enableGasInterceptorDEPRECATED: ret.flags |= GMFlags.CC_LONG.value ret.alphaLongitudinalAvailable = False diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index d1bc51cc..0301f664 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -1,7 +1,9 @@ import pytest +from types import SimpleNamespace from parameterized import parameterized from opendbc.car.car_helpers import interfaces +import opendbc.car.gm.interface as gm_interface from opendbc.car.common.conversions import Conversions as CV from opendbc.car.gm.fingerprints import FINGERPRINTS from opendbc.car.gm.values import CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, GM_RX_OFFSET @@ -20,6 +22,17 @@ def _empty_fingerprint(): return {bus: {} for bus in range(8)} +def _test_starpilot_toggles(): + return SimpleNamespace( + car_model="", + cluster_offset=1.0, + disable_openpilot_long=False, + force_fingerprint=False, + vEgoStopping=0.5, + volt_sng=False, + ) + + class TestGMFingerprint: @parameterized.expand(FINGERPRINTS.items()) def test_can_fingerprints(self, car_model, fingerprints): @@ -38,6 +51,23 @@ class TestGMInterface: @parameterized.expand(VOLT_CARS) def test_volt_min_steer_speed_is_7_mph(self, car_model): CarInterface = interfaces[car_model] - car_params = CarInterface.get_params(car_model, _empty_fingerprint(), [], alpha_long=False, is_release=False, docs=False) + car_params = CarInterface.get_params(car_model, _empty_fingerprint(), [], alpha_long=False, is_release=False, docs=False, + starpilot_toggles=_test_starpilot_toggles()) assert car_params.minSteerSpeed == pytest.approx(7 * CV.MPH_TO_MS) + + def test_volt_testing_ground_tune_sets_nonzero_p_and_starting_state(self, monkeypatch): + CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM] + fingerprint = _empty_fingerprint() + fingerprint[0][0x201] = 8 # pedal detected + fingerprint[0][0x2FF] = 8 # SASCM detected + + monkeypatch.setattr(gm_interface.testing_ground, "use_2", True, raising=False) + + car_params = CarInterface.get_params(CAR.CHEVROLET_VOLT_ASCM, fingerprint, [], alpha_long=False, is_release=False, docs=False, + starpilot_toggles=_test_starpilot_toggles()) + + assert list(car_params.longitudinalTuning.kpV) == [0.10, 0.08, 0.06, 0.045] + assert list(car_params.longitudinalTuning.kiV) == [0.025, 0.035, 0.055, 0.08] + assert car_params.startingState + assert car_params.startAccel == pytest.approx(1.15) diff --git a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py index 05746c4c..5ce80cbb 100644 --- a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py +++ b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py @@ -1,5 +1,6 @@ import os import math +from types import SimpleNamespace import hypothesis.strategies as st import pytest from hypothesis import Phase, given, settings @@ -29,6 +30,23 @@ DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64] MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '15')) +def get_test_starpilot_toggles() -> SimpleNamespace: + return SimpleNamespace( + car_model="", + cluster_offset=1.0, + disable_openpilot_long=False, + force_fingerprint=False, + frogsgomoo_tweak=False, + lock_doors=False, + reverse_cruise_increase=False, + sng_hack=False, + subaru_sng=False, + unlock_doors=False, + vEgoStopping=0.5, + volt_sng=False, + ) + + def get_fuzzy_car_interface(car_name: str, draw: DrawType) -> CarInterfaceBase: # Fuzzy CAN fingerprints and FW versions to test more states of the CarInterface fingerprint_strategy = st.fixed_dictionaries({0: st.dictionaries(st.integers(min_value=0, max_value=0x800), @@ -53,9 +71,14 @@ def get_fuzzy_car_interface(car_name: str, draw: DrawType) -> CarInterfaceBase: # initialize car interface CarInterface = interfaces[car_name] + starpilot_toggles = get_test_starpilot_toggles() car_params = CarInterface.get_params(car_name, params['fingerprints'], params['car_fw'], - alpha_long=params['alpha_long'], is_release=False, docs=False) - return CarInterface(car_params) + alpha_long=params['alpha_long'], is_release=False, docs=False, + starpilot_toggles=starpilot_toggles) + fp_car_params = CarInterface.get_starpilot_params(car_name, params['fingerprints'], params['car_fw'], car_params, starpilot_toggles) + car_interface = CarInterface(car_params, fp_car_params) + car_interface.starpilot_toggles = starpilot_toggles + return car_interface class TestCarInterfaces: @@ -79,6 +102,7 @@ class TestCarInterfaces: alpha_long=False, is_release=False, docs=False, + starpilot_toggles=get_test_starpilot_toggles(), ) assert pedal_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_NO_ACC.value assert pedal_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_BOLT_2022_PEDAL.value @@ -91,6 +115,7 @@ class TestCarInterfaces: alpha_long=False, is_release=False, docs=False, + starpilot_toggles=get_test_starpilot_toggles(), ) assert not (acc_params.safetyConfigs[0].safetyParam & GMSafetyFlags.FLAG_GM_NO_ACC.value) @@ -132,8 +157,8 @@ class TestCarInterfaces: now_nanos = 0 CC = structs.CarControl().as_reader() for _ in range(10): - car_interface.update([]) - car_interface.apply(CC, now_nanos) + car_interface.update([], car_interface.starpilot_toggles) + car_interface.apply(CC, now_nanos, car_interface.starpilot_toggles) now_nanos += DT_CTRL * 1e9 # 10 ms CC = structs.CarControl() @@ -142,8 +167,8 @@ class TestCarInterfaces: CC.longActive = True CC = CC.as_reader() for _ in range(10): - car_interface.update([]) - car_interface.apply(CC, now_nanos) + car_interface.update([], car_interface.starpilot_toggles) + car_interface.apply(CC, now_nanos, car_interface.starpilot_toggles) now_nanos += DT_CTRL * 1e9 # 10ms # Test radar interface diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 78dac3b2..38fc306a 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -43,14 +43,17 @@ UNLOCK_CMD = b"\x40\x05\x30\x11\x00\x40\x00\x00" def get_long_tune(CP, params): - if CP.carFingerprint in TSS2_CAR: - kiBP = [2., 5.] - kiV = [0.5, 0.25] - else: + kiBP = [2., 5.] + kiV = [0.5, 0.25] + k_f = 1.0 + + if CP.carFingerprint == CAR.TOYOTA_PRIUS: + k_f = 0.0 + elif CP.carFingerprint not in TSS2_CAR: kiBP = [0., 5., 35.] kiV = [3.6, 2.4, 1.5] - return PIDController(0.0, (kiBP, kiV), k_f=1.0, + return PIDController(0.0, (kiBP, kiV), k_f=k_f, pos_limit=params.ACCEL_MAX, neg_limit=params.ACCEL_MIN, rate=1 / (DT_CTRL * 3)) diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index 47341a07..bb0ed331 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -148,7 +148,8 @@ class CarSpecificEvents: self.CP.networkLocation == NetworkLocation.fwdCamera and (self.CP.carFingerprint in GM_STANDSTILL_BRAKE_CAMERA_CARS or self.CP.carFingerprint not in SDGM_CAR) ) - if CS.vEgo < self.CP.minEnableSpeed and not standstill_brake_enable_allowed: + below_min_enable_speed = CS.vEgo < self.CP.minEnableSpeed or getattr(CS, "moving_backward", False) + if below_min_enable_speed and not standstill_brake_enable_allowed: events.add(EventName.belowEngageSpeed) if CS.cruiseState.standstill and not self.CP.autoResumeSng: events.add(EventName.resumeRequired) diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 24d2faa0..1c6e731c 100644 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -35,8 +35,8 @@ class TestCarInterfaces: CC = car.CarControl.new_message(**cc_msg) CC = CC.as_reader() for _ in range(10): - car_interface.update([]) - car_interface.apply(CC, now_nanos) + car_interface.update([], car_interface.starpilot_toggles) + car_interface.apply(CC, now_nanos, car_interface.starpilot_toggles) now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) @@ -45,8 +45,8 @@ class TestCarInterfaces: CC.longActive = True CC = CC.as_reader() for _ in range(10): - car_interface.update([]) - car_interface.apply(CC, now_nanos) + car_interface.update([], car_interface.starpilot_toggles) + car_interface.apply(CC, now_nanos, car_interface.starpilot_toggles) now_nanos += DT_CTRL * 1e9 # 10ms # Test controller initialization diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 94f5b332..10f5356b 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -4,6 +4,7 @@ import pytest import random import unittest # noqa: TID251 from collections import defaultdict, Counter +from types import SimpleNamespace import hypothesis.strategies as st from hypothesis import Phase, given, settings from parameterized import parameterized_class @@ -35,6 +36,23 @@ MAX_EXAMPLES = int(os.environ.get("MAX_EXAMPLES", "300")) CI = os.environ.get("CI", None) is not None +def get_test_starpilot_toggles() -> SimpleNamespace: + return SimpleNamespace( + car_model="", + cluster_offset=1.0, + disable_openpilot_long=False, + force_fingerprint=False, + frogsgomoo_tweak=False, + lock_doors=False, + reverse_cruise_increase=False, + sng_hack=False, + subaru_sng=False, + unlock_doors=False, + vEgoStopping=0.5, + volt_sng=False, + ) + + def get_test_cases() -> list[tuple[str, CarTestRoute | None]]: # build list of test cases test_cases = [] @@ -149,7 +167,10 @@ class TestCarModelBase(unittest.TestCase): cls.openpilot_enabled = cls.car_safety_mode_frame is not None cls.CarInterface = interfaces[cls.platform] - cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False) + cls.starpilot_toggles = get_test_starpilot_toggles() + cls.CP = cls.CarInterface.get_params(cls.platform, cls.fingerprint, car_fw, alpha_long, False, docs=False, + starpilot_toggles=cls.starpilot_toggles) + cls.FPCP = cls.CarInterface.get_starpilot_params(cls.platform, cls.fingerprint, car_fw, cls.CP, cls.starpilot_toggles) assert cls.CP assert cls.CP.carFingerprint == cls.platform @@ -160,7 +181,7 @@ class TestCarModelBase(unittest.TestCase): del cls.can_msgs def setUp(self): - self.CI = self.CarInterface(self.CP.copy()) + self.CI = self.CarInterface(self.CP.copy(), self.FPCP) assert self.CI # TODO: check safetyModel is in release panda build @@ -193,8 +214,8 @@ class TestCarModelBase(unittest.TestCase): CC = structs.CarControl().as_reader() for i, msg in enumerate(self.can_msgs): - CS = self.CI.update(msg) - self.CI.apply(CC, msg[0]) + CS, _ = self.CI.update(msg, self.starpilot_toggles) + self.CI.apply(CC, msg[0], self.starpilot_toggles) # wait max of 2s for low frequency msgs to be seen if i > 250: @@ -266,10 +287,10 @@ class TestCarModelBase(unittest.TestCase): def test_car_controller(car_control): now_nanos = 0 msgs_sent = 0 - CI = self.CarInterface(self.CP) + CI = self.CarInterface(self.CP, self.FPCP) for _ in range(round(10.0 / DT_CTRL)): # make sure we hit the slowest messages - CI.update([]) - _, sendcan = CI.apply(car_control, now_nanos) + CI.update([], self.starpilot_toggles) + _, sendcan = CI.apply(car_control, now_nanos, self.starpilot_toggles) now_nanos += DT_CTRL * 1e9 msgs_sent += len(sendcan) @@ -334,7 +355,7 @@ class TestCarModelBase(unittest.TestCase): self.safety.safety_rx_hook(to_send) can = [(int(time.monotonic() * 1e9), [CanData(address=address, dat=dat, src=bus)])] - CS = self.CI.update(can) + CS, _ = self.CI.update(can, self.starpilot_toggles) if n < 5: # CANParser warmup time continue @@ -386,7 +407,7 @@ class TestCarModelBase(unittest.TestCase): # warm up pass, as initial states may be different for can in self.can_msgs[:300]: - self.CI.update(can) + self.CI.update(can, self.starpilot_toggles) for msg in filter(lambda m: m.src < 64, can[1]): to_send = libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) self.safety.safety_rx_hook(to_send) @@ -396,7 +417,8 @@ class TestCarModelBase(unittest.TestCase): checks = defaultdict(int) vehicle_speed_seen = self.CP.steerControlType == SteerControlType.angle and not self.CP.notCar for idx, can in enumerate(self.can_msgs): - CS = self.CI.update(can).as_reader() + CS, _ = self.CI.update(can, self.starpilot_toggles) + CS = CS.as_reader() for msg in filter(lambda m: m.src < 64, can[1]): to_send = libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) ret = self.safety.safety_rx_hook(to_send) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 3dd640e7..79b3f5a1 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -66,14 +66,23 @@ BOLT_2017_TORQUE_SCALE_RIGHT = [1.0, 1.0, 1.04, 1.03, 1.01, 1.0] BOLT_2018_2021_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 BOLT_2018_2021_STEER_RATIO_TEST_SCALE = 1.01 -BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.10 -BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.065 -BOLT_2018_2021_TORQUE_RISE = 0.24 -BOLT_2018_2021_TORQUE_FALL = 1.8 -BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.55 -BOLT_2018_2021_FRICTION_MULT = 1.11 -BOLT_2018_2021_FRICTION_THRESHOLD_BUMP = 0.018 -BOLT_2018_2021_FRICTION_THRESHOLD_SPEED = 8.0 +BOLT_2018_2021_TORQUE_GAIN_LEFT = 0.085 +BOLT_2018_2021_TORQUE_GAIN_RIGHT = 0.055 +BOLT_2018_2021_TORQUE_ONSET = 0.18 +BOLT_2018_2021_TORQUE_ONSET_WIDTH = 0.08 +BOLT_2018_2021_TORQUE_CUTOFF = 1.05 +BOLT_2018_2021_TORQUE_CUTOFF_WIDTH = 0.24 +BOLT_2018_2021_JERK_TAPER_CUTOFF = 0.42 +BOLT_2018_2021_TRANSITION_SPEED = 8.5 +BOLT_2018_2021_PHASE_SCALE = 0.10 +BOLT_2018_2021_UNWIND_TAPER_GAIN = 0.65 +BOLT_2018_2021_FRICTION_MULT = 1.03 +BOLT_2018_2021_FRICTION_LAT_RISE = 0.24 +BOLT_2018_2021_FRICTION_JERK_RISE = 0.28 +BOLT_2018_2021_TURN_IN_THRESHOLD_REDUCTION = 0.12 +BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE = 0.10 +BOLT_2018_2021_TURN_IN_FRICTION_BOOST = 0.06 +BOLT_2018_2021_UNWIND_FRICTION_REDUCTION = 0.12 def get_friction_threshold(v_ego: float) -> float: @@ -97,27 +106,64 @@ def bolt_2018_2021_lateral_testing_ground_active() -> bool: return testing_ground.use(BOLT_2018_2021_LATERAL_TESTING_GROUND_ID) +def _bolt_2018_2021_sigmoid(x: float) -> float: + return 1.0 / (1.0 + math.exp(-x)) + + +def _bolt_2018_2021_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / BOLT_2018_2021_TRANSITION_SPEED) ** 2) + + +def _bolt_2018_2021_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / BOLT_2018_2021_PHASE_SCALE) + + +def _bolt_2018_2021_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_2018_2021_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / BOLT_2018_2021_FRICTION_JERK_RISE) + return _bolt_2018_2021_low_speed_factor(v_ego) * lat_factor * jerk_factor + + def get_bolt_2018_2021_torque_scale(desired_lateral_accel: float) -> float: if desired_lateral_accel == 0.0: return 1.0 gain = BOLT_2018_2021_TORQUE_GAIN_LEFT if desired_lateral_accel > 0.0 else BOLT_2018_2021_TORQUE_GAIN_RIGHT abs_lateral_accel = abs(desired_lateral_accel) - mid_corner_bump = (1.0 - math.exp(-abs_lateral_accel / BOLT_2018_2021_TORQUE_RISE)) * math.exp(-abs_lateral_accel / BOLT_2018_2021_TORQUE_FALL) - return 1.0 + gain * mid_corner_bump + onset = _bolt_2018_2021_sigmoid((abs_lateral_accel - BOLT_2018_2021_TORQUE_ONSET) / BOLT_2018_2021_TORQUE_ONSET_WIDTH) + cutoff = _bolt_2018_2021_sigmoid((BOLT_2018_2021_TORQUE_CUTOFF - abs_lateral_accel) / BOLT_2018_2021_TORQUE_CUTOFF_WIDTH) + return 1.0 + gain * onset * cutoff -def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: +def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: base_scale = get_bolt_2018_2021_torque_scale(desired_lateral_accel) extra_scale = max(base_scale - 1.0, 0.0) jerk_taper = 1.0 / (1.0 + (abs(desired_lateral_jerk) / BOLT_2018_2021_JERK_TAPER_CUTOFF) ** 2) - return 1.0 + (extra_scale * jerk_taper) + unwind_weight = max(-_bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk), 0.0) + unwind_taper = 1.0 - (BOLT_2018_2021_UNWIND_TAPER_GAIN * unwind_weight * (0.55 + 0.45 * _bolt_2018_2021_low_speed_factor(v_ego))) + return 1.0 + (extra_scale * jerk_taper * max(unwind_taper, 0.0)) -def get_bolt_2018_2021_friction_threshold(v_ego: float) -> float: +def get_bolt_2018_2021_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) - low_speed_bump = BOLT_2018_2021_FRICTION_THRESHOLD_BUMP / (1.0 + (max(v_ego, 0.0) / BOLT_2018_2021_FRICTION_THRESHOLD_SPEED) ** 2) - return base_threshold + low_speed_bump + transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2018_2021_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_2018_2021_TURN_IN_THRESHOLD_REDUCTION * transition_envelope * turn_in_weight) + threshold_scale += BOLT_2018_2021_UNWIND_THRESHOLD_INCREASE * transition_envelope * unwind_weight + return base_threshold * min(max(threshold_scale, 0.82), 1.12) + + +def get_bolt_2018_2021_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = BOLT_2018_2021_FRICTION_MULT + friction_scale += BOLT_2018_2021_TURN_IN_FRICTION_BOOST * transition_envelope * turn_in_weight + friction_scale -= BOLT_2018_2021_UNWIND_FRICTION_REDUCTION * transition_envelope * unwind_weight + return min(max(friction_scale, 0.88), 1.10) class LatControlTorque(LatControl): @@ -229,8 +275,8 @@ class LatControlTorque(LatControl): friction_threshold = get_friction_threshold(CS.vEgo) friction_scale = 1.0 if bolt_2018_2021_test_active: - friction_threshold = get_bolt_2018_2021_friction_threshold(CS.vEgo) - friction_scale = BOLT_2018_2021_FRICTION_MULT + 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) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: @@ -247,7 +293,7 @@ class LatControlTorque(LatControl): if self.is_bolt_2017 and bolt_2017_lateral_testing_ground_active(): output_torque *= get_bolt_2017_torque_scale(setpoint) elif bolt_2018_2021_test_active: - output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk) + output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) pid_log.active = True pid_log.p = float(self.pid.p) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index a93823a8..90eb5aa6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -6,6 +6,7 @@ from openpilot.common.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.common.filter_simple import FirstOrderFilter from opendbc.car.gm.values import CarControllerParams, GMFlags +from openpilot.starpilot.common.testing_grounds import testing_ground CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] clip = np.clip @@ -117,6 +118,9 @@ class LongControl: self.is_gm_pedal_long = bool( CP.brand == "gm" and CP.enableGasInterceptorDEPRECATED and (CP.flags & GMFlags.PEDAL_LONG.value) ) + self.is_volt_interceptor = bool( + CP.brand == "gm" and CP.enableGasInterceptorDEPRECATED and str(CP.carFingerprint).startswith("CHEVROLET_VOLT") + ) def update_mpc_mode(self, experimental_mode): new_mode = 'blended' if experimental_mode else 'acc' @@ -173,6 +177,15 @@ class LongControl: return self.integrator_hold_frames > 0 or sat_pushing_lower or sat_pushing_upper + def _shape_volt_test_tune_integrator(self, error, v_ego): + if not (self.is_volt_interceptor and testing_ground.use_2): + return + + # Bleed stale I quickly when the target reverses against stored integrator. + if self.pid.i * error < 0.0 and abs(error) > 0.05: + bleed = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.86, 0.90, 0.94, 0.97]) + self.pid.i *= bleed + def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] @@ -199,6 +212,7 @@ class LongControl: else: # LongCtrlState.pid error = a_target - CS.aEgo self.update_mpc_mode(self.experimental_mode) + self._shape_volt_test_tune_integrator(error, CS.vEgo) feedforward = a_target * self.feedforward_gain freeze_integrator = self._get_pedal_long_freeze(a_target, error, CS.vEgo, accel_limits) raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index cb2e3ee8..88db5099 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_friction_threshold, get_bolt_2017_torque_scale, get_bolt_2018_2021_dynamic_torque_scale, + get_bolt_2018_2021_friction_scale, get_bolt_2018_2021_friction_threshold, get_bolt_2018_2021_torque_scale, ) @@ -58,16 +59,22 @@ class TestLatControl: assert get_bolt_2018_2021_torque_scale(0.2) > get_bolt_2018_2021_torque_scale(0.08) assert get_bolt_2018_2021_torque_scale(0.4) > get_bolt_2018_2021_torque_scale(-0.4) assert get_bolt_2018_2021_torque_scale(2.0) < get_bolt_2018_2021_torque_scale(0.8) - assert get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.8) < get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.1) + assert get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.8, 20.0) < get_bolt_2018_2021_dynamic_torque_scale(0.4, 0.1, 20.0) + assert get_bolt_2018_2021_dynamic_torque_scale(0.6, -0.6, 8.0) < get_bolt_2018_2021_dynamic_torque_scale(0.6, 0.6, 8.0) def test_bolt_2018_2021_friction_threshold_curve(self): - low = get_bolt_2018_2021_friction_threshold(2.0) - mid = get_bolt_2018_2021_friction_threshold(10.0) - high = get_bolt_2018_2021_friction_threshold(30.0) - assert low > get_friction_threshold(2.0) - assert mid > get_friction_threshold(10.0) - assert high > get_friction_threshold(30.0) - assert (low - get_friction_threshold(2.0)) > (mid - get_friction_threshold(10.0)) > (high - get_friction_threshold(30.0)) + base = get_friction_threshold(6.0) + turn_in = get_bolt_2018_2021_friction_threshold(6.0, 0.7, 0.8) + unwind = get_bolt_2018_2021_friction_threshold(6.0, 0.7, -0.8) + assert turn_in < base < unwind + assert get_bolt_2018_2021_friction_threshold(25.0, 0.7, 0.8) > turn_in + + def test_bolt_2018_2021_friction_scale_curve(self): + base = get_bolt_2018_2021_friction_scale(25.0, 0.7, 0.8) + turn_in = get_bolt_2018_2021_friction_scale(6.0, 0.7, 0.8) + unwind = get_bolt_2018_2021_friction_scale(6.0, 0.7, -0.8) + assert turn_in > base + assert unwind < turn_in 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) diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py index bb353998..e4594679 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -43,6 +43,7 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( "name": "volt test tune", "description": "Volt longitudinal tuning sandbox.", "aLabel": "A - Installed tune", + "bLabel": "B - Test Tune", }, { "id": TESTING_GROUND_3, diff --git a/starpilot/system/the_pond/assets/components/tools/testing_ground.js b/starpilot/system/the_pond/assets/components/tools/testing_ground.js index 46e9ba32..e5246a13 100644 --- a/starpilot/system/the_pond/assets/components/tools/testing_ground.js +++ b/starpilot/system/the_pond/assets/components/tools/testing_ground.js @@ -10,6 +10,34 @@ const state = reactive({ let initialized = false +function getApiUrl(path) { + const normalizedPath = String(path || "").replace(/^\/+/, "") + const currentUrl = new URL(window.location.href) + currentUrl.hash = "" + currentUrl.search = "" + currentUrl.pathname = currentUrl.pathname.replace(/\/+$/, "") || "/" + return new URL(normalizedPath, currentUrl).toString() +} + +async function parseApiPayload(response, fallbackMessage) { + const contentType = String(response.headers.get("content-type") || "").toLowerCase() + + if (contentType.includes("application/json")) { + const payload = await response.json() + return { + payload, + message: payload?.error || payload?.message || fallbackMessage, + } + } + + const text = (await response.text()).trim() + const cleaned = text.replace(/\s+/g, " ").slice(0, 200) + return { + payload: null, + message: cleaned || fallbackMessage, + } +} + function slotId(slot) { return String(slot?.id || "").trim() } @@ -98,7 +126,8 @@ function getSelectedMode() { const selectedSlot = getSelectedSlot() if (!selectedSlot) return "Not active" if (String(state.data?.activeSlot || "").trim() === String(state.selectedSlot || "").trim()) { - return String(state.data?.activeVariant || "").trim().toUpperCase() || getDefaultMode(selectedSlot) + const activeMode = String(state.data?.activeVariant || "").trim().toUpperCase() || getDefaultMode(selectedSlot) + return String(state.data?.activeVariantLabel || "").trim() || toModeLabel(selectedSlot, activeMode) } return "Not active" } @@ -114,10 +143,13 @@ function modeButtonClass(mode) { async function fetchTestingGrounds() { try { - const response = await fetch("/api/testing_grounds") - const payload = await response.json() + const response = await fetch(getApiUrl("api/testing_grounds")) + const { payload, message } = await parseApiPayload(response, "Failed to load testing grounds") if (!response.ok) { - throw new Error(payload.error || response.statusText || "Failed to load testing grounds") + throw new Error(message || response.statusText || "Failed to load testing grounds") + } + if (!payload || typeof payload !== "object") { + throw new Error(message || "Failed to load testing grounds") } state.data = payload @@ -144,21 +176,28 @@ async function applySelection(slotValue, mode, showToast = true) { state.busy = true try { - const response = await fetch("/api/testing_grounds/select", { + const response = await fetch(getApiUrl("api/testing_grounds/select"), { method: "POST", headers: { "Content-Type": "application/json" }, body: JSON.stringify({ slotId: normalizedSlot, variant: normalizedMode }), }) - const payload = await response.json() + const { payload, message } = await parseApiPayload(response, "Failed to update testing ground mode") if (!response.ok) { - throw new Error(payload.error || response.statusText || "Failed to update testing ground mode") + throw new Error(message || response.statusText || "Failed to update testing ground mode") + } + if (!payload || typeof payload !== "object") { + throw new Error(message || "Failed to update testing ground mode") } state.data = payload state.error = "" state.selectedSlot = normalizedSlot if (showToast) { - showSnackbar(payload.message || `Testing Ground ${normalizedSlot} set to ${normalizedMode}.`) + const selectedSlot = Array.isArray(payload.slots) + ? payload.slots.find((slot) => slotId(slot) === normalizedSlot) + : null + const selectedLabel = String(payload.activeVariantLabel || "").trim() || toModeLabel(selectedSlot, normalizedMode) + showSnackbar(payload.message || `Testing Ground ${normalizedSlot} set to ${selectedLabel}.`) } return true } catch (error) { @@ -271,7 +310,7 @@ export function TestingGround() { ${() => getActiveSlot() ? html`

- Currently active: ${getActiveSlot().id}. ${getActiveSlot().name} in mode ${state.data?.activeVariant || "A"}. + Currently active: ${getActiveSlot().id}. ${getActiveSlot().name} in mode ${state.data?.activeVariantLabel || toModeLabel(getActiveSlot(), state.data?.activeVariant || "A")}.

` : ""} diff --git a/system/manager/process.py b/system/manager/process.py index 87af7a02..c4071011 100644 --- a/system/manager/process.py +++ b/system/manager/process.py @@ -4,6 +4,7 @@ import signal import struct import time import subprocess +from pathlib import Path from collections.abc import Callable, ValuesView from abc import ABC, abstractmethod from multiprocessing import Process @@ -22,6 +23,25 @@ from openpilot.common.watchdog import WATCHDOG_FN ENABLE_WATCHDOG = os.getenv("NO_WATCHDOG") is None +def _debug_dump_dir() -> Path: + for candidate in (Path("/data/log"), Path("/tmp")): + if candidate.is_dir() and os.access(candidate, os.W_OK): + return candidate + return Path.cwd() + + +def _read_text_file(path: Path, max_bytes: int = 16384) -> str: + try: + data = path.read_bytes() + except OSError as e: + return f"" + + if len(data) > max_bytes: + data = data[:max_bytes] + b"\n\n" + + return data.decode("utf-8", errors="replace") + + def launcher(proc: str, name: str, nice: int | None = None) -> None: try: if nice is not None: @@ -95,6 +115,73 @@ class ManagerProcess(ABC): self.stop(sig=signal.SIGKILL) self.start() + def capture_watchdog_debug_dump(self, reason: str, dt: float) -> None: + if self.proc is None or self.proc.pid is None: + return + + pid = self.proc.pid + proc_dir = Path(f"/proc/{pid}") + if not proc_dir.exists(): + cloudlog.error(f"watchdog debug dump skipped for {self.name}: /proc/{pid} no longer exists") + return + + dump_path = _debug_dump_dir() / f"{self.name}_watchdog_dump_{pid}_{time.monotonic_ns()}.log" + lines = [ + f"name={self.name}", + f"pid={pid}", + f"dt={dt:.3f}", + f"reason={reason}", + f"wall_time={time.strftime('%Y-%m-%dT%H:%M:%S%z')}", + f"watchdog_file={WATCHDOG_FN}{pid}", + "", + "== /proc/status ==", + _read_text_file(proc_dir / "status"), + "", + "== /proc/wchan ==", + _read_text_file(proc_dir / "wchan", 1024), + "", + "== /proc/syscall ==", + _read_text_file(proc_dir / "syscall", 2048), + "", + ] + + task_dir = proc_dir / "task" + try: + task_entries = sorted(task_dir.iterdir(), key=lambda p: int(p.name)) + except OSError as e: + task_entries = [] + lines.extend([ + "== /proc/task ==", + f"", + "", + ]) + + for entry in task_entries: + lines.extend([ + f"== thread {entry.name} ==", + "-- comm --", + _read_text_file(entry / "comm", 1024), + "", + "-- wchan --", + _read_text_file(entry / "wchan", 1024), + "", + "-- syscall --", + _read_text_file(entry / "syscall", 2048), + "", + "-- status --", + _read_text_file(entry / "status"), + "", + "-- stack --", + _read_text_file(entry / "stack"), + "", + ]) + + try: + dump_path.write_text("\n".join(lines)) + cloudlog.error(f"Wrote watchdog debug dump for {self.name} to {dump_path}") + except OSError as e: + cloudlog.error(f"failed to write watchdog debug dump for {self.name} to {dump_path}: {e}") + def check_watchdog(self, started: bool) -> None: if self.watchdog_max_dt is None or self.proc is None: return @@ -110,6 +197,7 @@ class ManagerProcess(ABC): dt = time.monotonic() - self.last_watchdog_time / 1e9 if dt > self.watchdog_max_dt and ENABLE_WATCHDOG: + self.capture_watchdog_debug_dump(f"watchdog_timeout started={started}", dt) cloudlog.error(f"Watchdog timeout for {self.name} (exitcode {self.proc.exitcode}) restarting ({started=})") self.restart() diff --git a/tools/longitudinal_maneuvers/maneuversd.py b/tools/longitudinal_maneuvers/maneuversd.py index 2057e753..523997dd 100755 --- a/tools/longitudinal_maneuvers/maneuversd.py +++ b/tools/longitudinal_maneuvers/maneuversd.py @@ -143,6 +143,15 @@ def build_maneuvers(): ] +def should_force_stop_maneuver(maneuver: Maneuver | None, support, v_ego: float, CP) -> bool: + if maneuver is None or maneuver.description != "come to stop" or not support.expectedToReachZero: + return False + + # Hand off to the stopping state near the end of the run so EV creep / hold + # behavior does not leave the suite hovering just above zero forever. + return v_ego <= max(CP.vEgoStarting + 0.1, 1.5) + + def main(): config_realtime_process(5, Priority.CTRL_LOW) @@ -195,7 +204,7 @@ def main(): pm.send('alertDebug', alert_msg) longitudinalPlan.aTarget = accel - longitudinalPlan.shouldStop = v_ego < CP.vEgoStopping and accel < 1e-2 + longitudinalPlan.shouldStop = should_force_stop_maneuver(maneuver, support, v_ego, CP) or (v_ego < CP.vEgoStopping and accel < 1e-2) longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2'] longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2'] diff --git a/tools/tuning/analyze_bolt_lateral.py b/tools/tuning/analyze_bolt_lateral.py index 7bcff993..ea210542 100644 --- a/tools/tuning/analyze_bolt_lateral.py +++ b/tools/tuning/analyze_bolt_lateral.py @@ -51,6 +51,7 @@ def summarize_control_samples(samples: list[ControlSample]) -> None: torque_cmd = np.array([s.torque_cmd for s in samples]) base = lat_active & (~steering_pressed) & (v > 8.0) + transition_base = lat_active & (~steering_pressed) & (v > 4.0) & (~saturated) masks = ( ("all", base), ("all_non_sat", base & (~saturated)), @@ -59,6 +60,9 @@ def summarize_control_samples(samples: list[ControlSample]) -> None: ("center", base & (~saturated) & (np.abs(desired) < 0.1)), ("steady_left", base & (~saturated) & (desired >= 0.1) & (np.abs(jerk) < 0.2)), ("steady_right", base & (~saturated) & (desired <= -0.1) & (np.abs(jerk) < 0.2)), + ("low_speed_sharp", transition_base & (v < 14.0) & (np.abs(desired) >= 0.4) & (np.abs(jerk) >= 0.35)), + ("turn_in", transition_base & (v < 14.0) & (np.abs(desired) >= 0.4) & (np.abs(jerk) >= 0.35) & ((desired * jerk) > 0.0)), + ("unwind", transition_base & (v < 14.0) & (np.abs(desired) >= 0.4) & (np.abs(jerk) >= 0.35) & ((desired * jerk) < 0.0)), ) print("\nControlsState tracking:")