From a19ed91f616735193e41c905f7f33b52e6061434 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 27 Sep 2026 11:01:27 -0500 Subject: [PATCH] Tunes and Bugfixes --- common/params_keys.h | 1 + .../opendbc/car/hyundai/carcontroller.py | 2 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 25 ++++ .../car/hyundai/tests/test_ray_pedal.py | 6 + opendbc_repo/opendbc/car/interfaces.py | 6 +- .../opendbc/safety/tests/test_hyundai.py | 17 +++ selfdrive/controls/controlsd.py | 13 ++ .../controls/lib/latcontrol_vehicle_tunes.py | 60 ++++++++++ selfdrive/controls/lib/longcontrol.py | 10 +- .../controls/lib/longcontrol_vehicle_tunes.py | 15 +++ .../tests/test_gv70_highway_stabilizer.py | 63 ++++++++++ selfdrive/controls/tests/test_longcontrol.py | 60 ++++++++++ .../controls/tests/test_starpilot_planner.py | 14 +++ .../ui/layouts/settings/starpilot/lateral.py | 7 ++ selfdrive/ui/lib/starpilot_state.py | 3 + .../common/assets/device_settings_layout.json | 11 ++ starpilot/common/starpilot_variables.py | 3 + starpilot/controls/starpilot_card.py | 29 ++++- starpilot/controls/starpilot_planner.py | 2 - .../controls/tests/test_starpilot_card.py | 113 ++++++++++++++++++ .../tests/test_device_settings_layout.py | 9 ++ 21 files changed, 462 insertions(+), 7 deletions(-) create mode 100644 selfdrive/controls/tests/test_gv70_highway_stabilizer.py diff --git a/common/params_keys.h b/common/params_keys.h index 409cd182fe..2719e0436a 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -744,6 +744,7 @@ inline static std::unordered_map keys = { {"SubaruAvhStartup", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"SubaruRedneckCruise", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}}, + {"TeslaAOLDisengageOnBrake", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, {"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}}, {"TetheringEnabled", {PERSISTENT, INT, "0", "0", 0}}, diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 667f16f6e8..ba4ee8fb5a 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -834,7 +834,7 @@ class CarController(CarControllerBase): not CS.out.gasPressed and not CS.out.brakePressed) if pedal_active: set_speed = hud_control.setSpeed - if not np.isfinite(set_speed) or not 1.0 <= set_speed <= 40.0: + if not np.isfinite(set_speed) or set_speed < 1.0: self._ray_pedal_gas_last = 0.0 else: speed_error = set_speed - CS.out.vEgo diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index ec49bb1a8b..ff05eaddbf 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -1086,6 +1086,31 @@ class TestHyundaiFingerprint: assert not (FPCP.flags & HyundaiStarPilotFlags.HAS_LKAS12) + @pytest.mark.parametrize("length, expected", ((6, True), (8, False))) + def test_stinger_only_replaces_six_byte_lkas12(self, length, expected): + fingerprint = gen_empty_fingerprint() + fingerprint[2][0x53E] = length + CP = CarInterface.get_params(CAR.KIA_STINGER_2022, fingerprint, [], True, False, False, None) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_STINGER_2022, fingerprint, [], CP, get_test_toggles()) + + assert bool(FPCP.flags & HyundaiStarPilotFlags.HAS_LKAS12) is expected + + @pytest.mark.parametrize("alpha_long, main_aol, expected", ( + (True, True, True), (True, False, True), (False, True, False), + )) + def test_stinger_aol_latches_lkas_after_long_engagement(self, alpha_long, main_aol, expected): + toggles = get_test_toggles() + toggles.always_on_lateral_main = main_aol + fingerprint = gen_empty_fingerprint() + CP = CarInterface.get_params(CAR.KIA_STINGER_2022, fingerprint, [], alpha_long, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_STINGER_2022, fingerprint, [], CP, toggles) + + assert bool(FPCP.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE) is expected + + sonata_cp = CarInterface.get_params(CAR.HYUNDAI_SONATA, fingerprint, [], alpha_long, False, False, toggles) + sonata_fpcp = CarInterface.get_starpilot_params(CAR.HYUNDAI_SONATA, fingerprint, [], sonata_cp, toggles) + assert not (sonata_fpcp.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE) + def test_ray_ev_does_not_treat_eight_byte_485_as_lfa(self): fingerprint = gen_empty_fingerprint() fingerprint[2][0x485] = 8 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py b/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py index af9148cdc7..96be49223c 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_ray_pedal.py @@ -235,6 +235,12 @@ def test_ray_controller_heartbeats_and_only_actuates_when_ready(speed): CS.out.vEgo = 12.0 hud.setSpeed = 8.0 / 3.6 assert pedal_msg(-0.3, 472)[:4] == bytes(4) + hud.setSpeed = 145.0 / 3.6 + assert pedal_msg(1.5, 476)[4] & 0x80 + assert controller._ray_pedal_gas_last == pytest.approx(0.02) + assert pedal_msg(1.5, 480)[4] & 0x80 + assert controller._ray_pedal_gas_last == pytest.approx(0.04) + assert pedal_msg(-0.3, 484)[:4] == bytes(4) @pytest.mark.parametrize("candidate", [CAR.KIA_RAY_EV, CAR.HYUNDAI_KONA_EV_NON_SCC]) diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index ece8506250..b8f585ff49 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -244,7 +244,8 @@ class CarInterfaceBase(ABC): if 0x1FA in fingerprint[CAN.ECAN]: fp_ret.flags |= HyundaiStarPilotFlags.SPEED_LIMIT_AVAILABLE.value - if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2]: + if candidate != HYUNDAI.KIA_RAY_EV and not (CP.flags & HyundaiFlags.CANFD) and 0x53E in fingerprint[2] and \ + (candidate != HYUNDAI.KIA_STINGER_2022 or fingerprint[2][0x53E] == 6): fp_ret.flags |= HyundaiStarPilotFlags.HAS_LKAS12.value fp_ret.redneckCruiseAvailable = (bool(CP.flags & HyundaiFlags.NON_SCC) and @@ -275,6 +276,9 @@ class CarInterfaceBase(ABC): if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID: fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value + if candidate == HYUNDAI.KIA_STINGER_2022 and CP.openpilotLongitudinalControl: + fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value + # The refresh Elantra's safety mapping comes from the resolved Galaxy # toggle above, not from this legacy persisted-parameter fallback. if candidate != HYUNDAI.HYUNDAI_ELANTRA_HEV_2024 and \ diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index b82cccdd79..a0c848abc3 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -635,6 +635,23 @@ class TestHyundaiLongitudinalAolLkasOnEngageSafety(HyundaiAolLkasOnEngageBase, T HyundaiSafetyFlags.LONG | HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE) self.safety.init_tests() + def test_main_off_after_brake_keeps_lateral_permission(self): + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + + self._rx(self._button_msg(Buttons.NONE, main_button=1)) + self._rx(self._button_msg(Buttons.NONE, main_button=0)) + self._rx(self._button_msg(Buttons.SET)) + self._rx(self._button_msg(Buttons.NONE)) + self._rx(self._user_brake_msg(True)) + self._rx(self._button_msg(Buttons.NONE, main_button=1)) + self._rx(self._button_msg(Buttons.NONE, main_button=0)) + + self.assertFalse(self.safety.get_controls_allowed()) + self.assertFalse(self.safety.get_acc_main_on()) + self.assertTrue(self.safety.get_lkas_on()) + self._set_prev_torque(0) + self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP))) + class TestHyundaiLongitudinalAolMainLkasOnEngageSafety(TestHyundaiLongitudinalSafety): def setUp(self): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0fdf7c97cb..320eea60a9 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -31,6 +31,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.steering_saturation import is_angle_steering_limited from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import GENESIS_GV70_CARS, GenesisGV70HighwayCommandStabilizer from openpilot.selfdrive.controls.lib.latcontrol_torque import ( BOLT_2018_2021_STEER_RATIO_TEST_SCALE, LatControlTorque, @@ -426,6 +427,9 @@ class Controls: self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL) elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL) + self.gv70_highway_stabilizer = (GenesisGV70HighwayCommandStabilizer() + if self.CP.carFingerprint in GENESIS_GV70_CARS and self.CP.lateralTuning.which() == 'torque' + else None) self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan']) @@ -747,6 +751,15 @@ class Controls: bool(CS.leftBlinker or CS.rightBlinker), bool(CS.steeringPressed)) + if self.gv70_highway_stabilizer is not None: + stabilize_gv70 = (CC.latActive and isinstance(self.LaC, LatControlTorque) and + not CS.steeringPressed and not CS.leftBlinker and not CS.rightBlinker and + not self.starpilot_toggles.lane_centering and + model_v2.meta.laneChangeState == LaneChangeState.off and + self.sm.all_checks(['modelV2'])) + new_desired_curvature = self.gv70_highway_stabilizer.update( + new_desired_curvature, CS.vEgo, stabilize_gv70, DT_CTRL) + jerk_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: set_jerk = self.starpilot_toggles.lane_change_jerk_factor diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index d2ec1f2d1f..f18513d09c 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -1,4 +1,5 @@ import ast +from collections import deque import json import math import numpy as np @@ -274,6 +275,14 @@ GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE = 0.04 GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065 +GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP = [40.0 * CV.MPH_TO_MS, 50.0 * CV.MPH_TO_MS] +GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [0.45, 0.65] +GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC = 0.85 +GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC = 0.35 +GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT = 0.06 +GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW = 4.0 +GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION = 0.70 +GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA = 0.20 GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10 GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION = 0.50 @@ -3291,6 +3300,57 @@ def get_genesis_gv70_stabilized_output(output_torque: float, prev_output_torque: return float(output_torque + speed_weight * (smoothed_output - output_torque)) +class GenesisGV70HighwayCommandStabilizer: + def __init__(self) -> None: + self.reset() + + def reset(self) -> None: + self.baseline: float | None = None + self.last_sign = 0 + self.reversals: deque[float] = deque() + self.elapsed = 0.0 + self.blend = 0.0 + + def update(self, curvature: float, v_ego: float, enabled: bool, dt: float) -> float: + if not enabled or v_ego <= GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP[0] or not math.isfinite(curvature): + self.reset() + return curvature + + self.elapsed += dt + lateral_accel = curvature * v_ego ** 2 + if self.baseline is None: + self.baseline = lateral_accel + self.baseline += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC + dt) * (lateral_accel - self.baseline) + residual = lateral_accel - self.baseline + + if abs(lateral_accel) >= GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP[1]: + self.last_sign = 0 + self.reversals.clear() + self.blend = 0.0 + return curvature + + sign = 0 + if residual > GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT: + sign = 1 + elif residual < -GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT: + sign = -1 + if sign and sign != self.last_sign: + if self.last_sign: + self.reversals.append(self.elapsed) + self.last_sign = sign + while self.reversals and self.elapsed - self.reversals[0] > GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_WINDOW: + self.reversals.popleft() + + speed_weight = float(np.interp(v_ego, GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP, [0.0, 1.0])) + target_blend = speed_weight if len(self.reversals) >= 3 else 0.0 + self.blend += dt / (GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC + dt) * (target_blend - self.blend) + center_weight = float(np.interp(abs(lateral_accel), GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP, [1.0, 0.0])) + correction = float(np.clip(GENESIS_GV70_HIGHWAY_STABILIZER_REDUCTION * residual, + -GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA, + GENESIS_GV70_HIGHWAY_STABILIZER_MAX_DELTA)) + return float((lateral_accel - self.blend * center_weight * correction) / v_ego ** 2) + + def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: base_threshold = get_standard_friction_threshold(v_ego) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 985e49b9ec..00d98861ef 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -161,7 +161,7 @@ class LongControl: if not preserve_stop_release: self.stop_release_counter = 0 - def _stop_release_ready(self, CS, a_target, should_stop, has_lead, starpilot_toggles): + def _stop_release_ready(self, CS, a_target, should_stop, has_lead, starpilot_toggles, leads=None): if self.long_control_state != LongCtrlState.stopping: self.stop_release_counter = 0 return True @@ -170,6 +170,10 @@ class LongControl: self.stop_release_counter = 0 return False + if self.vehicle_tuning.hold_toyota_corolla_for_stopped_lead(CS.vEgo, leads): + self.stop_release_counter = 0 + return False + if CS.vEgo > starpilot_toggles.vEgoStarting: self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) return True @@ -247,7 +251,9 @@ class LongControl: ) previous_long_control_state = self.long_control_state - allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, has_lead, starpilot_toggles) + allow_stopping_release = self._stop_release_ready( + CS, a_target, should_stop, has_lead, starpilot_toggles, leads=leads, + ) self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, should_stop, CS.brakePressed, CS.cruiseState.standstill, starpilot_toggles, diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index ed1ccc3b4c..a622ab588a 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py @@ -46,6 +46,9 @@ TOYOTA_COROLLA_TARGET_FILTER_UP_TAU = 0.30 TOYOTA_COROLLA_TARGET_FILTER_DOWN_TAU = 0.18 TOYOTA_COROLLA_TARGET_FILTER_BRAKE_BYPASS = -0.75 TOYOTA_COROLLA_TARGET_FILTER_DROP_BYPASS = 0.45 +TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_EGO_SPEED = 0.5 +TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_DISTANCE = 8.0 +TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_SPEED = 0.35 VOLT_CRUISE_INTEGRATOR_MIN_SPEED = 8.0 VOLT_CRUISE_INTEGRATOR_TARGET_MAX = 0.12 VOLT_CRUISE_INTEGRATOR_ERROR_MAX = 0.12 @@ -453,6 +456,18 @@ class LongControlVehicleTuning: self.toyota_corolla_filtered_a_target += alpha * (float(a_target) - self.toyota_corolla_filtered_a_target) return self.toyota_corolla_filtered_a_target + def hold_toyota_corolla_for_stopped_lead(self, v_ego, leads=None): + if not self.is_toyota_corolla_tss2 or v_ego > TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_EGO_SPEED: + return False + + return any( + bool(getattr(lead, "status", False)) and + 0.0 < float(getattr(lead, "dRel", 0.0)) <= TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_DISTANCE and + abs(float(getattr(lead, "yRel", 0.0))) <= 1.75 and + abs(float(getattr(lead, "vLead", 0.0))) <= TOYOTA_COROLLA_STOPPED_LEAD_HOLD_MAX_SPEED + for lead in (leads or ()) + ) + def get_integrator_freeze(self, last_output_accel, a_target, error, v_ego, accel_limits): volt_test_tune_handoff = self.is_volt and testing_ground.use_2 diff --git a/selfdrive/controls/tests/test_gv70_highway_stabilizer.py b/selfdrive/controls/tests/test_gv70_highway_stabilizer.py new file mode 100644 index 0000000000..d395bbe7ee --- /dev/null +++ b/selfdrive/controls/tests/test_gv70_highway_stabilizer.py @@ -0,0 +1,63 @@ +import math + +import numpy as np +import pytest + +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR +from openpilot.common.constants import CV +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( + GENESIS_GV70_CARS, + GenesisGV70HighwayCommandStabilizer, +) + + +def update_accel(stabilizer: GenesisGV70HighwayCommandStabilizer, lateral_accel: float, + speed: float = 30.0, enabled: bool = True) -> float: + return stabilizer.update(lateral_accel / speed ** 2, speed, enabled, 0.01) * speed ** 2 + + +def test_only_electrified_gv70_selected(): + assert GENESIS_GV70_CARS == (HYUNDAI_CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN,) + assert HYUNDAI_CAR.GENESIS_G70_2020 not in GENESIS_GV70_CARS + + +@pytest.mark.parametrize('speed', [10.0, 30.0 * CV.MPH_TO_MS, 40.0 * CV.MPH_TO_MS]) +def test_no_change_at_low_speed(speed): + stabilizer = GenesisGV70HighwayCommandStabilizer() + for i in range(1200): + accel = 0.3 * math.sin(2.0 * math.pi * 0.5 * i * 0.01) + assert update_accel(stabilizer, accel, speed) == pytest.approx(accel) + + +def test_repeated_highway_reversals_are_bounded_and_damped(): + stabilizer = GenesisGV70HighwayCommandStabilizer() + raw, shaped = [], [] + for i in range(1200): + accel = 0.15 + 0.3 * math.sin(2.0 * math.pi * 0.5 * i * 0.01) + raw.append(accel) + shaped.append(update_accel(stabilizer, accel)) + + assert np.std(shaped[600:]) < 0.75 * np.std(raw[600:]) + assert np.max(np.abs(np.array(raw) - np.array(shaped))) <= 0.20 + 1e-6 + + +def test_sustained_curve_is_unchanged(): + stabilizer = GenesisGV70HighwayCommandStabilizer() + curve = np.concatenate((np.linspace(0.0, 0.55, 150), np.full(300, 0.55), np.linspace(0.55, 0.0, 150))) + for accel in curve: + assert update_accel(stabilizer, float(accel)) == pytest.approx(accel) + + +def test_strong_turn_and_driver_input_reset_stabilizer(): + stabilizer = GenesisGV70HighwayCommandStabilizer() + for i in range(1000): + accel = 0.3 * math.sin(2.0 * math.pi * 0.5 * i * 0.01) + update_accel(stabilizer, accel) + + for _ in range(100): + assert update_accel(stabilizer, 0.8) == pytest.approx(0.8) + for _ in range(100): + assert update_accel(stabilizer, 0.4) == pytest.approx(0.4) + + assert update_accel(stabilizer, -0.3, enabled=False) == pytest.approx(-0.3) + assert update_accel(stabilizer, 0.3) == pytest.approx(0.3) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index f4d7992bdb..299f23aab2 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -558,6 +558,66 @@ def test_update_releases_stopping_on_small_sustained_positive_target(): assert lc.long_control_state == LongCtrlState.starting +def test_corolla_holds_stop_until_close_lead_moves(): + CP = make_longcontrol_cp( + brand="toyota", + carFingerprint=TOYOTA_CAR.TOYOTA_COROLLA_TSS2, + ) + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.stopping + CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + lead = SimpleNamespace(status=True, dRel=6.2, yRel=0.0, vLead=0.1) + + for _ in range(40): + output_accel = lc.update( + active=True, + CS=CS, + a_target=0.18, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + has_lead=True, + leads=(lead, None), + ) + assert lc.long_control_state == LongCtrlState.stopping + assert output_accel <= 0.0 + + lead.vLead = 0.6 + lc.update( + active=True, + CS=CS, + a_target=0.18, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + has_lead=True, + leads=(lead, None), + ) + assert lc.long_control_state == LongCtrlState.pid + + +def test_non_corolla_releases_stop_with_stopped_lead_as_before(): + CP = make_longcontrol_cp(brand="honda") + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.stopping + CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + lead = SimpleNamespace(status=True, dRel=6.2, yRel=0.0, vLead=0.1) + + lc.update( + active=True, + CS=CS, + a_target=0.18, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + has_lead=True, + leads=(lead, None), + ) + assert lc.long_control_state == LongCtrlState.pid + + def test_corolla_tss2_stop_release_ramps_positive_target(): CP = make_longcontrol_cp( brand="toyota", diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index fddc1a95ff..b3630520c1 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/selfdrive/controls/tests/test_starpilot_planner.py @@ -6,6 +6,7 @@ import pytest from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.drive_helpers import get_lateral_active from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner, get_force_stop_jerk_scale from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_hyundai_canfd_scc_jerk_limits, @@ -162,6 +163,19 @@ def test_standstill_without_turn_signal_keeps_lateral_allowed(monkeypatch): planner.shutdown() +@pytest.mark.parametrize("left_blinker", [False, True]) +def test_pause_steering_below_speed_includes_standstill(monkeypatch, left_blinker): + planner = make_planner(monkeypatch) + + try: + toggles = make_toggles(pause_lateral_below_speed=35.0 * CV.MPH_TO_MS, pause_lateral_below_signal=False) + planner.update(0.0, False, make_sm(planner, frame=1, v_ego=0.0, left_blinker=left_blinker, standstill=True), toggles) + assert planner.lateral_check is False + assert not get_lateral_active(False, False, True, False, False, True, True, planner.lateral_check) + finally: + planner.shutdown() + + def test_manual_lateral_pause_blocks_lateral_while_cruise_is_enabled(monkeypatch): planner = make_planner(monkeypatch) diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 0c861b5b34..03032e41e7 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -117,6 +117,13 @@ class StarPilotLateralLayout(_SettingsPage): # ── 1. Steering Behavior ── self._behavior_rows = [ + SettingRow( + "TeslaAOLDisengageOnBrake", "toggle", tr_noop("Disengage AOL on Brake"), + subtitle=tr_noop("Keep steering off after pressing the brake until openpilot is engaged again."), + get_state=lambda: p.get_bool("TeslaAOLDisengageOnBrake"), + set_state=lambda s: p.put_bool("TeslaAOLDisengageOnBrake", s), + visible=lambda: aol_on() and cs.isTesla, + ), SettingRow( "PauseAOLOnBrake", "value", tr_noop("Pause AOL On Brake"), subtitle=tr_noop("Pause AOL below this speed while brake is pressed."), diff --git a/selfdrive/ui/lib/starpilot_state.py b/selfdrive/ui/lib/starpilot_state.py index 896da931ed..a736156658 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/selfdrive/ui/lib/starpilot_state.py @@ -20,6 +20,7 @@ class StarPilotCarState: isJeep: bool = False isToyota: bool = False isSubaru: bool = False + isTesla: bool = False isVolt: bool = False isBolt: bool = False isAngleCar: bool = False @@ -95,6 +96,7 @@ class StarPilotState: self.car_state.isHKG = brand == "hyundai" self.car_state.isJeep = brand == "chrysler" and fallback_model_str.startswith("JEEP_") self.car_state.isSubaru = brand == "subaru" + self.car_state.isTesla = brand == "tesla" self.car_state.isToyota = brand == "toyota" self.car_state.isHKGCanFd = False self.car_state.hasModeStarButtons = False @@ -170,6 +172,7 @@ class StarPilotState: self.car_state.isHKGCanFd = self.car_state.isHKG and safety_model == car.CarParams.SafetyModel.hyundaiCanfd self.car_state.isJeep = car_make == "chrysler" and car_fingerprint.startswith("JEEP_") self.car_state.isSubaru = car_make == "subaru" + self.car_state.isTesla = car_make == "tesla" self.car_state.isToyota = car_make == "toyota" self.car_state.isTSK = bool(self._safe_get(CP, "secOcRequired", False)) self.car_state.isVolt = car_fingerprint.startswith("CHEVROLET_VOLT") diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 7e0bb9dcc7..8d67a60e4b 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -150,6 +150,17 @@ "parent_key": "AlwaysOnLateral", "settings_tier": "simple" }, + { + "key": "TeslaAOLDisengageOnBrake", + "label": "Disengage AOL on Brake", + "description": "Pressing the brake fully turns off Always On Lateral. Steering stays off after the brake is released until openpilot is engaged again or AOL is manually toggled back on.", + "picker_description": "Keep steering off after pressing the brake until you deliberately re-engage it.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "AlwaysOnLateral", + "vehicle_makes": ["Tesla"], + "settings_tier": "simple" + }, { "key": "LaneChanges", "label": "Lane Changes", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 06122fb01f..817bd7e7cb 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -862,6 +862,9 @@ class StarPilotVariables: ) toggle.always_on_lateral_main = toggle.always_on_lateral and not prohibited_main_aol toggle.always_on_lateral_pause_speed = self.get_value("PauseAOLOnBrake", cast=float, condition=toggle.always_on_lateral) + toggle.tesla_aol_disengage_on_brake = self.get_value( + "TeslaAOLDisengageOnBrake", condition=toggle.always_on_lateral and toggle.car_make == "tesla" + ) main_cruise_button_control = self.get_button_function("MainCruiseButtonControl") toggle.main_cruise_aol_toggle = _main_cruise_aol_allowed(main_cruise_button_control) diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index ee1957fb77..1f1db0f039 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -73,7 +73,9 @@ class StarPilotCard: self.g70_main_cruise_aol_pending_frames = 0 self.prev_cruise_available = None self.prev_active = False + self.prev_brake_pressed = False self.prev_cruise_enabled = False + self.tesla_aol_brake_disengaged = False self.decel_pressed = False self.cancelPressed_previously = False self.cancel_pulse_glide_suppressed = False @@ -161,9 +163,17 @@ class StarPilotCard: def _toggle_controller_aol(self, carState, starpilot_toggles): if not self.always_on_lateral_supported or not getattr(starpilot_toggles, "always_on_lateral", False): return False + tesla_disengage_on_brake = ( + self.CP.brand == "tesla" and + getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False) + ) + if tesla_disengage_on_brake and not self.always_on_lateral_allowed and carState.brakePressed: + return False if self.hyundai_aol_needs_engagement: self.hyundai_aol_ready = True self.always_on_lateral_allowed = not self.always_on_lateral_allowed + if tesla_disengage_on_brake and self.always_on_lateral_allowed: + self.tesla_aol_brake_disengaged = False if carState.cruiseState.enabled or self.pause_lateral: self.pause_lateral = not self.always_on_lateral_allowed return True @@ -260,6 +270,12 @@ class StarPilotCard: and starpilot_toggles.main_cruise_aol_toggle ) forte_main_cruise_aol_managed = self.kia_forte_non_scc and starpilot_toggles.main_cruise_aol_toggle + tesla_disengage_on_brake = ( + self.CP.brand == "tesla" and + getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False) + ) + if not tesla_disengage_on_brake: + self.tesla_aol_brake_disengaged = False if carState.gearShifter in NON_DRIVING_GEARS or not g70_main_cruise_aol_managed: self.g70_main_cruise_aol_pending = False @@ -335,12 +351,23 @@ class StarPilotCard: # On rising edge of engagement (SET press enabling lat+long), auto-enable AOL # so that lateral persists when braking disengages longitudinal - if sm["selfdriveState"].active and not self.prev_active and self.always_on_lateral_set and starpilot_toggles.always_on_lateral_lkas: + engagement_started = sm["selfdriveState"].active and not self.prev_active + if (engagement_started and self.always_on_lateral_set and + (starpilot_toggles.always_on_lateral_lkas or tesla_disengage_on_brake)): if hyundai_aol_needs_engagement: self.hyundai_aol_ready = True + self.tesla_aol_brake_disengaged = False self.always_on_lateral_allowed = True + if (tesla_disengage_on_brake and carState.brakePressed and not self.prev_brake_pressed and + self.always_on_lateral_set): + self.tesla_aol_brake_disengaged = True + + if self.tesla_aol_brake_disengaged: + self.always_on_lateral_allowed = False + self.prev_active = sm["selfdriveState"].active + self.prev_brake_pressed = carState.brakePressed self.prev_cruise_enabled = carState.cruiseState.enabled self.prev_cruise_available = carState.cruiseState.available diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 17e60fea06..7eda736be1 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -166,11 +166,9 @@ class StarPilotPlanner: CS = sm["carState"] blinker_on = CS.leftBlinker or CS.rightBlinker - signal_pause = blinker_on and starpilot_toggles.pause_lateral_below_signal self.lateral_check = v_ego >= starpilot_toggles.pause_lateral_below_speed self.lateral_check |= not blinker_on and starpilot_toggles.pause_lateral_below_signal - self.lateral_check |= CS.standstill and not signal_pause self.lateral_check &= not sm["starpilotCarState"].pauseLateral # Blinker-based lateral resume delay: after blinker turns off, delay lateral diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index cdf85187c8..356ca146ae 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -70,6 +70,7 @@ def make_toggles(**overrides): "always_on_lateral_lkas": False, "always_on_lateral_main": False, "always_on_lateral_pause_speed": 0.0, + "tesla_aol_disengage_on_brake": False, "bookmark_via_cancel": False, "bookmark_via_cancel_long": False, "bookmark_via_cancel_very_long": False, @@ -1172,6 +1173,118 @@ def test_hyundai_main_aol_persists_after_brake_disengage_without_manual_aol_butt assert ret.alwaysOnLateralEnabled is True +def test_tesla_aol_disengages_on_brake_until_deliberate_reengagement(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="tesla", carFingerprint="TESLA_MODEL_Y", pcmCruise=True), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + sm = make_sm() + toggles = make_toggles( + always_on_lateral=True, + always_on_lateral_main=True, + tesla_aol_disengage_on_brake=True, + ) + starpilot_car_state = SimpleNamespace(distancePressed=False) + + sm["selfdriveState"].active = True + ret = card.update(make_car_state(available=True, enabled=True), starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralEnabled is True + + sm["selfdriveState"].active = False + ret = card.update( + make_car_state(available=True, brake_pressed=True), starpilot_car_state, sm, toggles, + ) + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + ret = card.update( + make_car_state(available=True, gas_pressed=True), starpilot_car_state, sm, toggles, + ) + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + sm["selfdriveState"].active = True + ret = card.update(make_car_state(available=True, enabled=True), starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + +def test_tesla_aol_can_be_manually_reenabled_after_brake_release(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="tesla", carFingerprint="TESLA_MODEL_3", pcmCruise=True), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + sm = make_sm() + toggles = make_toggles( + always_on_lateral=True, + always_on_lateral_main=True, + tesla_aol_disengage_on_brake=True, + ) + starpilot_car_state = SimpleNamespace(distancePressed=False) + + card.update(make_car_state(available=True), starpilot_car_state, sm, toggles) + card.update(make_car_state(available=True, brake_pressed=True), starpilot_car_state, sm, toggles) + released_state = make_car_state(available=True) + card.update(released_state, starpilot_car_state, sm, toggles) + + assert card._toggle_controller_aol(released_state, toggles) is True + ret = card.update(released_state, starpilot_car_state, sm, toggles) + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + +def test_tesla_aol_cannot_be_reenabled_while_brake_is_held(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="tesla", carFingerprint="TESLA_MODEL_Y", pcmCruise=True), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + toggles = make_toggles( + always_on_lateral=True, + always_on_lateral_main=True, + tesla_aol_disengage_on_brake=True, + ) + starpilot_car_state = SimpleNamespace(distancePressed=False) + brake_state = make_car_state(available=True, brake_pressed=True) + + card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles) + card.update(brake_state, starpilot_car_state, make_sm(), toggles) + + assert card._toggle_controller_aol(brake_state, toggles) is False + assert card.always_on_lateral_allowed is False + + +def test_tesla_brake_disengage_toggle_does_not_change_other_brands(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="gm"), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + toggles = make_toggles( + always_on_lateral=True, + always_on_lateral_main=True, + tesla_aol_disengage_on_brake=True, + ) + starpilot_car_state = SimpleNamespace(distancePressed=False) + + card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles) + card.update(make_car_state(available=True, brake_pressed=True), starpilot_car_state, make_sm(), toggles) + ret = card.update(make_car_state(available=True), starpilot_car_state, make_sm(), toggles) + + assert ret.alwaysOnLateralAllowed is True + assert ret.alwaysOnLateralEnabled is True + + def test_aol_persists_through_longitudinal_speed_too_low_disable(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index a6de4f1285..d23602a57a 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -80,6 +80,15 @@ def test_galaxy_layout_contains_basic_mode_controls(): assert {"AlphaLongitudinalEnabled", "ForceOffroad", "GalaxyDeveloperMode"} <= sections["Developer"].keys() +def test_tesla_aol_brake_disengage_is_tesla_only_and_opt_in(): + setting = _params_by_section(_layout())["Lateral (Steering)"]["TeslaAOLDisengageOnBrake"] + + assert _declared_default("TeslaAOLDisengageOnBrake") == "0" + assert setting["vehicle_makes"] == ["Tesla"] + assert setting["parent_key"] == "AlwaysOnLateral" + assert setting["ui_type"] == "toggle" + + def test_galaxy_new_ui_is_the_visible_default_choice(): galaxy_default = _params_by_section(_layout())["Developer"]["GalaxyMobileDefault"]