mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-09-09 09:43:47 +08:00
fine day, Sunday.
This commit is contained in:
@@ -345,6 +345,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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<std::string, ParamKeyAttributes> 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"}},
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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
|
||||
)
|
||||
|
||||
@@ -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 = []
|
||||
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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")
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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 (
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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."),
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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):
|
||||
|
||||
+35
-9
@@ -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:
|
||||
|
||||
@@ -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'])
|
||||
|
||||
@@ -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]))
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -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"
|
||||
},
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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) {
|
||||
<div class="mm-row-meta">
|
||||
<span class="mm-chip">${key}</span>
|
||||
${model.builtin ? html`<span class="mm-chip">Built-in</span>` : ""}
|
||||
<span class="mm-chip ${model.requiresGpu ? "mm-chip-egpu" : "mm-chip-device-gpu"}">${modelHardwareTag(model)}</span>
|
||||
${state.sortMode === "release_date" ? "" : model.series ? html`<span class="mm-chip">${safeText(model.series)}</span>` : ""}
|
||||
${model.version ? html`<span class="mm-chip">Version ${safeText(model.version)}</span>` : ""}
|
||||
${model.released ? html`<span class="mm-chip">Released ${safeText(model.released)}</span>` : ""}
|
||||
|
||||
@@ -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()
|
||||
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user