From cfd221b89cec223397f0a805791136aa9b958758 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 23 Aug 2026 10:40:20 -0500 Subject: [PATCH] fine day, Sunday. --- common/params_keys.h | 2 + .../opendbc/car/ford/carcontroller.py | 8 +- opendbc_repo/opendbc/car/ford/fordcan.py | 4 +- .../opendbc/car/ford/tests/test_ford.py | 29 +++++++ .../opendbc/car/hyundai/tests/test_hyundai.py | 39 ++++++++++ opendbc_repo/opendbc/car/hyundai/values.py | 5 +- opendbc_repo/opendbc/car/nissan/interface.py | 3 +- .../opendbc/car/nissan/tests/test_nissan.py | 52 ++++++++++--- opendbc_repo/opendbc/safety/tests/common.py | 2 + .../opendbc/safety/tests/test_hyundai.py | 8 ++ selfdrive/controls/lib/latcontrol_torque.py | 4 + .../controls/lib/latcontrol_vehicle_tunes.py | 38 ++++++++- .../lib/longitudinal_mpc_lib/long_mpc.py | 5 +- .../controls/lib/longitudinal_planner.py | 28 ++++++- .../lib/longitudinal_vehicle_tunes.py | 58 ++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 19 +++++ .../tests/test_longitudinal_planner.py | 74 ++++++++++++++++++ .../ui/layouts/settings/starpilot/lateral.py | 17 +++- selfdrive/ui/onroad/starpilot/pulse_glide.py | 34 ++++++++ .../onroad/starpilot/starpilot_onroad_view.py | 15 +++- selfdrive/ui/soundd.py | 44 ++++++++--- selfdrive/ui/tests/test_soundd.py | 20 ++++- starpilot/car/ford/lateral.py | 42 +++++----- starpilot/car/ford/tests/test_lateral.py | 50 ++++++++---- .../common/assets/device_settings_layout.json | 16 +++- starpilot/common/starpilot_variables.py | 8 ++ .../assets/components/tools/model_manager.css | 12 +++ .../assets/components/tools/model_manager.js | 5 ++ .../the_galaxy/tests/test_dashboard_stats.py | 21 +++++ .../tests/test_device_settings_layout.py | 28 +++++++ starpilot/system/the_galaxy/the_galaxy.py | 78 ++++++++++++++++++- 31 files changed, 694 insertions(+), 74 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 03dfb85d4..ac8a23f3d 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -345,6 +345,7 @@ inline static std::unordered_map keys = { {"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4", "0.4", 2}}, {"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4", "0.4", 2}}, {"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85", "0.85", 2}}, + {"FordHandsFreeCluster", {PERSISTENT, BOOL, "0", "0", 2}}, {"FordHumanTurnDetection", {PERSISTENT, BOOL, "1", "1", 2}}, {"FordLateralMode", {PERSISTENT, INT, "1", "1", 2}}, {"FLMActiveOverrides", {PERSISTENT, JSON, "{}", "{}", 2}}, @@ -679,6 +680,7 @@ inline static std::unordered_map keys = { {"TuningLevel", {PERSISTENT, INT, "0", "0", 0}}, {"TuningLevelConfirmed", {PERSISTENT, BOOL, "0", "0", 0}}, {"TurnDesires", {PERSISTENT, BOOL, "0", "0", 2}}, + {"TurnSteeringLimitMuteSpeed", {PERSISTENT, INT, "0", "0", 0}}, {"UnlockDoors", {PERSISTENT, BOOL, "1", "0", 0}}, {"Updated", {PERSISTENT, STRING, "0", "0"}}, {"UpdateSpeedLimits", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index b07986b78..e71f7ebd3 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -255,9 +255,15 @@ class CarController(CarControllerBase): if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: show_distance_bars = self.frame - self.distance_bar_frame < 400 + hands_free_cluster = bool( + self.ford_lateral is not None + and self.ford_lateral.mode != FordLateralMode.native + and self.ford_lateral.mode == self.ford_lateral_announced_mode + and self.ford_lateral.hands_free_cluster_enabled) can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, fcw_alert, CS.out.cruiseState.standstill, show_distance_bars, - hud_control, CS.acc_tja_status_stock_values)) + hud_control, CS.acc_tja_status_stock_values, + hands_free_cluster)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive diff --git a/opendbc_repo/opendbc/car/ford/fordcan.py b/opendbc_repo/opendbc/car/ford/fordcan.py index 721569b82..094297398 100644 --- a/opendbc_repo/opendbc/car/ford/fordcan.py +++ b/opendbc_repo/opendbc/car/ford/fordcan.py @@ -181,7 +181,7 @@ def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: fl def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool, - show_distance_bars: bool, hud_control, stock_values: dict): + show_distance_bars: bool, hud_control, stock_values: dict, hands_free_cluster: bool = False): """ Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam assist status. @@ -197,6 +197,8 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw status = 3 # ActiveInterventionLeft elif hud_control.rightLaneDepart: status = 4 # ActiveInterventionRight + elif hands_free_cluster: + status = 7 # Hands-free assistance display else: status = 2 # Active elif main_on: diff --git a/opendbc_repo/opendbc/car/ford/tests/test_ford.py b/opendbc_repo/opendbc/car/ford/tests/test_ford.py index 40f533aac..3d3348f14 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_ford.py +++ b/opendbc_repo/opendbc/car/ford/tests/test_ford.py @@ -1,10 +1,13 @@ import random from collections.abc import Iterable +from types import SimpleNamespace from hypothesis import settings, given, strategies as st from parameterized import parameterized from opendbc.car import gen_empty_fingerprint +from opendbc.can import CANPacker +from opendbc.car.ford import fordcan from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict from opendbc.car.ford.interface import CarInterface @@ -172,3 +175,29 @@ def test_mach_e_longitudinal_toggle_controls_stock_acc_selection(): assert enhanced.alphaLongitudinalAvailable assert enhanced.openpilotLongitudinalControl assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL + + +def test_hands_free_cluster_status_is_opt_in(): + packer = CANPacker("ford_lincoln_base_pt") + CAN = SimpleNamespace(main=0) + CP = SimpleNamespace(openpilotLongitudinalControl=False) + hud = SimpleNamespace(leftLaneDepart=False, rightLaneDepart=False) + stock_values = dict.fromkeys([ + "HaDsply_No_Cs", "HaDsply_No_Cnt", "AccStopStat_D_Dsply", "AccTrgDist2_D_Dsply", + "AccStopRes_B_Dsply", "TjaWarn_D_Rq", "TjaMsgTxt_D_Dsply", "IaccLamp_D_Rq", + "AccMsgTxt_D2_Rq", "FcwDeny_B_Dsply", "FcwMemStat_B_Actl", "AccTGap_B_Dsply", + "CadsAlignIncplt_B_Actl", "AccFllwMde_B_Dsply", "CadsRadrBlck_B_Actl", + "CmbbPostEvnt_B_Dsply", "AccStopMde_B_Dsply", "FcwMemSens_D_Actl", + "FcwMsgTxt_D_Rq", "AccWarn_D_Dsply", "FcwVisblWarn_B_Rq", "FcwAudioWarn_B_Rq", + "AccTGap_D_Dsply", "AccMemEnbl_B_RqDrv", "FdaMem_B_Stat", + ], 0) + + regular = fordcan.create_acc_ui_msg( + packer, CAN, CP, True, True, False, False, False, hud, stock_values) + hands_free = fordcan.create_acc_ui_msg( + packer, CAN, CP, True, True, False, False, False, hud, stock_values, True) + expected_regular = packer.make_can_msg("ACCDATA_3", 0, {"Tja_D_Stat": 2}) + expected_hands_free = packer.make_can_msg("ACCDATA_3", 0, {"Tja_D_Stat": 7}) + + assert regular == expected_regular + assert hands_free == expected_hands_free diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 93dd1073c..b3aa43aaf 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -988,6 +988,45 @@ class TestHyundaiFingerprint: assert long_xceed.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.HYBRID_GAS assert long_xceed.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG + def test_g80_alpha_long_preserves_legacy_safety(self): + toggles = get_test_toggles() + + stock_g80 = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], False, False, False, toggles) + assert CAR.GENESIS_G80 in LEGACY_LONGITUDINAL_CAR + assert stock_g80.alphaLongitudinalAvailable + assert not stock_g80.openpilotLongitudinalControl + assert stock_g80.pcmCruise + assert stock_g80.safetyConfigs[-1].safetyModel == CarParams.SafetyModel.hyundaiLegacy + assert not (stock_g80.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG) + + long_g80 = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], True, False, False, toggles) + assert long_g80.alphaLongitudinalAvailable + assert long_g80.openpilotLongitudinalControl + assert not long_g80.pcmCruise + assert long_g80.safetyConfigs[-1].safetyModel == CarParams.SafetyModel.hyundaiLegacy + assert long_g80.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG + + @pytest.mark.parametrize("ecu_disabled", (True, False)) + def test_g80_alpha_long_disables_stock_scc(self, monkeypatch, ecu_disabled): + toggles = get_test_toggles() + CP = CarInterface.get_params(CAR.GENESIS_G80, gen_empty_fingerprint(), [], True, False, False, toggles) + + called = {} + + def fake_disable_ecu(*args, **kwargs): + called.update(kwargs) + return ecu_disabled + + monkeypatch.setattr("opendbc.car.hyundai.interface.disable_ecu", fake_disable_ecu) + CarInterface.init(CP, None, None) + + assert called["addr"] == 0x7d0 + assert called["bus"] == 0 + assert called["reset"] is False + assert CP.openpilotLongitudinalControl == ecu_disabled + assert CP.pcmCruise != ecu_disabled + assert bool(CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG) == ecu_disabled + def test_xceed_phev_disable_failure_falls_back_to_stock_acc(self, monkeypatch): toggles = get_test_toggles() CP = CarInterface.get_params(CAR.KIA_XCEED_PHEV, gen_empty_fingerprint(), [], True, False, False, toggles) diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 16eb84388..df10d1df3 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -1213,6 +1213,9 @@ NON_SCC_CAR = CAR.with_flags(HyundaiFlags.NON_SCC) # HyundaiFlags.CANFD_RADAR_SCC | HyundaiFlags.CANFD_NO_RADAR_DISABLE | ) UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.LEGACY) | CAR.with_flags(HyundaiFlags.UNSUPPORTED_LONGITUDINAL) -LEGACY_LONGITUDINAL_CAR = {CAR.KIA_XCEED_PHEV} +LEGACY_LONGITUDINAL_CAR = { + CAR.GENESIS_G80, + CAR.KIA_XCEED_PHEV, +} DBC = CAR.create_dbc_map() diff --git a/opendbc_repo/opendbc/car/nissan/interface.py b/opendbc_repo/opendbc/car/nissan/interface.py index c6f202fc2..70c68dc73 100644 --- a/opendbc_repo/opendbc/car/nissan/interface.py +++ b/opendbc_repo/opendbc/car/nissan/interface.py @@ -15,6 +15,7 @@ LEAF_ADAS_ECU_BUS = 0 LEAF_ADAS_COMMAND_BUS = 1 LEAF_ADAS_COMMAND_ADDRS = frozenset((0x1C3, 0x2B0)) LEAF_2025_SV_PLUS_CAMERA_FW = b'6WK2CDB\x04\x18\x00\x00\x00\x00\x00R=1\x18\x99\x10\x00\x00\x00\x80' +LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED = False LEAF_KWP_EXTENDED_REQUEST = b"\x10\xC0" LEAF_KWP_EXTENDED_RESPONSE = b"\x50\xC0" @@ -26,7 +27,7 @@ LEAF_KWP_TAKEOVER_SESSIONS = ( def is_leaf_2025_sv_plus_longitudinal(candidate, car_fw): - return candidate == CAR.NISSAN_LEAF and any( + return LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED and candidate == CAR.NISSAN_LEAF and any( fw.address == LEAF_ADAS_ECU_ADDR and bytes(fw.fwVersion) == LEAF_2025_SV_PLUS_CAMERA_FW for fw in car_fw ) diff --git a/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py b/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py index 01f745895..995fcd98a 100644 --- a/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py +++ b/opendbc_repo/opendbc/car/nissan/tests/test_nissan.py @@ -4,6 +4,7 @@ import pytest from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs from opendbc.car.can_definitions import CanData +from opendbc.car.nissan import interface as nissan_interface from opendbc.car.nissan.carstate import CarState from opendbc.car.nissan.interface import CarInterface, LEAF_2025_SV_PLUS_CAMERA_FW, leaf_adas_commands_present, \ leaf_adas_commands_silent, restore_leaf_adas_tx @@ -18,6 +19,12 @@ SUPPORTED_LEAF_FW = [structs.CarParams.CarFw( )] +@pytest.fixture +def experimental_leaf_long(monkeypatch): + """Exercise the dormant implementation without making it available in production.""" + monkeypatch.setattr(nissan_interface, "LEAF_2025_SV_PLUS_ALPHA_LONG_ENABLED", True) + + def run_controller(alpha_long, accel=0.0, long_active=True, long_state=structs.CarControl.Actuators.LongControlState.pid): CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, alpha_long, False, False, TEST_TOGGLES) FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, CP, TEST_TOGGLES) @@ -33,7 +40,28 @@ def run_controller(alpha_long, accel=0.0, long_active=True, long_state=structs.C return {msg[0]: msg for msg in can_sends} -def test_leaf_2025_sv_plus_alpha_long_params(): +def test_leaf_2025_sv_plus_alpha_long_is_disabled(monkeypatch): + stock = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, False, False, False, None) + alpha_long = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None) + + assert not stock.alphaLongitudinalAvailable + assert not stock.openpilotLongitudinalControl + assert stock.pcmCruise + assert not (stock.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) + + assert not alpha_long.alphaLongitudinalAvailable + assert not alpha_long.openpilotLongitudinalControl + assert alpha_long.pcmCruise + assert not alpha_long.autoResumeSng + assert not (alpha_long.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) + + disable_calls = [] + monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: disable_calls.append((args, kwargs))) + CarInterface.init(alpha_long, None, None) + assert not disable_calls + + +def test_dormant_leaf_2025_sv_plus_alpha_long_params(experimental_leaf_long): stock = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, False, False, False, None) alpha_long = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None) @@ -79,7 +107,13 @@ def test_stock_controller_does_not_send_longitudinal_messages(): assert not ({0x2B0, 0x1C3, 0x707} & can_sends.keys()) -def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive(): +def test_disabled_alpha_long_controller_does_not_send_longitudinal_messages(): + can_sends = run_controller(True) + + assert not ({0x2B0, 0x1C3, 0x707} & can_sends.keys()) + + +def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive(experimental_leaf_long): can_sends = run_controller(True) assert can_sends[0x2B0][1].hex() == "ff6090ac5b000e03" @@ -89,13 +123,13 @@ def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive(): assert can_sends[0x707][2] == 0 -def test_alpha_long_controller_clamps_to_panda_accel_limit(): +def test_alpha_long_controller_clamps_to_panda_accel_limit(experimental_leaf_long): can_sends = run_controller(True, accel=5.0) assert can_sends[0x2B0][1].hex() == "007f8fac5b000e0c" -def test_alpha_long_controller_blends_friction_brake_below_regen_limit(): +def test_alpha_long_controller_blends_friction_brake_below_regen_limit(experimental_leaf_long): can_sends = run_controller(True, accel=-2.0) assert can_sends[0x2B0][1].hex() == "a827d5ac5b000e09" @@ -104,7 +138,7 @@ def test_alpha_long_controller_blends_friction_brake_below_regen_limit(): assert brake[5] & 0x84 == 0x84 -def test_alpha_long_controller_sends_inactive_commands_when_disengaged(): +def test_alpha_long_controller_sends_inactive_commands_when_disengaged(experimental_leaf_long): can_sends = run_controller(True, accel=1.0, long_active=False) assert can_sends[0x2B0][1].hex() == "dc53a2ac1b000e03" @@ -113,7 +147,7 @@ def test_alpha_long_controller_sends_inactive_commands_when_disengaged(): @pytest.mark.parametrize(("signal", "button_type"), [("SET_BUTTON", ButtonType.decelCruise), ("RES_BUTTON", ButtonType.accelCruise)]) -def test_leaf_set_resume_release_enables_alpha_long(signal, button_type): +def test_leaf_set_resume_release_enables_alpha_long(signal, button_type, experimental_leaf_long): CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, TEST_TOGGLES) FPCP = CarInterface.get_starpilot_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, CP, TEST_TOGGLES) CS = CarState(CP, FPCP) @@ -130,7 +164,7 @@ def test_leaf_set_resume_release_enables_alpha_long(signal, button_type): @pytest.mark.parametrize("ecu_disabled", [False, True]) -def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled): +def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled, experimental_leaf_long): CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None) calls = [] @@ -158,7 +192,7 @@ def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled): assert bool(CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) is ecu_disabled -def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch): +def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch, experimental_leaf_long): CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None) monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: True) @@ -171,7 +205,7 @@ def test_leaf_kwp_no_response_disable_can_confirm_ecu_silence(monkeypatch): assert CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL -def test_leaf_positive_disable_response_without_command_silence_falls_back(monkeypatch): +def test_leaf_positive_disable_response_without_command_silence_falls_back(monkeypatch, experimental_leaf_long): CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None) restore_calls = [] diff --git a/opendbc_repo/opendbc/safety/tests/common.py b/opendbc_repo/opendbc/safety/tests/common.py index 8b897b044..da4f36bfb 100644 --- a/opendbc_repo/opendbc/safety/tests/common.py +++ b/opendbc_repo/opendbc/safety/tests/common.py @@ -1103,6 +1103,7 @@ class SafetyTest(SafetyTestBase): 'TestHyundaiSafetyFCEVLong', 'TestHyundaiLongitudinalAolLkasOnEngageSafety', 'TestHyundaiSafetyCanRefreshLong', 'TestHyundaiSafetyCanRefreshLongCameraSCC', 'TestHyundaiCanCanfdBlendedLongitudinalSafety', + 'TestHyundaiLegacyLongitudinalSafety', 'TestHyundaiLegacyLongitudinalSafetyHEV'}): continue volkswagen_shared = ('TestVolkswagenMqb', 'TestVolkswagenMlb', 'TestVolkswagenMeb') @@ -1156,6 +1157,7 @@ class SafetyTest(SafetyTestBase): if attr.startswith('TestHyundaiLongitudinal') or attr in ('TestHyundaiSafetyFCEVLong', 'TestHyundaiLongitudinalAolLkasOnEngageSafety', 'TestHyundaiCanCanfdBlendedLongitudinalSafety', + 'TestHyundaiLegacyLongitudinalSafety', 'TestHyundaiLegacyLongitudinalSafetyHEV'): # exceptions for common msgs across different Hyundai CAN platforms tx = list(filter(lambda m: m[0] not in [0x420, 0x50A, 0x389, 0x4A2], tx)) diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index 998c97f71..29d07ee43 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -597,6 +597,14 @@ class TestHyundaiSafetyFCEVLong(TestHyundaiLongitudinalSafety, TestHyundaiSafety self.safety.init_tests() +class TestHyundaiLegacyLongitudinalSafety(TestHyundaiLongitudinalSafety, TestHyundaiLegacySafety): + def setUp(self): + self.packer = CANPackerSafety("hyundai_kia_generic") + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundaiLegacy, HyundaiSafetyFlags.LONG) + self.safety.init_tests() + + class TestHyundaiLegacyLongitudinalSafetyHEV(TestHyundaiLongitudinalSafety, TestHyundaiLegacySafetyHEV): def setUp(self): self.packer = CANPackerSafety("hyundai_kia_generic") diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 0d44460f7..cf3fb7132 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -469,6 +469,8 @@ class LatControlTorque(LatControl): friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper) + if self.is_honda_accord: + ff *= get_honda_accord_ff_scale(setpoint) if flm_surface_active and self.flm_surface_profile_key and not ioniq_6_active: universal_flm_profile = self.flm_surface_profile_key == FLM_UNIVERSAL_PROFILE_KEY flm_full_surface_center_taper = get_flm_full_surface_center_taper_scale(self.flm_surface_profile_key, setpoint, CS.vEgo, @@ -580,6 +582,8 @@ class LatControlTorque(LatControl): output_torque *= get_ram_1500_center_output_scale(setpoint, CS.vEgo) if output_torque * setpoint > 0.0: output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo) + if setpoint * desired_lateral_jerk < 0.0: + output_torque *= get_ram_1500_unwind_output_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif self.is_kona_non_scc: output_torque *= get_kona_non_scc_center_taper_scale(setpoint, CS.vEgo) rapid_reversal = setpoint * desired_lateral_jerk < 0.0 diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 88ca71fc7..cd3577d93 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -76,6 +76,9 @@ BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS HONDA_ACCORD_STEER_RATIO_SCALE = 14.0 / 16.33 HONDA_ACCORD_TORQUE_KP = 0.8 HONDA_ACCORD_TORQUE_KI = 0.15 +HONDA_ACCORD_TURN_FF_REDUCTION_MAX = 0.10 +HONDA_ACCORD_TURN_FF_ONSET = 0.45 +HONDA_ACCORD_TURN_FF_WIDTH = 0.12 VOLT_STANDARD_CARS = ( GM_CAR.CHEVROLET_VOLT, GM_CAR.CHEVROLET_VOLT_2019, @@ -433,7 +436,7 @@ GMC_YUKON_CC_UNWIND_FF_REDUCTION = 0.12 SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 SONATA_HYBRID_FF_REDUCTION_LEFT = 0.09 -SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.22 +SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.26 SONATA_HYBRID_FF_ONSET = 0.18 SONATA_HYBRID_FF_ONSET_WIDTH = 0.08 SONATA_HYBRID_FF_CUTOFF = 1.35 @@ -444,7 +447,7 @@ SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.12 SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.02 SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.18 SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.10 -SONATA_HYBRID_CENTER_TAPER_MAX = 0.07 +SONATA_HYBRID_CENTER_TAPER_MAX = 0.10 SONATA_HYBRID_CENTER_TAPER_LAT = 0.16 SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025 SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0 @@ -1178,6 +1181,13 @@ RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET = 5.5 RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET_WIDTH = 1.5 RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX = 16.0 RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX_WIDTH = 2.0 +RAM_1500_UNWIND_OUTPUT_TAPER_MAX = 0.18 +RAM_1500_UNWIND_OUTPUT_SPEED_ONSET = 20.0 +RAM_1500_UNWIND_OUTPUT_SPEED_FULL = 29.0 +RAM_1500_UNWIND_OUTPUT_JERK_ONSET = 0.50 +RAM_1500_UNWIND_OUTPUT_JERK_FULL = 1.80 +RAM_1500_UNWIND_OUTPUT_LAT_ONSET = 0.65 +RAM_1500_UNWIND_OUTPUT_LAT_WIDTH = 0.30 # The Kona route is exceptionally accurate below highway speed, but Pop V2 # reverses the requested lateral acceleration roughly once per second at @@ -1787,6 +1797,23 @@ def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_l return 1.0 - (RAM_1500_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight) +def get_ram_1500_unwind_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, + v_ego: float) -> float: + """Soften only rapid high-speed unwind reversals on the RAM 1500.""" + if desired_lateral_accel * desired_lateral_jerk >= 0.0: + return 1.0 + + speed_weight = float(np.interp(v_ego, + [RAM_1500_UNWIND_OUTPUT_SPEED_ONSET, RAM_1500_UNWIND_OUTPUT_SPEED_FULL], + [0.0, 1.0])) + jerk_weight = float(np.interp(abs(desired_lateral_jerk), + [RAM_1500_UNWIND_OUTPUT_JERK_ONSET, RAM_1500_UNWIND_OUTPUT_JERK_FULL], + [0.0, 1.0])) + curve_weight = _sigmoid((abs(desired_lateral_accel) - RAM_1500_UNWIND_OUTPUT_LAT_ONSET) / + RAM_1500_UNWIND_OUTPUT_LAT_WIDTH) + return 1.0 - (RAM_1500_UNWIND_OUTPUT_TAPER_MAX * speed_weight * jerk_weight * curve_weight) + + def get_ram_1500_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: """Damp only center corrections at low/mid speed, not turn authority.""" center_weight = _sigmoid((RAM_1500_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / @@ -2005,6 +2032,13 @@ def get_honda_accord_steer_ratio_scale(_v_ego: float) -> float: return HONDA_ACCORD_STEER_RATIO_SCALE +def get_honda_accord_ff_scale(desired_lateral_accel: float) -> float: + """Taper only sharp-turn feedforward where the Accord carries excess curvature.""" + turn_weight = _sigmoid((abs(desired_lateral_accel) - HONDA_ACCORD_TURN_FF_ONSET) / + HONDA_ACCORD_TURN_FF_WIDTH) + return 1.0 - (HONDA_ACCORD_TURN_FF_REDUCTION_MAX * turn_weight) + + def get_bolt_2017_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: center_window = _bolt_2017_sigmoid((BOLT_2017_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / BOLT_2017_CENTER_TAPER_WIDTH) return 1.0 - (BOLT_2017_CENTER_TAPER_GAIN * _bolt_2017_high_speed_factor(v_ego) * center_window) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index bcb01b916..e7d9f7ece 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -923,7 +923,8 @@ class LongitudinalMpc: def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow, personality=log.LongitudinalPersonality.standard, tracking_lead=True, optional_far_lead_comfort=True, smooth_duplicate_vision=False, - stop_x=None, silverado_early_follow=False, modelV2=None): + stop_x=None, silverado_early_follow=False, modelV2=None, + lead_obstacle_bias=(0.0, 0.0)): v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo @@ -943,6 +944,8 @@ class LongitudinalMpc: # and then treat that as a stopped car/obstacle at this new distance. lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + lead_0_obstacle -= float(lead_obstacle_bias[0]) + lead_1_obstacle -= float(lead_obstacle_bias[1]) self.params[:,0] = ACCEL_MIN self.params[:,1] = max(0.0, self.max_a) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f1b64f94e..991f38778 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -21,6 +21,8 @@ from openpilot.selfdrive.controls.lib.lead_follow_policy import is_nonurgent_dup from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_far_follow_output_slew_rates, get_follow_prebrake_min_headway, + get_honda_accord_lead_departure_tune, + get_toyota_rav4_tss2_lead_departure_tune, get_force_stop_distance_bias, get_force_stop_handoff_distance, allow_radar_standstill_gap_settle, @@ -30,6 +32,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( is_toyota_rav4_tss2_radar_follow_lead, get_toyota_sienna_post_departure_restop_cap, get_untracked_slow_lead_decel_scale, + get_toyota_prius_stopped_lead_obstacle_bias, ) from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET @@ -1505,9 +1508,14 @@ class LongitudinalPlanner: max(LEAD_DEPART_ACCEL_HOLD_FULL_GAP - LEAD_DEPART_ACCEL_HOLD_MIN_GAP, 0.1), 0.0, 1.0)) lead_factor = float(np.clip((lead_speed - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED) / max(LEAD_DEPART_ACCEL_HOLD_FULL_LEAD_SPEED - LEAD_DEPART_ACCEL_HOLD_MIN_LEAD_SPEED, 0.1), 0.0, 1.0)) - accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip( + departure_tune = get_honda_accord_lead_departure_tune(self.CP) + if departure_tune is None: + departure_tune = get_toyota_rav4_tss2_lead_departure_tune(self.CP) + max_accel = LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL if departure_tune is None else departure_tune[0] + assist = LEAD_DEPART_ACCEL_ASSIST if departure_tune is None else departure_tune[1] + accel_cap = LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL + (max_accel - LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL) * np.clip( 0.55 * lead_factor + 0.45 * gap_factor, 0.0, 1.0) - assisted_model_accel = float(model_desired_accel) + LEAD_DEPART_ACCEL_ASSIST + assisted_model_accel = float(model_desired_accel) + assist return min(accel_cap, max(assisted_model_accel, LEAD_DEPART_ACCEL_HOLD_MIN_ACCEL)) def get_reusable_lead_depart_accel_floor(self, lead, v_ego, t_follow): @@ -2201,6 +2209,19 @@ class LongitudinalPlanner: get_force_stop_distance_bias(self.CP.carFingerprint) ) + prius_lead_obstacle_bias = (0.0, 0.0) + if ( + self.mode == 'acc' and + not bool(getattr(sm['modelV2'].action, 'shouldStop', False)) and + not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and + not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and + not bool(getattr(sm['carState'], 'standstill', False)) + ): + prius_lead_obstacle_bias = ( + get_toyota_prius_stopped_lead_obstacle_bias(self.CP, self.lead_one, scene_v_ego), + get_toyota_prius_stopped_lead_obstacle_bias(self.CP, self.lead_two, scene_v_ego), + ) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['starpilotPlan'].dangerFactor, effective_t_follow, personality=personality, tracking_lead=lead_control_active, @@ -2208,7 +2229,8 @@ class LongitudinalPlanner: smooth_duplicate_vision=nonurgent_duplicate_vision_follow and not panic_bypass, stop_x=force_stop_x, silverado_early_follow=early_truck_follow, - modelV2=sm['modelV2']) + modelV2=sm['modelV2'], + lead_obstacle_bias=prius_lead_obstacle_bias) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index e110dea73..48ef9c312 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -4,6 +4,8 @@ import numpy as np HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0 HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0 HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35 +HONDA_ACCORD_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.85 +HONDA_ACCORD_LEAD_DEPART_ACCEL_ASSIST = 0.25 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GM_SILVERADO_EARLY_FOLLOW_MIN_EGO_SPEED = 18.0 GM_SILVERADO_EARLY_FOLLOW_MAX_DISTANCE = 130.0 @@ -36,6 +38,15 @@ TOYOTA_RAV4_TSS2_RADAR_FOLLOW_DISTANCE_OFFSET = 32.0 TOYOTA_RAV4_TSS2_RADAR_FOLLOW_MAX_LATERAL_OFFSET = 1.75 TOYOTA_RAV4_TSS2_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5 TOYOTA_RAV4_TSS2_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75 +TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL = 0.70 +TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST = 0.20 +TOYOTA_PRIUS_STOPPED_LEAD_OBSTACLE_BIAS_M = 1.5 +TOYOTA_PRIUS_STOPPED_LEAD_MAX_EGO_SPEED = 22.0 +TOYOTA_PRIUS_STOPPED_LEAD_MAX_SPEED = 1.0 +TOYOTA_PRIUS_STOPPED_LEAD_MIN_CLOSING_SPEED = 0.15 +TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE = 80.0 +TOYOTA_PRIUS_STOPPED_LEAD_RAMP_DISTANCE = 10.0 +TOYOTA_PRIUS_STOPPED_LEAD_MAX_LATERAL_OFFSET = 1.75 TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M = 4.5 # The Camry's force-stop path otherwise consumes the model endpoint before the # normal MPC stop-distance margin can be applied. Keep it within the forward @@ -44,6 +55,35 @@ TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 6.0 DEFAULT_FORCE_STOP_HANDOFF_M = 6.0 +def get_toyota_prius_stopped_lead_obstacle_bias(CP, lead, v_ego): + """Move the ordinary Prius stopped-lead target back without touching stop targets.""" + if ( + getattr(CP, "brand", "") != "toyota" or + str(getattr(CP, "carFingerprint", "")) != "TOYOTA_PRIUS" or + lead is None or not bool(getattr(lead, "status", False)) or + float(v_ego) <= 0.0 or float(v_ego) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_EGO_SPEED or + float(getattr(lead, "vLead", 0.0)) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_SPEED or + abs(float(getattr(lead, "yRel", 0.0))) > TOYOTA_PRIUS_STOPPED_LEAD_MAX_LATERAL_OFFSET + ): + return 0.0 + + distance = float(getattr(lead, "dRel", float("inf"))) + closing_speed = float(v_ego) - float(getattr(lead, "vLead", 0.0)) + if ( + distance <= 0.0 or distance > TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE or + closing_speed < TOYOTA_PRIUS_STOPPED_LEAD_MIN_CLOSING_SPEED + ): + return 0.0 + + strength = np.clip( + (TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE - distance) / + (TOYOTA_PRIUS_STOPPED_LEAD_MAX_DISTANCE - TOYOTA_PRIUS_STOPPED_LEAD_RAMP_DISTANCE), + 0.0, 1.0, + ) + bias = TOYOTA_PRIUS_STOPPED_LEAD_OBSTACLE_BIAS_M * strength + return float(min(bias, max(distance - 0.5, 0.0))) + + def is_toyota_rav4_tss2_post_departure_tune(CP): """Identify RAV4 TSS2 variants that need normal catch-up caps after departure.""" return ( @@ -52,6 +92,15 @@ def is_toyota_rav4_tss2_post_departure_tune(CP): ) +def get_toyota_rav4_tss2_lead_departure_tune(CP): + if is_toyota_rav4_tss2_post_departure_tune(CP): + return ( + TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL, + TOYOTA_RAV4_TSS2_LEAD_DEPART_ACCEL_ASSIST, + ) + return None + + def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min): """Start a mild RAV4 coast/brake response before a hard lead approach.""" if ( @@ -155,6 +204,15 @@ def get_lead_follow_jerk_scale(CP): return 1.0 +def get_honda_accord_lead_departure_tune(CP): + if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD": + return ( + HONDA_ACCORD_LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL, + HONDA_ACCORD_LEAD_DEPART_ACCEL_ASSIST, + ) + return None + + def is_gm_silverado_early_follow_lead(CP, lead, v_ego): """Admit a credible centered vision lead before it becomes a close lead.""" if ( diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 71f258a7c..5863a6f64 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -43,6 +43,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( get_gmc_yukon_cc_ff_scale, get_ram_1500_center_output_scale, get_ram_1500_transition_output_scale, + get_ram_1500_unwind_output_scale, get_ram_1500_ff_scale, get_rav4_tss2_pid_output, get_subaru_impreza_pid_output_scale, @@ -86,6 +87,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_gv70_friction_threshold, get_genesis_gv70_high_speed_error_scale, get_genesis_gv70_unwind_ff_scale, + get_honda_accord_ff_scale, get_elantra_non_scc_ff_scale, get_honda_accord_steer_ratio_scale, get_palisade_ff_scale, @@ -1079,6 +1081,18 @@ class TestLatControl: assert crawl > center assert center > 0.85 + def test_ram_1500_unwind_output_taper_is_high_speed_and_phase_gated(self): + turn_in = get_ram_1500_unwind_output_scale(1.2, 1.1, 25.0) + low_speed = get_ram_1500_unwind_output_scale(1.2, -1.1, 15.0) + high_speed = get_ram_1500_unwind_output_scale(1.2, -1.1, 25.0) + sharp_reversal = get_ram_1500_unwind_output_scale(2.4, -2.0, 29.0) + + assert turn_in == pytest.approx(1.0) + assert low_speed == pytest.approx(1.0) + assert 0.95 < high_speed < 1.0 + assert sharp_reversal < high_speed + assert sharp_reversal > 0.80 + def test_ram_1500_phase_feedforward_curve(self): assert get_ram_1500_ff_scale(0.0, 1.0, 15.0) == pytest.approx(1.0) assert get_ram_1500_ff_scale(1.2, 1.1, 17.0) > 1.0 @@ -1761,6 +1775,11 @@ class TestLatControl: assert get_honda_accord_steer_ratio_scale(0.0) == pytest.approx(expected_scale) assert get_honda_accord_steer_ratio_scale(20.0) == pytest.approx(expected_scale) + def test_honda_accord_turn_feedforward_taper(self): + assert get_honda_accord_ff_scale(0.0) > get_honda_accord_ff_scale(0.8) + assert get_honda_accord_ff_scale(-0.8) == pytest.approx(get_honda_accord_ff_scale(0.8)) + assert get_honda_accord_ff_scale(0.0) == pytest.approx(1.0, abs=0.01) + def test_subaru_impreza_pid_output_scale_preserves_small_errors(self): assert get_subaru_impreza_pid_output_scale(0.0) == 1.0 assert get_subaru_impreza_pid_output_scale(0.75) == 1.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 6cd533bf8..5f9147c94 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -27,6 +27,9 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( allow_radar_standstill_gap_settle, get_far_follow_output_slew_rates, get_follow_prebrake_min_headway, + get_honda_accord_lead_departure_tune, + get_toyota_prius_stopped_lead_obstacle_bias, + get_toyota_rav4_tss2_lead_departure_tune, get_toyota_rav4_tss2_early_lead_cap, get_toyota_sienna_post_departure_restop_cap, is_toyota_rav4_tss2_radar_follow_lead, @@ -91,6 +94,28 @@ def test_mpc_duplicate_lead_filters_do_not_cross_contaminate_tracks(): assert mpc.duplicate_lead_v_filters[1].x == pytest.approx(28.0) +def test_prius_stopped_lead_obstacle_bias_is_small_and_vehicle_specific(): + prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS) + other = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + stopped_lead = make_lead(status=True, d_rel=18.0, v_lead=0.2, model_prob=0.99) + + bias = get_toyota_prius_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=8.0) + assert 0.0 < bias < 1.5 + assert get_toyota_prius_stopped_lead_obstacle_bias(other, stopped_lead, v_ego=8.0) == pytest.approx(0.0) + assert get_toyota_prius_stopped_lead_obstacle_bias( + prius, make_lead(status=True, d_rel=18.0, v_lead=4.0), v_ego=8.0, + ) == pytest.approx(0.0) + + +def test_prius_stopped_lead_obstacle_bias_does_not_apply_at_standstill_or_to_departures(): + prius = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_PRIUS) + stopped_lead = make_lead(status=True, d_rel=4.0, v_lead=0.0, model_prob=0.99) + departing_lead = make_lead(status=True, d_rel=18.0, v_lead=2.0, model_prob=0.99) + + assert get_toyota_prius_stopped_lead_obstacle_bias(prius, stopped_lead, v_ego=0.0) == pytest.approx(0.0) + assert get_toyota_prius_stopped_lead_obstacle_bias(prius, departing_lead, v_ego=8.0) == pytest.approx(0.0) + + def test_mpc_duplicate_vision_filter_smooths_distance_jumps_per_track(): mpc = LongitudinalMpc() mpc.set_cur_state(27.0, 0.0) @@ -2479,6 +2504,55 @@ def test_route_251682_rav4_confirmed_depart_adds_bounded_accel_assist(): assert floor <= longitudinal_planner_module.LEAD_DEPART_ACCEL_HOLD_MAX_ACCEL +def test_honda_accord_lead_departure_assist_is_stronger_but_vehicle_scoped(): + accord = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD) + civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(accord, init_v=0.0) + lead = make_lead( + status=True, + d_rel=7.7, + v_lead=2.0, + a_lead=1.79, + radar=False, + model_prob=1.0, + ) + + accord_floor = planner.get_lead_depart_accel_floor(lead, v_ego=0.0, model_desired_accel=0.44) + civic_floor = LongitudinalPlanner(civic, init_v=0.0).get_lead_depart_accel_floor( + lead, v_ego=0.0, model_desired_accel=0.44, + ) + + assert get_honda_accord_lead_departure_tune(accord) is not None + assert get_honda_accord_lead_departure_tune(civic) is None + assert accord_floor > civic_floor + assert accord_floor <= get_honda_accord_lead_departure_tune(accord)[0] + + +def test_rav4_tss2_lead_departure_assist_is_vehicle_scoped(): + rav4 = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2_2023) + civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + lead = make_lead( + status=True, + d_rel=7.7, + v_lead=2.0, + a_lead=1.79, + radar=False, + model_prob=1.0, + ) + + rav4_floor = LongitudinalPlanner(rav4, init_v=0.0).get_lead_depart_accel_floor( + lead, v_ego=0.0, model_desired_accel=0.44, + ) + civic_floor = LongitudinalPlanner(civic, init_v=0.0).get_lead_depart_accel_floor( + lead, v_ego=0.0, model_desired_accel=0.44, + ) + + assert get_toyota_rav4_tss2_lead_departure_tune(rav4) is not None + assert get_toyota_rav4_tss2_lead_departure_tune(civic) is None + assert rav4_floor > civic_floor + assert rav4_floor == pytest.approx(0.64) + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_depart_accel_hold_reuses_floor_through_softening_lead_delta(model_version): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 005aeb9ea..04d95b6ea 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -2,7 +2,7 @@ from __future__ import annotations from openpilot.system.hardware import HARDWARE from openpilot.selfdrive.ui.lib.starpilot_state import starpilot_state -from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.multilang import tr, tr_noop from openpilot.system.ui.widgets import DialogResult from openpilot.system.ui.widgets.option_dialog import MultiOptionDialog @@ -205,7 +205,8 @@ class StarPilotLateralLayout(_SettingsPage): ), SettingRow( "LaneChangeCloseGap", "toggle", tr_noop("Close Gap On Lane Change"), - subtitle=tr_noop("Allows for a temporary shorter follow distance behind lead so that openpilot merges smoothly out of current lane, it will allow car to accelerate as it changes lanes."), + subtitle=tr_noop("Allows for a temporary shorter follow distance behind lead so that openpilot merges smoothly " + + "out of current lane, it will allow car to accelerate as it changes lanes."), get_state=lambda: p.get_bool("LaneChangeCloseGap"), set_state=lambda s: p.put_bool("LaneChangeCloseGap", s), visible=lc_on, @@ -316,7 +317,8 @@ class StarPilotLateralLayout(_SettingsPage): "SteerLatAccel", "value", tr_noop("Lateral Acceleration"), subtitle=tr_noop("Maps steering torque to turning response."), get_value=lambda: f"{p.get_float('SteerLatAccel'):.2f}", - on_click=lambda: self._show_slider("SteerLatAccel", max(0.01, cs.latAccelFactor) * 0.5, max(0.01, cs.latAccelFactor) * 1.5, step=0.01, value_type="float"), + on_click=lambda: self._show_slider("SteerLatAccel", max(0.01, cs.latAccelFactor) * 0.5, + max(0.01, cs.latAccelFactor) * 1.5, step=0.01, value_type="float"), visible=lambda: alt_on() and cs.latAccelFactor != 0 and cs.isTorqueCar and not cs.isAngleCar, ), SettingRow( @@ -347,11 +349,18 @@ class StarPilotLateralLayout(_SettingsPage): ), SettingRow( "FordHumanTurnDetection", "toggle", tr_noop("Manual Turn Release"), - subtitle=tr_noop("Release lateral control during a sustained hands-on turn, then ramp back in smoothly."), + subtitle=tr_noop("Yield during an intentional manual turn while keeping the Ford steering session ready."), get_state=lambda: p.get_bool("FordHumanTurnDetection"), set_state=lambda s: p.put_bool("FordHumanTurnDetection", s), visible=ford_enhanced_mode, ), + SettingRow( + "FordHandsFreeCluster", "toggle", tr_noop("Hands-Free Cluster Display"), + subtitle=tr_noop("Show the vehicle's hands-free assistance graphic while lateral control is active. Driver monitoring requirements do not change."), + get_state=lambda: p.get_bool("FordHandsFreeCluster"), + set_state=lambda s: p.put_bool("FordHandsFreeCluster", s), + visible=ford_enhanced_mode, + ), SettingRow( "FordCurvatureBlendLow", "value", tr_noop("Small-Curve Prediction"), subtitle=tr_noop("Blend model-predicted curvature into gentle turns."), diff --git a/selfdrive/ui/onroad/starpilot/pulse_glide.py b/selfdrive/ui/onroad/starpilot/pulse_glide.py index fe90fc023..7c193c20c 100644 --- a/selfdrive/ui/onroad/starpilot/pulse_glide.py +++ b/selfdrive/ui/onroad/starpilot/pulse_glide.py @@ -6,6 +6,7 @@ from openpilot.system.ui.lib.text_measure import draw_text_with_shadow, measure_ PULSE_COLOR = rl.Color(52, 190, 112, 255) GLIDE_COLOR = rl.Color(65, 155, 235, 255) +BANNER_BACKGROUND = rl.Color(0, 0, 0, 210) def render_pulse_glide(rect: rl.Rectangle, coasting: bool) -> None: @@ -25,3 +26,36 @@ def render_pulse_glide(rect: rl.Rectangle, coasting: bool) -> None: font_size, rl.WHITE, ) + + +def render_pulse_glide_banner(content_rect: rl.Rectangle, coasting: bool) -> None: + """Render a non-alert on-road status banner while developer P&G is armed.""" + border = GLIDE_COLOR if coasting else PULSE_COLOR + state_label = "GLIDING" if coasting else "PULSE" + title_font = gui_app.font(FontWeight.MEDIUM) + state_font = gui_app.font(FontWeight.BOLD) + title_size = measure_text_cached(title_font, "PULSE & GLIDE", 22) + state_size = measure_text_cached(state_font, state_label, 34) + + banner_w = max(340.0, title_size.x + 48.0, state_size.x + 96.0) + banner_h = 86.0 + banner_rect = rl.Rectangle( + content_rect.x + (content_rect.width - banner_w) / 2.0, + content_rect.y + 28.0, + banner_w, + banner_h, + ) + + rl.draw_rectangle_rounded(banner_rect, 0.25, 12, BANNER_BACKGROUND) + rl.draw_rectangle_rounded_lines_ex(banner_rect, 0.25, 12, 4, border) + + title_pos = rl.Vector2( + banner_rect.x + (banner_rect.width - title_size.x) / 2.0, + banner_rect.y + 10.0, + ) + state_pos = rl.Vector2( + banner_rect.x + (banner_rect.width - state_size.x) / 2.0, + banner_rect.y + 38.0, + ) + draw_text_with_shadow(title_font, "PULSE & GLIDE", title_pos, 22, rl.WHITE) + draw_text_with_shadow(state_font, state_label, state_pos, 34, rl.WHITE) diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 0125c3cf6..a3689dfc8 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -14,7 +14,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.widgets import ( ) from openpilot.selfdrive.ui.onroad.starpilot.stopping_point import render_stopping_point from openpilot.selfdrive.ui.onroad.starpilot.pause_indicators import render_lateral_paused, render_longitudinal_paused -from openpilot.selfdrive.ui.onroad.starpilot.pulse_glide import render_pulse_glide +from openpilot.selfdrive.ui.onroad.starpilot.pulse_glide import render_pulse_glide, render_pulse_glide_banner from openpilot.selfdrive.ui.onroad.starpilot.pip_sidecam import PipSideCamera from openpilot.selfdrive.ui.onroad.starpilot.favorite_radial_menu import FavoriteRadialMenu from openpilot.selfdrive.ui.onroad.starpilot.weather_icon import render_weather_icon @@ -178,6 +178,7 @@ class StarPilotOnroadView(AugmentedRoadView): if alert_showing is not None: return + self._render_pulse_glide_banner() self._render_developer_metrics() self.layout_manager.render_widgets(exclude={"speed_limit", "set_speed"}) @@ -185,6 +186,18 @@ class StarPilotOnroadView(AugmentedRoadView): self._render_torque_bar() self._render_bottom_row_widgets() + def _render_pulse_glide_banner(self) -> None: + starpilot_car_state = ( + ui_state.sm["starpilotCarState"] + if ui_state.sm.valid.get("starpilotCarState", False) else None + ) + if not starpilot_car_state or not starpilot_car_state.pulseAndGlide: + return + + plan = ui_state.sm["starpilotPlan"] if ui_state.sm.valid.get("starpilotPlan", False) else None + coasting = bool(getattr(plan, "pulseGlideCoasting", False)) if plan else False + render_pulse_glide_banner(self._content_rect, coasting) + def _render_torque_bar(self) -> None: """Draw the curved torque-utilization indicator at the bottom of the screen.""" if not self._params.get_bool("EnableTorqueBarWidget", default=True): diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index ac5e69508..a09e6816d 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -40,6 +40,7 @@ StarPilotAudibleAlert = custom.StarPilotCarControl.HUDControl.AudibleAlert # stock sounds still work, and only offset custom random-event sounds. STARPILOT_CUSTOM_ALERT_OFFSET = 1000 STARPILOT_CUSTOM_ALERT_START = int(StarPilotAudibleAlert.angry) +TURN_STEERING_LIMIT_ALERT_SUFFIX = "steersaturated" def starpilot_alert_key(alert): @@ -47,6 +48,21 @@ def starpilot_alert_key(alert): return STARPILOT_CUSTOM_ALERT_OFFSET + raw_alert if raw_alert >= STARPILOT_CUSTOM_ALERT_START else raw_alert +def is_turn_steering_limit_alert(alert_type: str) -> bool: + """Return whether an alert type represents Turn Exceeds Steering Limit.""" + alert_name = str(alert_type or "").split("/", 1)[0].casefold() + return alert_name.endswith(TURN_STEERING_LIMIT_ALERT_SUFFIX) + + +def should_mute_turn_steering_limit_alert(alert_type: str, v_ego: float, mute_below_speed: float) -> bool: + """Mute only the audio for steering-limit alerts below the configured speed.""" + return ( + mute_below_speed > 0.0 and + v_ego < mute_below_speed and + is_turn_steering_limit_alert(alert_type) + ) + + sound_list: dict[int, tuple[str, int | None, float]] = { # AudibleAlert, file name, play count (none for infinite) AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), @@ -110,7 +126,7 @@ class Soundd: self.openpilot_crashed_played = False - self.auto_volume = 0 + self.auto_volume = MIN_VOLUME self.pending_stream_status = None self.previous_sound_pack = None @@ -285,7 +301,7 @@ class Soundd: # sounddevice must be imported after forking processes import sounddevice as sd - sm = messaging.SubMaster(['selfdriveState', 'soundPressure']) + sm = messaging.SubMaster(['selfdriveState', 'soundPressure', 'carState']) sm = sm.extend(['starpilotSelfdriveState', 'starpilotPlan']) @@ -305,19 +321,29 @@ class Soundd: if sm.updated['soundPressure'] and self.current_alert == AudibleAlert.none: # only update volume filter when not playing alert self.spl_filter_weighted.update(sm["soundPressure"].soundPressureWeightedDb) - self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) + self.auto_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) + self.current_volume = self.auto_volume if self.starpilot_toggles.alert_volume_controller: - self.auto_volume = self.current_volume self.current_volume = 0.0 - elif self.current_alert != AudibleAlert.none and self.starpilot_toggles.alert_volume_controller: - self.current_volume = self.get_volume_override() - if self.current_volume == 1.01: - self.current_volume = self.auto_volume - self.get_audible_alert(sm) + if self.current_alert != AudibleAlert.none: + v_ego = max(float(getattr(sm["carState"], "vEgo", 0.0)), 0.0) + if should_mute_turn_steering_limit_alert( + self.current_alert_type, + v_ego, + float(getattr(self.starpilot_toggles, "turn_steering_limit_mute_speed", 0.0)), + ): + self.current_volume = 0.0 + elif self.starpilot_toggles.alert_volume_controller: + self.current_volume = self.get_volume_override() + if self.current_volume == 1.01: + self.current_volume = self.auto_volume + else: + self.current_volume = self.auto_volume + rk.keep_time() if not stream.active: diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index b85f42dba..c146dce75 100644 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -1,7 +1,12 @@ from cereal import log from cereal import messaging from cereal.messaging import SubMaster, PubMaster -from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert +from openpilot.selfdrive.ui.soundd import ( + SELFDRIVE_STATE_TIMEOUT, + check_selfdrive_timeout_alert, + is_turn_steering_limit_alert, + should_mute_turn_steering_limit_alert, +) import time @@ -9,6 +14,19 @@ AudibleAlert = log.SelfdriveState.AudibleAlert class TestSoundd: + def test_turn_steering_limit_alert_detection(self): + assert is_turn_steering_limit_alert("steerSaturated/warning") + assert is_turn_steering_limit_alert("goatSteerSaturated/warning") + assert is_turn_steering_limit_alert("thisIsFineSteerSaturated/warning") + assert not is_turn_steering_limit_alert("laneChangeBlocked/warning") + + def test_turn_steering_limit_alert_is_muted_only_below_threshold(self): + assert should_mute_turn_steering_limit_alert("steerSaturated/warning", 10.0, 25.0) + assert not should_mute_turn_steering_limit_alert("steerSaturated/warning", 25.0, 25.0) + assert not should_mute_turn_steering_limit_alert("steerSaturated/warning", 30.0, 25.0) + assert not should_mute_turn_steering_limit_alert("steerSaturated/warning", 10.0, 0.0) + assert not should_mute_turn_steering_limit_alert("laneChangeBlocked/warning", 10.0, 25.0) + def test_check_selfdrive_timeout_alert(self): sm = SubMaster(['selfdriveState']) pm = PubMaster(['selfdriveState']) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 6c61a1f6d..cbd3da231 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -29,7 +29,7 @@ PATH_ANGLE_MAX = 0.5235 STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL CURVATURE_LOOKAHEAD_MIN = 0.20 CURVATURE_LOOKAHEAD_MAX = 0.40 -HANDOFF_PRESS_SECONDS = 0.5 +ANGLE_HANDOFF_PRESS_SECONDS = 0.5 HANDOFF_PAUSE_FRAMES = 6 HANDOFF_COOLDOWN_SECONDS = 2.0 HANDOFF_MAX_PATH_ANGLE = 0.10 @@ -108,6 +108,7 @@ class FordLateralController: self.model = None self.mode = FordLateralMode.curvature + self.hands_free_cluster_enabled = False self.human_turn_enabled = True self.curvature_blend_low = 0.4 self.curvature_blend_high = 0.4 @@ -137,6 +138,8 @@ class FordLateralController: except ValueError: self.mode = FordLateralMode.native + self.hands_free_cluster_enabled = bool( + self.CP.flags & FordFlags.CANFD and self.params.get_bool("FordHandsFreeCluster")) self.human_turn_enabled = self.params.get_bool("FordHumanTurnDetection") self.curvature_blend_low = float(np.clip(self.params.get_float("FordCurvatureBlendLow", return_default=True), 0.0, 1.0)) self.curvature_blend_high = float(np.clip(self.params.get_float("FordCurvatureBlendHigh", return_default=True), 0.0, 1.0)) @@ -219,21 +222,28 @@ class FordLateralController: self.angle_stall_timer = 0.0 self.angle_stall_recoveries = 0 - def _driver_handoff_active(self, CS) -> bool: + def _angle_handoff_pause_active(self, CS) -> bool: if not self.human_turn_enabled: self._reset_handoff() return False + self.angle_pause_cooldown = max(0.0, self.angle_pause_cooldown - STEER_DT) if CS.out.steeringPressed: self.handoff_press_timer += STEER_DT - self.handoff_driver_override |= self.handoff_press_timer + 1e-9 >= HANDOFF_PRESS_SECONDS + self.handoff_driver_override |= self.handoff_press_timer + 1e-9 >= ANGLE_HANDOFF_PRESS_SECONDS else: - if self.handoff_driver_override: - self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS + if (self.handoff_driver_override and self.angle_pause_cooldown <= 0.0 + and self.angle_pause_frames <= 0 and abs(self.path_angle_last) < HANDOFF_MAX_PATH_ANGLE): + self.angle_pause_frames = HANDOFF_PAUSE_FRAMES self.handoff_driver_override = False self.handoff_press_timer = 0.0 - return self.handoff_driver_override + if self.angle_pause_frames > 0: + self.angle_pause_frames -= 1 + if self.angle_pause_frames == 0: + self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS + return True + return False def _inactive_angle_result(self, current_curvature: float) -> FordLateralResult: self.path_angle_last = 0.0 @@ -248,16 +258,11 @@ class FordLateralController: self.curvature_last = 0.0 return FordLateralResult(shadow_curvature=current) - if self._manual_turn(CC, CS): + if self._manual_turn(CC, CS) or CS.out.vEgoRaw < 0.1: self._reset_handoff() self.curvature_samples.clear() self.curvature_last = 0.0 - return FordLateralResult(shadow_curvature=current) - - if self._driver_handoff_active(CS) or CS.out.vEgoRaw < 0.1: - self.curvature_samples.clear() - self.curvature_last = 0.0 - return FordLateralResult(shadow_curvature=current) + return FordLateralResult(active=True, shadow_curvature=current) v_ego = float(CS.out.vEgoRaw) predicted = self._predicted_curvature(v_ego, self._curvature_lookahead()) @@ -310,18 +315,9 @@ class FordLateralController: self._reset_handoff() return self._inactive_angle_result(current) - if self._driver_handoff_active(CS): + if self._angle_handoff_pause_active(CS): return self._inactive_angle_result(current) - if self.human_turn_enabled: - if self.angle_pause_frames > 0: - self.angle_pause_frames -= 1 - if self.angle_pause_frames == 0: - self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS - return self._inactive_angle_result(current) - - self.angle_pause_cooldown = max(0.0, self.angle_pause_cooldown - STEER_DT) - v_ego = float(CS.out.vEgoRaw) live_delay = 0.12 if self.sm is None else float(np.clip(self.sm["liveDelay"].lateralDelay, 0.1, 0.15)) speed_factor = float(np.interp(v_ego, [11.176, 24.587], [1.0, 0.0])) diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index f650b4cb6..c160c8164 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -102,34 +102,58 @@ def test_manual_turn_releases_lateral(controller): assert result.path_angle == 0.0 -@pytest.mark.parametrize("strategy", ["update_curvature", "update_angle"]) -def test_enhanced_control_yields_during_sustained_driver_correction(controller, strategy): +def test_curvature_control_stays_active_during_driver_correction(controller): controller.human_turn_enabled = True CC = SimpleNamespace(latActive=True) actuators = SimpleNamespace(curvature=0.001) - update = getattr(controller, strategy) - for _ in range(9): - assert update(CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active + for _ in range(20): + result = controller.update_curvature( + CC, car_state(steering_pressed=True, steering_angle=10.0), actuators) + assert result.active - result = update(CC, car_state(steering_pressed=True, steering_angle=10.0), actuators) - assert not result.active + +def test_curvature_manual_turn_keeps_session_active_with_neutral_command(controller): + controller.human_turn_enabled = True + CC = SimpleNamespace(latActive=True) + actuators = SimpleNamespace(curvature=0.001) + + controller.update_curvature( + CC, car_state(steering_pressed=True, steering_angle=0.0), actuators) + for _ in range(30): + result = controller.update_curvature( + CC, car_state(steering_pressed=True, steering_angle=50.0), actuators) + + assert result.active + assert result.curvature == 0.0 assert result.path_angle == 0.0 - assert update(CC, car_state(), actuators).active +def test_angle_control_pulses_inactive_after_sustained_driver_correction(controller): + controller.human_turn_enabled = True + CC = SimpleNamespace(latActive=True) + actuators = SimpleNamespace(curvature=0.001) -@pytest.mark.parametrize("strategy", ["update_curvature", "update_angle"]) -def test_short_driver_correction_does_not_pause_enhanced_control(controller, strategy): + for _ in range(10): + assert controller.update_angle( + CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active + + for _ in range(HANDOFF_PAUSE_FRAMES): + assert not controller.update_angle(CC, car_state(), actuators).active + + assert controller.update_angle(CC, car_state(), actuators).active + + +def test_short_driver_correction_does_not_pause_angle_control(controller): controller.human_turn_enabled = True CC = SimpleNamespace(latActive=True) actuators = SimpleNamespace(curvature=0.001) - update = getattr(controller, strategy) for _ in range(9): - assert update(CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active + assert controller.update_angle( + CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active - assert update(CC, car_state(), actuators).active + assert controller.update_angle(CC, car_state(), actuators).active def test_angle_control_recovers_from_bounded_tracking_stall(controller): diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 0fd2160fd..7ee7b669c 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -4398,6 +4398,18 @@ "parent_key": "GalaxyDeveloperMode", "settings_tier": "advanced" }, + { + "key": "TurnSteeringLimitMuteSpeed", + "label": "Mute Turn Limit Alert Below", + "description": "Mute only the sound for \"Turn Exceeds Steering Limit\" below this speed. The visual alert is always kept visible. Set to 0 to disable. The value uses your selected mph or km/h unit.", + "data_type": "int", + "ui_type": "numeric", + "min": 0.0, + "max": 99.0, + "step": 1.0, + "parent_key": "GalaxyDeveloperMode", + "settings_tier": "advanced" + }, { "key": "CameraOffset", "label": "Camera Offset", @@ -4487,7 +4499,7 @@ "ui_type": "numeric", "min": 0.1, "max": 4.0, - "step": 0.05, + "step": 0.01, "precision": 2, "settings_tier": "advanced" }, @@ -4499,7 +4511,7 @@ "ui_type": "numeric", "min": 0.1, "max": 4.0, - "step": 0.05, + "step": 0.01, "precision": 2, "settings_tier": "advanced" }, diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 7a379ca6b..033e5b7be 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -770,6 +770,14 @@ class StarPilotVariables: toggle.alert_volume_controller = self.get_value("AlertVolumeControl") toggle.below_steer_speed_volume = self.get_value("BelowSteerSpeedVolume", cast=float, condition=toggle.alert_volume_controller) + toggle.turn_steering_limit_mute_speed = self.get_value( + "TurnSteeringLimitMuteSpeed", + cast=float, + condition=self.params.get_bool("GalaxyDeveloperMode"), + conversion=speed_conversion, + min=0, + max=99 * speed_conversion, + ) toggle.switchback_mode_cooldown = self.get_value("SwitchbackModeCooldown", cast=float, conversion=60, min=0, max=1800) toggle.disengage_volume = self.get_value("DisengageVolume", cast=float, condition=toggle.alert_volume_controller) toggle.engage_volume = self.get_value("EngageVolume", cast=float, condition=toggle.alert_volume_controller) diff --git a/starpilot/system/the_galaxy/assets/components/tools/model_manager.css b/starpilot/system/the_galaxy/assets/components/tools/model_manager.css index acec127dc..b47929475 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/model_manager.css +++ b/starpilot/system/the_galaxy/assets/components/tools/model_manager.css @@ -219,6 +219,18 @@ color: var(--danger-fg); } +.mm-chip-egpu { + background: rgba(224, 85, 119, 0.12); + border-color: rgba(224, 85, 119, 0.45); + color: var(--danger-fg); +} + +.mm-chip-device-gpu { + background: rgba(94, 170, 224, 0.12); + border-color: rgba(94, 170, 224, 0.45); + color: var(--text-color); +} + .mm-icon-btn, .mm-star { background: transparent; diff --git a/starpilot/system/the_galaxy/assets/components/tools/model_manager.js b/starpilot/system/the_galaxy/assets/components/tools/model_manager.js index 32b59e6bf..d97da0b69 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/model_manager.js +++ b/starpilot/system/the_galaxy/assets/components/tools/model_manager.js @@ -76,6 +76,10 @@ function normalizeSeries(model) { return safeText(model?.series, "Custom Series") || "Custom Series"; } +function modelHardwareTag(model) { + return model?.requiresGpu ? "eGPU" : "On-device GPU"; +} + function modelSortCompare(a, b) { if (state.sortMode === "release_date") { const dateDelta = parseReleased(b?.released) - parseReleased(a?.released); @@ -516,6 +520,7 @@ function renderModelRow(model) {
${key} ${model.builtin ? html`Built-in` : ""} + ${modelHardwareTag(model)} ${state.sortMode === "release_date" ? "" : model.series ? html`${safeText(model.series)}` : ""} ${model.version ? html`Version ${safeText(model.version)}` : ""} ${model.released ? html`Released ${safeText(model.released)}` : ""} diff --git a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py index 5db789bd6..95a94a738 100644 --- a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py +++ b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py @@ -1671,6 +1671,27 @@ def test_clear_generated_build_state_preserves_prebuilts_and_user_data(tmp_path) assert user_model.read_text() == "test" +def test_sentry_notification_rate_limit_persists_and_expires(monkeypatch, tmp_path): + server = _load_server_module() + rate_limit_path = tmp_path / "sentry_notification_rate_limit.json" + now = [1000.0] + monkeypatch.setattr(server, "_sentry_notification_rate_limit_path", lambda: rate_limit_path) + monkeypatch.setattr(server.time, "time", lambda: now[0]) + server._SENTRY_NOTIFICATION_LAST_AT = None + event = {"eventId": "event-1"} + + assert server._claim_sentry_notification_slot(event) is True + assert rate_limit_path.exists() + server._SENTRY_NOTIFICATION_LAST_AT = None + assert server._claim_sentry_notification_slot({"eventId": "event-2"}) is False + + now[0] += server.SENTRY_NOTIFICATION_RATE_LIMIT_SECONDS - 0.1 + assert server._claim_sentry_notification_slot({"eventId": "event-3"}) is False + + now[0] += 0.1 + assert server._claim_sentry_notification_slot({"eventId": "event-4"}) is True + + def test_troubleshoot_steer_delay_normalizes_vehicle_delay_for_display(): server = _load_server_module() 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 79d938c60..65c832517 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -175,6 +175,34 @@ def test_requested_simple_and_advanced_settings_tiers(): assert sections["Visual (Display & UI)"]["DisableWideRoad"]["settings_tier"] == "advanced" +def test_turn_steering_limit_mute_speed_is_galaxy_developer_only(): + sections = _params_by_section(_layout()) + setting = sections["Developer"]["TurnSteeringLimitMuteSpeed"] + + assert setting["parent_key"] == "GalaxyDeveloperMode" + assert setting["settings_tier"] == "advanced" + assert setting["data_type"] == "int" + assert setting["min"] == 0.0 + assert setting["max"] == 99.0 + assert _declared_default("TurnSteeringLimitMuteSpeed") == "0" + + physical_settings = ( + REPO_ROOT / "selfdrive/ui/layouts/settings/starpilot/sounds.py", + REPO_ROOT / "selfdrive/ui/layouts/settings/starpilot/aethergrid.py", + ) + assert all("TurnSteeringLimitMuteSpeed" not in path.read_text(encoding="utf-8") for path in physical_settings) + + +def test_honda_pid_scale_controls_use_galaxy_fine_granularity(): + developer = _params_by_section(_layout())["Developer"] + + for key in ("HondaLateralPidKpScale", "HondaLateralPidKiScale"): + setting = developer[key] + assert setting["step"] == 0.01 + assert setting["precision"] == 2 + assert setting["settings_tier"] == "advanced" + + def test_hidden_feature_defaults_remain_enabled(): assert _declared_default("GalaxyDeveloperMode") == "0" assert _declared_default("NavDesiresAllowed") == "1" diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index fb65b9993..15c6eaea6 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -113,6 +113,7 @@ LEGACY_LATERAL_METHOD_API_PREFIX = "/api/" + "".join(("f", "t", "m")) VASM_CONFIGURATION_KEYS = {"VASMEnabled", "VASMConfidenceThreshold", "VASMSmoothSeconds", "VASMAnnotationConfig"} PIP_PREVIEW_CONFIGURATION_KEYS = {"PIPPreviewEnabled", "PIPPreviewMask", "PIPPreviewShowOnBlinker", "PIPPreviewShowOnBSM"} MODEL_SMOOTHING_KEYS = {"LatSmoothSeconds", "LongSmoothSeconds"} +GALAXY_DEVELOPER_ONLY_KEYS = {"TurnSteeringLimitMuteSpeed"} PULSE_GLIDE_BUTTON_KEYS = { "CancelButtonControl", "DistanceButtonControl", "LongCancelButtonControl", "LongDistanceButtonControl", @@ -124,6 +125,7 @@ SENTRY_NUMERIC_PARAM_BOUNDS = { "SentryModeSensitivity": (0.005, 1.0), "SentryModeWarningTime": (0.1, 10.0), } +SENTRY_NOTIFICATION_RATE_LIMIT_SECONDS = 180.0 GALAXY_DEPS_PATH = "/data/galaxy_deps" LEGACY_GALAXY_DEPS_PATH = "/data/" + "".join(chr(code) for code in (112, 111, 110, 100)) + "_deps" @@ -786,6 +788,8 @@ def _capture_sentry_live_images() -> list[str]: _SENTRY_PUSH_LOCK = threading.Lock() +_SENTRY_NOTIFICATION_RATE_LIMIT_LOCK = threading.Lock() +_SENTRY_NOTIFICATION_LAST_AT: float | None = None _SENTRY_PUSH_PRIVATE_KEY_NAME = "sentry_vapid_private.pem" _SENTRY_PUSH_SUBSCRIPTIONS_NAME = "sentry_push_subscriptions.json" _SENTRY_PUSH_SUBJECT = os.getenv("STARPILOT_VAPID_SUBJECT", "mailto:galaxy@firestar.link") @@ -908,6 +912,61 @@ def _sentry_notification_channels() -> dict[str, bool]: } +def _sentry_notification_rate_limit_path() -> Path: + return _get_galaxy_dir() / "sentry_notification_rate_limit.json" + + +def _load_sentry_notification_last_at() -> float | None: + try: + payload = json.loads(_sentry_notification_rate_limit_path().read_text()) + value = float(payload.get("lastNotificationAt")) if isinstance(payload, dict) else None + except (OSError, TypeError, ValueError, json.JSONDecodeError): + return None + return value if value is not None and math.isfinite(value) else None + + +def _claim_sentry_notification_slot(event: dict) -> bool: + """Reserve the shared notification slot for a real Sentry event.""" + global _SENTRY_NOTIFICATION_LAST_AT + + now = time.time() + with _SENTRY_NOTIFICATION_RATE_LIMIT_LOCK: + persisted_last_at = _load_sentry_notification_last_at() + last_at = max( + (value for value in (_SENTRY_NOTIFICATION_LAST_AT, persisted_last_at) if value is not None), + default=None, + ) + if last_at is not None: + elapsed = max(0.0, now - last_at) + if elapsed < SENTRY_NOTIFICATION_RATE_LIMIT_SECONDS: + remaining = SENTRY_NOTIFICATION_RATE_LIMIT_SECONDS - elapsed + cloudlog.info( + "Galaxy: Sentry notification suppressed by rate limit (%.0f seconds remaining; event=%s)", + remaining, + event.get("eventId", ""), + ) + return False + + _SENTRY_NOTIFICATION_LAST_AT = now + rate_limit_path = _sentry_notification_rate_limit_path() + temporary_path = rate_limit_path.with_suffix(".tmp") + try: + rate_limit_path.parent.mkdir(parents=True, exist_ok=True) + temporary_path.write_text(json.dumps({ + "lastNotificationAt": now, + "eventId": str(event.get("eventId") or ""), + }, separators=(",", ":"))) + temporary_path.chmod(0o600) + temporary_path.replace(rate_limit_path) + except OSError: + cloudlog.warning("Galaxy: unable to persist Sentry notification rate-limit state") + try: + temporary_path.unlink(missing_ok=True) + except OSError: + pass + return True + + def _sentry_test_notification_event() -> dict: return { "eventId": f"notification-test-{int(time.time())}-{secrets.token_hex(4)}", @@ -968,7 +1027,12 @@ def _dispatch_sentry_push(event: dict) -> None: ]) -def _dispatch_sentry_event(event: dict) -> None: +def _dispatch_sentry_event(event: dict, *, bypass_rate_limit: bool = False) -> None: + if not any(_sentry_notification_channels().values()): + return + if not bypass_rate_limit and not _claim_sentry_notification_slot(event): + return + _dispatch_sentry_push(event) message = f"🚨 StarPilot Sentry Mode: {event['message']}" webhook = (params.get("SentryModeWebhook", encoding="utf-8") or "").strip() @@ -5005,6 +5069,9 @@ def setup(app): if not params.get_bool("GalaxyDeveloperMode"): return jsonify({"error": "Pulse and Glide is available only with Galaxy Developer Mode enabled."}), 403 + if key in GALAXY_DEVELOPER_ONLY_KEYS and not params.get_bool("GalaxyDeveloperMode"): + return jsonify({"error": f"{key} is available only with Galaxy Developer Mode enabled."}), 403 + if key in SENTRY_NUMERIC_PARAM_BOUNDS: minimum, maximum = SENTRY_NUMERIC_PARAM_BOUNDS[key] try: @@ -7209,6 +7276,7 @@ def setup(app): threading.Thread( target=_dispatch_sentry_event, args=(event,), + kwargs={"bypass_rate_limit": True}, name="galaxy-sentry-notification-test", daemon=True, ).start() @@ -7322,7 +7390,13 @@ def setup(app): event["imagePaths"] = _capture_sentry_test_images(event_id) _record_sentry_event(event) params.put("SentryModeLastEvent", json.dumps(event, separators=(",", ":"))) - threading.Thread(target=_dispatch_sentry_event, args=(event,), name="galaxy-sentry-test-notify", daemon=True).start() + threading.Thread( + target=_dispatch_sentry_event, + args=(event,), + kwargs={"bypass_rate_limit": True}, + name="galaxy-sentry-test-notify", + daemon=True, + ).start() threading.Thread(target=capture_and_publish, name="galaxy-sentry-test-capture", daemon=True).start() return jsonify({"accepted": True, "eventId": event_id}), 202