From 5e8501002de1bd5fa033ca67dedbce723e513b74 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 5 Oct 2026 11:50:06 -0500 Subject: [PATCH] Fire Sauce --- common/params_keys.h | 1 + opendbc_repo/opendbc/car/tesla/carstate.py | 35 +++- opendbc_repo/opendbc/car/tesla/interface.py | 11 +- .../car/tesla/tests/test_screen_button.py | 126 ++++++++++++++ opendbc_repo/opendbc/car/tesla/values.py | 4 + .../opendbc/dbc/tesla_model3_vehicle.dbc | 3 + opendbc_repo/opendbc/safety/modes/tesla.h | 38 ++++- .../safety/tests/test_tesla_screen_button.py | 160 ++++++++++++++++++ .../controls/lib/longitudinal_planner.py | 14 +- .../lib/longitudinal_vehicle_tunes.py | 21 ++- .../tests/test_longitudinal_planner.py | 4 +- .../controls/tests/test_niro_lead_follow.py | 132 +++++++++++++++ .../common/assets/device_settings_layout.json | 12 ++ starpilot/common/starpilot_variables.py | 8 +- starpilot/controls/starpilot_card.py | 40 ++++- .../controls/tests/test_starpilot_card.py | 114 +++++++++++++ .../tests/test_device_settings_layout.py | 10 ++ 17 files changed, 717 insertions(+), 16 deletions(-) create mode 100644 opendbc_repo/opendbc/car/tesla/tests/test_screen_button.py create mode 100644 opendbc_repo/opendbc/safety/tests/test_tesla_screen_button.py create mode 100644 selfdrive/controls/tests/test_niro_lead_follow.py diff --git a/common/params_keys.h b/common/params_keys.h index 8f6b7adcc0..789729f4d8 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -746,6 +746,7 @@ inline static std::unordered_map keys = { {"SubaruRedneckCruise", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"TacoTune", {PERSISTENT, BOOL, "0", "0", 2}}, {"TeslaAOLDisengageOnBrake", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, + {"TeslaAOLScreenTap", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, {"TeslaCoopSteering", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"TestAlert", {CLEAR_ON_MANAGER_START, STRING, "", ""}}, {"TetheringEnabled", {PERSISTENT, INT, "0", "0", 0}}, diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index 3a83afbb8e..3e37a4937e 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/carstate.py @@ -1,12 +1,12 @@ import copy from cereal import custom from opendbc.can import CANDefine, CANParser -from opendbc.car import Bus, structs +from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase from opendbc.car.tesla.values import ( DBC, CANBUS, GEAR_MAP, STEER_DISENGAGE_THRESHOLD, STEER_THRESHOLD, TeslaSafetyFlags, - CAR, LEGACY_CARS, + CAR, LEGACY_CARS, TeslaFlags, ) from opendbc.car.tesla.preap.carstate import get_preap_can_parsers, update_preap from opendbc.car.tesla.preap.engagement import PreAPEngagement @@ -19,6 +19,20 @@ TESLA_GAS_PRESS_ON = 0.8 TESLA_GAS_PRESS_OFF = 0.4 +class TeslaScreenCANParser(CANParser): + def __init__(self): + super().__init__("tesla_model3_vehicle", [("UI_status2", 0)], CANBUS.vehicle) + + def update(self, strings, sendcan=False): + if strings and not isinstance(strings[0], list | tuple): + strings = [strings] + # Match Panda's exact-length check before producing engagement button events. + return super().update([ + (timestamp, [frame for frame in frames if frame[0] == 0x3DF and len(frame[1]) == 8]) + for timestamp, frames in strings + ], sendcan) + + def update_tesla_gas_pressed(previous: bool, pedal_position: float) -> bool: threshold = TESLA_GAS_PRESS_OFF if previous else TESLA_GAS_PRESS_ON return float(pedal_position) > threshold @@ -51,6 +65,7 @@ class CarState(CarStateBase): self.cruise_buttons = 0 self.prev_cruise_buttons = 0 self.gas_pressed = False + self.active_touch_points = None self.msg_stw_actn_req = None self.speed_units = "MPH" self.cooperative_steering = any( @@ -86,6 +101,16 @@ class CarState(CarStateBase): return False return super().update_button_enable(buttonEvents) + def update_screen_button(self, cp_vehicle): + events = [] + for touch_points in cp_vehicle.vl_all["UI_status2"]["UI_activeTouchPoints"]: + touch_points = int(touch_points) + # Establish a baseline first; a touch already held during boot is not an engagement request. + if self.active_touch_points is not None: + events.extend(create_button_events(touch_points, self.active_touch_points, {3: ButtonType.lkas})) + self.active_touch_points = touch_points + return events + def update(self, can_parsers, starpilot_toggles) -> structs.CarState: if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP: return update_preap(self, can_parsers) @@ -181,6 +206,8 @@ class CarState(CarStateBase): else: pass # Buttons # ToDo: add Gap adjust button + if self.CP.flags & TeslaFlags.AOL_SCREEN_BUTTON: + ret.buttonEvents = list(ret.buttonEvents) + self.update_screen_button(can_parsers[Bus.adas]) # Messages needed by carcontroller self.das_control = copy.copy(cp_ap_party.vl["DAS_control"]) @@ -279,5 +306,7 @@ class CarState(CarStateBase): } return { Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party), - Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party) + Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party), + **({Bus.adas: TeslaScreenCANParser()} + if CP.flags & TeslaFlags.AOL_SCREEN_BUTTON else {}), } diff --git a/opendbc_repo/opendbc/car/tesla/interface.py b/opendbc_repo/opendbc/car/tesla/interface.py index fbaa9e6675..391321bc78 100644 --- a/opendbc_repo/opendbc/car/tesla/interface.py +++ b/opendbc_repo/opendbc/car/tesla/interface.py @@ -3,7 +3,7 @@ from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.tesla.carcontroller import CarController from opendbc.car.tesla.carstate import CarState from opendbc.car.tesla.radar_interface import RadarInterface -from opendbc.car.tesla.values import TeslaSafetyFlags, CAR, DBC, LEGACY_CARS +from opendbc.car.tesla.values import TeslaFlags, TeslaSafetyFlags, CANBUS, CAR, DBC, LEGACY_CARS from opendbc.car.tesla.preap.interface import get_preap_accel_limits, get_preap_params @@ -23,6 +23,12 @@ class CarInterface(CarInterfaceBase): ret = super().get_params(candidate, fingerprint, car_fw, alpha_long, is_release, docs, starpilot_toggles) if candidate == CAR.TESLA_MODEL_3 and getattr(starpilot_toggles, "tesla_cooperative_steering", False): ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.COOP_STEERING.value + if (ret.flags & TeslaFlags.HAS_VEHICLE_BUS and + getattr(starpilot_toggles, "tesla_aol_screen_tap_requested", False)): + ret.flags |= TeslaFlags.AOL_SCREEN_BUTTON.value + ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.AOL_SCREEN_BUTTON.value + if getattr(starpilot_toggles, "tesla_aol_screen_brake_disengage_requested", False): + ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE.value return ret @staticmethod @@ -49,6 +55,9 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)] + if candidate in (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y) and fingerprint[CANBUS.vehicle].get(0x3DF) == 8: + ret.flags |= TeslaFlags.HAS_VEHICLE_BUS.value + ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 ret.steerAtStandstill = True diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_screen_button.py b/opendbc_repo/opendbc/car/tesla/tests/test_screen_button.py new file mode 100644 index 0000000000..1a35140dd0 --- /dev/null +++ b/opendbc_repo/opendbc/car/tesla/tests/test_screen_button.py @@ -0,0 +1,126 @@ +from types import SimpleNamespace + +import pytest + +from cereal import custom +from opendbc.can import CANPacker +from opendbc.car import Bus, gen_empty_fingerprint +from opendbc.car.tesla.carstate import ButtonType, CarState +from opendbc.car.tesla.interface import CarInterface +from opendbc.car.tesla.values import CANBUS, CAR, TeslaFlags, TeslaSafetyFlags + + +def screen_params(candidate=CAR.TESLA_MODEL_3, enabled=True, bus=CANBUS.vehicle, length=8, alpha_long=False): + fingerprint = gen_empty_fingerprint() + if bus is not None: + fingerprint[bus][0x3DF] = length + toggles = SimpleNamespace(tesla_aol_screen_tap_requested=enabled, trailer_load_kg=0.0) + return CarInterface.get_params(candidate, fingerprint, [], alpha_long, False, False, toggles) + + +@pytest.mark.parametrize("candidate", tuple(CAR)) +@pytest.mark.parametrize("enabled", (False, True)) +def test_detection_and_safety_flags_are_model_3_y_only(candidate, enabled): + cp = screen_params(candidate, enabled) + detected = candidate in (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y) + assert bool(cp.flags & TeslaFlags.HAS_VEHICLE_BUS) == detected + assert bool(cp.flags & TeslaFlags.AOL_SCREEN_BUTTON) == (detected and enabled) + assert any(config.safetyParam & TeslaSafetyFlags.AOL_SCREEN_BUTTON for config in cp.safetyConfigs) == (detected and enabled) + + +@pytest.mark.parametrize(("bus", "length"), ((None, 8), (0, 8), (2, 8), (1, 7), (1, 4))) +def test_harness_requires_correct_bus_and_length(bus, length): + cp = screen_params(bus=bus, length=length) + assert not cp.flags & (TeslaFlags.HAS_VEHICLE_BUS | TeslaFlags.AOL_SCREEN_BUTTON) + assert not cp.safetyConfigs[0].safetyParam & TeslaSafetyFlags.AOL_SCREEN_BUTTON + assert Bus.adas not in CarState.get_can_parsers(cp) + + +@pytest.mark.parametrize("alpha_long", (False, True)) +def test_gesture_does_not_select_longitudinal(alpha_long): + baseline = screen_params(enabled=False, alpha_long=alpha_long) + enabled = screen_params(alpha_long=alpha_long) + assert enabled.openpilotLongitudinalControl == baseline.openpilotLongitudinalControl + assert enabled.pcmCruise == baseline.pcmCruise + assert enabled.safetyConfigs[0].safetyParam == baseline.safetyConfigs[0].safetyParam | TeslaSafetyFlags.AOL_SCREEN_BUTTON + + +@pytest.mark.parametrize("enabled", (False, True)) +def test_screen_brake_safety_flag_requires_the_gesture_feature(enabled): + fingerprint = gen_empty_fingerprint() + fingerprint[1][0x3DF] = 8 + toggles = SimpleNamespace(tesla_aol_screen_tap_requested=enabled, tesla_aol_screen_brake_disengage_requested=True, + trailer_load_kg=0.0) + cp = CarInterface.get_params(CAR.TESLA_MODEL_3, fingerprint, [], False, False, False, toggles) + assert bool(cp.safetyConfigs[0].safetyParam & TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE) == enabled + + +def test_parser_only_added_with_setting_and_detected_addon(): + assert Bus.adas not in CarState.get_can_parsers(screen_params(enabled=False)) + parser = CarState.get_can_parsers(screen_params())[Bus.adas] + assert parser.bus == CANBUS.vehicle + assert parser.message_states[0x3DF].ignore_alive + + +@pytest.mark.parametrize(("counts", "expected"), ( + ([0, 3, 3, 3, 0, 0, 3, 0], [True, False, True, False]), + ([3, 3, 0, 3, 0], [False, True, False]), + ([0, 1, 2, 4, 5, 0], []), +)) +def test_touch_edges_and_startup_baseline(counts, expected): + cp = screen_params() + fp_cp = custom.StarPilotCarParams.new_message() + cs = CarState(cp, fp_cp) + parser = CarState.get_can_parsers(cp)[Bus.adas] + packer = CANPacker("tesla_model3_vehicle") + events = [] + for i, count in enumerate(counts): + parser.update([[int((i + 1) * 0.5e9), [packer.make_can_msg("UI_status2", CANBUS.vehicle, {"UI_activeTouchPoints": count})]]]) + events.extend(cs.update_screen_button(parser)) + assert [event.pressed for event in events if event.type == ButtonType.lkas] == expected + parser.update([[60_000_000_000, []]]) + assert cs.update_screen_button(parser) == [] + assert parser.can_valid + assert not parser.bus_timeout + + +def test_multiple_touch_edges_in_one_update_are_not_lost(): + cp = screen_params() + fp_cp = custom.StarPilotCarParams.new_message() + cs = CarState(cp, fp_cp) + parser = CarState.get_can_parsers(cp)[Bus.adas] + packer = CANPacker("tesla_model3_vehicle") + parser.update([[i * 10_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": count})]] + for i, count in enumerate((0, 3, 0, 3, 0), 1)]) + assert [event.pressed for event in cs.update_screen_button(parser) if event.type == ButtonType.lkas] == [True, False, True, False] + + +@pytest.mark.parametrize("length", (0, 3, 4, 5, 6, 7, 9, 12)) +def test_malformed_touch_frames_do_not_produce_button_events(length): + cp = screen_params() + cs = CarState(cp, custom.StarPilotCarParams.new_message()) + parser = CarState.get_can_parsers(cp)[Bus.adas] + packer = CANPacker("tesla_model3_vehicle") + parser.update([[1_000_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": 0})]]]) + cs.update_screen_button(parser) + address, data, bus = packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": 3}) + parser.update([[2_000_000_000, [(address, data[:length].ljust(length, b"\x00"), bus)]]]) + assert cs.update_screen_button(parser) == [] + assert cs.active_touch_points == 0 + parser.update([[3_000_000_000, [(address, data, bus)]]]) + assert any(event.type == ButtonType.lkas and event.pressed for event in cs.update_screen_button(parser)) + + +def test_button_is_appended_without_changing_cruise_state(): + cp = screen_params() + fp_cp = custom.StarPilotCarParams.new_message() + cs = CarState(cp, fp_cp) + parsers = CarState.get_can_parsers(cp) + parsers[Bus.party].vl["DI_state"]["DI_cruiseState"] = 0 + packer = CANPacker("tesla_model3_vehicle") + for count in (0, 3): + parsers[Bus.adas].update([[1_000_000_000, [packer.make_can_msg("UI_status2", 1, {"UI_activeTouchPoints": count})]]]) + ret, _ = cs.update(parsers, SimpleNamespace()) + assert any(event.type == ButtonType.lkas and event.pressed for event in ret.buttonEvents) + assert not ret.cruiseState.enabled + assert not ret.cruiseState.available diff --git a/opendbc_repo/opendbc/car/tesla/values.py b/opendbc_repo/opendbc/car/tesla/values.py index 50c74f381f..cfc29c049d 100644 --- a/opendbc_repo/opendbc/car/tesla/values.py +++ b/opendbc_repo/opendbc/car/tesla/values.py @@ -144,10 +144,14 @@ class TeslaSafetyFlags(IntFlag): FLAG_EXTERNAL_PANDA = 4 FLAG_HW1 = 8 COOP_STEERING = 256 + AOL_SCREEN_BUTTON = 512 + AOL_SCREEN_DISENGAGE_ON_BRAKE = 1024 class TeslaFlags(IntFlag): LONG_CONTROL = 1 + HAS_VEHICLE_BUS = 2 + AOL_SCREEN_BUTTON = 4 class CruiseButtons: diff --git a/opendbc_repo/opendbc/dbc/tesla_model3_vehicle.dbc b/opendbc_repo/opendbc/dbc/tesla_model3_vehicle.dbc index 604db8fe27..8e47460c38 100644 --- a/opendbc_repo/opendbc/dbc/tesla_model3_vehicle.dbc +++ b/opendbc_repo/opendbc/dbc/tesla_model3_vehicle.dbc @@ -236,6 +236,9 @@ BO_ 1013 ID3F5VCFRONT_lighting: 8 VEH SG_ VCFRONT_indicatorRightRequest : 2|2@1+ (1,0) [0|2] "" Receiver SG_ VCFRONT_indicatorLeftRequest : 0|2@1+ (1,0) [0|2] "" Receiver +BO_ 991 UI_status2: 8 VehicleBus + SG_ UI_activeTouchPoints : 24|8@1+ (1,0) [0|255] "" VehicleBus + VAL_ 568 SpdCtrlLvr_Stat 32 "DN_1ST" 16 "UP_1ST" 8 "DN_2ND" 4 "UP_2ND" 2 "RWD" 1 "FWD" 0 "IDLE" ; VAL_ 568 DTR_Dist_Rq 255 "SNA" 200 "ACC_DIST_7" 166 "ACC_DIST_6" 133 "ACC_DIST_5" 100 "ACC_DIST_4" 66 "ACC_DIST_3" 33 "ACC_DIST_2" 0 "ACC_DIST_1" ; VAL_ 568 TurnIndLvr_Stat 3 "SNA" 2 "RIGHT" 1 "LEFT" 0 "IDLE" ; diff --git a/opendbc_repo/opendbc/safety/modes/tesla.h b/opendbc_repo/opendbc/safety/modes/tesla.h index d5081653b6..89e360fd9d 100644 --- a/opendbc_repo/opendbc/safety/modes/tesla.h +++ b/opendbc_repo/opendbc/safety/modes/tesla.h @@ -4,6 +4,9 @@ static bool tesla_longitudinal = false; static bool tesla_coop_steering = false; +static bool tesla_aol_screen_button = false; +static bool tesla_screen_disengage_on_brake = false; +static bool tesla_touch_initialized = false; static bool tesla_stock_aeb = false; #define TESLA_STEERING_DISENGAGE_TORQUE 500 // cNm @@ -94,6 +97,20 @@ static bool tesla_get_quality_flag_valid(const CANPacket_t *msg) { return valid; } +static void tesla_rx_all_hook(const CANPacket_t *msg) { + // The add-on's UI message is asynchronous, not a required periodic safety input. + if (tesla_aol_screen_button && (msg->bus == 1U) && (msg->addr == 0x3DFU) && (GET_LEN(msg) == 8)) { + const bool screen_button = msg->data[3] == 3U; // UI_activeTouchPoints + if (tesla_touch_initialized && screen_button && !lkas_button_prev && !steering_disengage && + !(tesla_screen_disengage_on_brake && brake_pressed)) { + lkas_on = !aol_allowed; + } + // A touch held during startup is not an engagement request. + tesla_touch_initialized = true; + lkas_button_prev = screen_button; + } +} + static void tesla_rx_hook(const CANPacket_t *msg) { if (msg->bus == 0U) { @@ -112,6 +129,9 @@ static void tesla_rx_hook(const CANPacket_t *msg) { steering_disengage = (hands_on_level >= 3) || (tesla_coop_steering && (SAFETY_ABS(torsion_bar_torque) > TESLA_STEERING_DISENGAGE_TORQUE)) || ((eac_status == 0) && (eac_error_code == 9)); + if (tesla_aol_screen_button && steering_disengage) { + lkas_on = false; + } } // Vehicle speed (DI_speed) @@ -136,6 +156,9 @@ static void tesla_rx_hook(const CANPacket_t *msg) { // Brake pressed if (msg->addr == 0x39dU) { brake_pressed = (msg->data[2] & 0x03U) == 2U; + if (tesla_screen_disengage_on_brake && brake_pressed) { + lkas_on = false; + } } // Cruise and Autopark/Summon state @@ -166,7 +189,11 @@ static void tesla_rx_hook(const CANPacket_t *msg) { pcm_cruise_check(cruise_engaged); - acc_main_on = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark; + const bool acc_main_on_now = ((cruise_state == 1) || cruise_engaged) && !tesla_autopark; + if (tesla_aol_screen_button && acc_main_on && !acc_main_on_now && !brake_pressed) { + lkas_on = false; + } + acc_main_on = acc_main_on_now; } if (msg->addr == 0x155U) { @@ -187,7 +214,8 @@ static void tesla_rx_hook(const CANPacket_t *msg) { bool tesla_stock_lkas_now = steering_control_type == 2; // "LANE_KEEP_ASSIST" // Only consider rising edges while controls are not allowed - if (tesla_stock_lkas_now && !tesla_stock_lkas_prev && !controls_allowed) { + if (tesla_stock_lkas_now && !tesla_stock_lkas_prev && + !(controls_allowed || (tesla_aol_screen_button && aol_allowed))) { tesla_stock_lkas = true; } if (!tesla_stock_lkas_now) { @@ -337,6 +365,11 @@ static safety_config tesla_init(uint16_t param) { }; SAFETY_UNUSED(param); + const uint16_t TESLA_FLAG_AOL_SCREEN_BUTTON = 512; + const uint16_t TESLA_FLAG_AOL_SCREEN_DISENGAGE_ON_BRAKE = 1024; + tesla_aol_screen_button = GET_FLAG(param, TESLA_FLAG_AOL_SCREEN_BUTTON); + tesla_screen_disengage_on_brake = tesla_aol_screen_button && GET_FLAG(param, TESLA_FLAG_AOL_SCREEN_DISENGAGE_ON_BRAKE); + tesla_touch_initialized = false; #ifdef ALLOW_DEBUG const uint16_t TESLA_FLAG_LONGITUDINAL_CONTROL = 1; const uint16_t TESLA_FLAG_COOP_STEERING = 256; @@ -377,6 +410,7 @@ static safety_config tesla_init(uint16_t param) { const safety_hooks tesla_hooks = { .init = tesla_init, + .rx_all = tesla_rx_all_hook, .rx = tesla_rx_hook, .tx = tesla_tx_hook, .fwd = tesla_fwd_hook, diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_screen_button.py b/opendbc_repo/opendbc/safety/tests/test_tesla_screen_button.py new file mode 100644 index 0000000000..d67d08aac5 --- /dev/null +++ b/opendbc_repo/opendbc/safety/tests/test_tesla_screen_button.py @@ -0,0 +1,160 @@ +import unittest + +from opendbc.car.structs import CarParams +from opendbc.car.tesla.values import TeslaSafetyFlags +from opendbc.safety import ALTERNATIVE_EXPERIENCE +from opendbc.safety.tests.common import CANPackerSafety, make_msg +from opendbc.safety.tests import test_tesla + + +class TestTeslaScreenButton(unittest.TestCase): + TX_MSGS = None + + def setUp(self): + self.helper = test_tesla.TestTeslaStockSafety() + self.helper.setUp() + self.safety = self.helper.safety + self.vehicle_packer = CANPackerSafety("tesla_model3_vehicle") + self.init_safety() + + def init_safety(self, flag=True, aol=True, long_control=False, disengage_on_brake=False): + param = (TeslaSafetyFlags.AOL_SCREEN_BUTTON if flag else 0) | (TeslaSafetyFlags.LONG_CONTROL if long_control else 0) + if disengage_on_brake: + param |= TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE + self.assertEqual(self.safety.set_safety_hooks(CarParams.SafetyModel.tesla, param), 0) + self.safety.init_tests() + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL if aol else 0) + self.helper._rx(self.helper._pcm_status_msg(False)) + self.safety.set_angle_meas(0, 0) + self.safety.set_desired_angle_last(0) + + def touch(self, fingers, bus=1): + self.helper._rx(self.vehicle_packer.make_can_msg_safety("UI_status2", bus, {"UI_activeTouchPoints": fingers})) + + def gesture(self): + self.touch(0) + self.touch(3) + + def test_gesture_enables_only_lateral(self): + self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0))) + self.gesture() + self.assertTrue(self.safety.get_aol_allowed()) + self.assertTrue(self.safety.get_lkas_on()) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0))) + self.assertFalse(self.helper._tx(self.helper._long_control_msg(10, acc_state=2, accel_limits=(0, 1)))) + self.gesture() + self.assertFalse(self.safety.get_aol_allowed()) + self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0))) + + def test_does_not_enable_alpha_long_actuation(self): + self.init_safety(long_control=True) + self.gesture() + self.assertTrue(self.safety.get_aol_allowed()) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertFalse(self.helper._tx(self.helper._long_control_msg(10, acc_state=2, accel_limits=(0, 1)))) + + def test_flag_and_aol_are_required(self): + for flag, aol in ((False, False), (False, True), (True, False)): + with self.subTest(flag=flag, aol=aol): + self.init_safety(flag=flag, aol=aol) + self.gesture() + self.assertFalse(self.safety.get_aol_allowed()) + self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0))) + + def test_wrong_bus_and_length_do_not_toggle(self): + for bus in (0, 2, 3): + self.gesture_on_wrong_bus(bus) + self.assertFalse(self.safety.get_lkas_on()) + self.touch(0) + for length in (0, 3, 4, 5, 6, 7): + msg = make_msg(1, 0x3DF, length) + if length > 3: + msg[0].data[3] = 3 + self.helper._rx(msg) + self.assertFalse(self.safety.get_lkas_on()) + + def gesture_on_wrong_bus(self, bus): + self.touch(0, bus) + self.touch(3, bus) + + def test_held_touch_and_other_finger_counts(self): + for fingers in (3, 3, 0, 1, 2, 4, 5, 255, 0): + self.touch(fingers) + self.assertFalse(self.safety.get_lkas_on()) + self.touch(3) + for _ in range(20): + self.touch(3) + self.assertTrue(self.safety.get_lkas_on()) + self.touch(0) + self.touch(3) + self.assertFalse(self.safety.get_lkas_on()) + + def test_brake_disables_long_but_not_gesture_lateral(self): + self.gesture() + self.helper._rx(self.helper._user_brake_msg(True)) + self.assertTrue(self.safety.get_aol_allowed()) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0))) + + def test_opt_in_brake_disengage_needs_brake_release_and_new_gesture(self): + self.init_safety(disengage_on_brake=True) + self.gesture() + self.helper._rx(self.helper._user_brake_msg(True)) + self.assertFalse(self.safety.get_aol_allowed()) + self.gesture() + self.assertFalse(self.safety.get_aol_allowed()) + self.helper._rx(self.helper._user_brake_msg(False)) + self.assertFalse(self.safety.get_aol_allowed()) + self.gesture() + self.assertTrue(self.safety.get_aol_allowed()) + + def test_steering_override_requires_a_new_gesture(self): + self.gesture() + self.helper._rx(self.helper._angle_meas_msg(0, hands_on_level=3)) + self.assertFalse(self.safety.get_aol_allowed()) + self.gesture() + self.assertFalse(self.safety.get_aol_allowed()) + self.helper._rx(self.helper._angle_meas_msg(0)) + self.assertFalse(self.safety.get_aol_allowed()) + self.gesture() + self.assertTrue(self.safety.get_aol_allowed()) + + def test_cancelled_cruise_clears_gesture_latch(self): + self.gesture() + self.helper._rx(self.helper._pcm_status_msg(True)) + self.helper._rx(self.helper._pcm_status_msg(False)) + self.assertFalse(self.safety.get_aol_allowed()) + self.assertFalse(self.safety.get_lkas_on()) + + def test_safety_reinitialization_clears_touch_latch(self): + self.gesture() + self.init_safety() + self.touch(3) + self.assertFalse(self.safety.get_aol_allowed()) + + def test_stock_lkas_does_not_block_authorized_gesture_steering(self): + self.gesture() + self.helper._rx(self.helper._angle_cmd_msg(0, state=2, bus=2)) + self.assertTrue(self.helper._tx(self.helper._angle_cmd_msg(0))) + + def test_stock_lkas_still_blocks_when_not_authorized(self): + self.helper._rx(self.helper._angle_cmd_msg(0, state=2, bus=2)) + self.gesture() + self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0))) + + def test_autopark_still_blocks_steering(self): + self.helper._rx(self.helper._pcm_status_msg(False, autopark_state=3)) + self.gesture() + self.assertFalse(self.helper._tx(self.helper._angle_cmd_msg(0))) + + def test_aeb_still_blocks_longitudinal_and_is_forwarded(self): + self.init_safety(long_control=True) + self.gesture() + self.helper._rx(self.helper._long_control_msg(0, aeb_event=1, bus=2)) + self.assertFalse(self.helper._tx(self.helper._long_control_msg(0, acc_state=13))) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x2B9), 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 1683b6bf34..da953fb304 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 apply as apply_f from openpilot.selfdrive.controls.lib.lead_follow_policy import is_nonurgent_duplicate_vision_follow from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_far_follow_output_slew_rates, + get_far_follow_output_slew_min_speed, + is_kia_niro_ev_follow_lead, get_follow_prebrake_min_headway, get_honda_accord_lead_departure_tune, get_honda_accord_stop_go_accel_cap, @@ -606,6 +608,7 @@ class LongitudinalPlanner: self.output_a_target = 0.0 self.output_should_stop = False self.far_follow_brake_slew_rate, self.far_follow_release_slew_rate = get_far_follow_output_slew_rates(CP) + self.far_follow_slew_min_speed = get_far_follow_output_slew_min_speed(CP, VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED) self.untracked_slow_lead_decel_scale = get_untracked_slow_lead_decel_scale(CP) self.tracked_lead_catchup_headway_margins = get_tracked_lead_catchup_headway_margins(CP) self.far_follow_output_slew_active = False @@ -1415,7 +1418,7 @@ class LongitudinalPlanner: if bool(getattr(lead, "status", False)) and abs(float(getattr(lead, "yRel", 0.0))) <= VEHICLE_FAR_FOLLOW_SLEW_MAX_LATERAL_OFFSET ] - safe_far_follow = bool(centered_leads and float(v_ego) >= VEHICLE_FAR_FOLLOW_SLEW_MIN_SPEED) + safe_far_follow = bool(centered_leads and float(v_ego) >= self.far_follow_slew_min_speed) for lead in centered_leads: distance = float(getattr(lead, "dRel", 0.0)) closing_speed = max(0.0, float(v_ego) - float(getattr(lead, "vLead", v_ego))) @@ -2137,6 +2140,13 @@ class LongitudinalPlanner: any(is_toyota_rav4_tss2_radar_follow_lead(self.CP, lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) ) + niro_follow = ( + not experimental_mode and + not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and + not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and + not bool(getattr(sm['starpilotPlan'], 'stopSignConfirmed', False)) and + any(is_kia_niro_ev_follow_lead(self.CP, lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) + ) lightning_stopped_radar_follow = ( experimental_mode and not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and @@ -2148,7 +2158,7 @@ class LongitudinalPlanner: # StarPilot trackingLead is debounce/model-length based. Keep a raw close-lead # safety path so ACC/chill does not ignore a visible lead during that debounce. lead_control_active = ( - tracking_lead or raw_close_lead_control or early_truck_follow or rav4_radar_follow or + tracking_lead or raw_close_lead_control or early_truck_follow or rav4_radar_follow or niro_follow or lightning_stopped_radar_follow or any(is_toyota_corolla_early_radar_follow_lead(self.CP, lead, scene_v_ego) for lead in (self.lead_one, self.lead_two)) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index eef254e4be..5103b73e01 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -24,8 +24,8 @@ HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED = 0.25 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75 KIA_NIRO_EV_LEAD_FOLLOW_JERK_SCALE = 1.5 -KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.5 -KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.75 +KIA_NIRO_EV_FAR_FOLLOW_BRAKE_SLEW_RATE = 2.0 +KIA_NIRO_EV_FAR_FOLLOW_RELEASE_SLEW_RATE = 1.25 GENESIS_GV70_ELECTRIFIED_SCC_JERK_UPPER = 1.5 GENESIS_GV70_ELECTRIFIED_SCC_JERK_LOWER = 2.0 GENESIS_GV70_ELECTRIFIED_SCC_URGENT_JERK_LOWER = 5.0 @@ -588,6 +588,23 @@ def allow_radar_standstill_gap_settle(CP): ) +def is_kia_niro_ev_follow_lead(CP, lead, v_ego): + if ( + getattr(CP, "brand", "") != "hyundai" or str(getattr(CP, "carFingerprint", "")) != "KIA_NIRO_EV" or + lead is None or not bool(getattr(lead, "status", False)) or bool(getattr(lead, "radar", False)) or + float(getattr(lead, "modelProb", 0.0)) < 0.95 or + abs(float(getattr(lead, "yRel", 0.0))) > 1.5 or float(v_ego) < 5.0 + ): + return False + return 10.0 <= float(lead.dRel) <= max(40.0, 2.5 * float(v_ego)) + + +def get_far_follow_output_slew_min_speed(CP, default_min_speed): + if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "KIA_NIRO_EV": + return 5.0 + return default_min_speed + + def get_far_follow_output_slew_rates(CP): if getattr(CP, "brand", "") == "hyundai" and str(getattr(CP, "carFingerprint", "")) == "KIA_NIRO_EV": return ( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3133c26827..e5a3c41903 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -383,12 +383,12 @@ def test_niro_ev_far_follow_slew_is_vehicle_specific_and_preserves_urgent_brakin planner.lead_one = make_lead(status=True, d_rel=45.0, v_lead=15.0, model_prob=0.99, y_rel=0.0) planner.lead_two = make_lead(status=False) - assert get_far_follow_output_slew_rates(CP) == pytest.approx((2.5, 1.75)) + assert get_far_follow_output_slew_rates(CP) == pytest.approx((2.0, 1.25)) assert get_far_follow_output_slew_rates(next_gen) == (0.0, 0.0) first = planner.get_vehicle_far_follow_slew_target(16.0, 0.0, -0.4, False, False) release = planner.get_vehicle_far_follow_slew_target(16.0, first, 0.3, False, False) assert first == pytest.approx(-0.4) - assert release == pytest.approx(first + 1.75 * planner.dt) + assert release == pytest.approx(first + 1.25 * planner.dt) planner.lead_one.dRel = 18.0 assert planner.get_vehicle_far_follow_slew_target(16.0, release, -1.5, False, False) == pytest.approx(-1.5) diff --git a/selfdrive/controls/tests/test_niro_lead_follow.py b/selfdrive/controls/tests/test_niro_lead_follow.py new file mode 100644 index 0000000000..7c1b6ca38c --- /dev/null +++ b/selfdrive/controls/tests/test_niro_lead_follow.py @@ -0,0 +1,132 @@ +from types import SimpleNamespace + +import pytest + +from opendbc.car.hyundai.interface import CarInterface +from opendbc.car.hyundai.values import CAR +from openpilot.selfdrive.controls.lib import longitudinal_planner as planner_module +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( + get_far_follow_output_slew_min_speed, + is_kia_niro_ev_follow_lead, +) +from openpilot.selfdrive.controls.tests.test_longitudinal_planner import make_lead, make_sm, make_toggles + + +@pytest.mark.parametrize("brand,fingerprint", [ + ("hyundai", "KIA_NIRO_EV_2ND_GEN"), + ("hyundai", "HYUNDAI_IONIQ_6"), + ("hyundai", "KIA_EV9"), + ("toyota", "TOYOTA_COROLLA_TSS2"), + ("gm", "CHEVROLET_BOLT_EUV"), + ("gm", "KIA_NIRO_EV"), +]) +def test_follow_tune_does_not_apply_to_other_cars(brand, fingerprint): + cp = SimpleNamespace(brand=brand, carFingerprint=fingerprint) + lead = make_lead(status=True, d_rel=32.0, v_lead=8.5, model_prob=0.99) + assert not is_kia_niro_ev_follow_lead(cp, lead, 9.3) + assert get_far_follow_output_slew_min_speed(cp, 10.0) == 10.0 + + +@pytest.mark.parametrize("speed,fields", [ + (9.3, {"status": False}), + (9.3, {"radar": True}), + (9.3, {"model_prob": 0.94}), + (9.3, {"y_rel": 1.51}), + (9.3, {"d_rel": 40.1}), + (25.0, {"d_rel": 62.6}), + (9.3, {"d_rel": 9.9}), + (4.9, {}), +]) +def test_follow_tune_excludes_uncertain_far_and_launch_leads(speed, fields): + cp = SimpleNamespace(brand="hyundai", carFingerprint="KIA_NIRO_EV") + values = dict(status=True, d_rel=32.0, v_lead=8.5, model_prob=0.99) + values.update(fields) + assert not is_kia_niro_ev_follow_lead(cp, make_lead(**values), speed) + + +@pytest.mark.parametrize("experimental,scene,expected", [ + (False, None, True), + (True, None, False), + (False, "forcingStop", False), + (False, "redLight", False), + (False, "stopSignConfirmed", False), +]) +@pytest.mark.parametrize("lead_slot", ["lead_one", "lead_two"]) +def test_follow_admission_keeps_scene_and_mode_selection_unchanged(monkeypatch, experimental, scene, expected, lead_slot): + cp = CarInterface.get_non_essential_params(CAR.KIA_NIRO_EV) + planner = LongitudinalPlanner(cp, init_v=9.3) + lead = make_lead(status=True, d_rel=32.0, v_lead=8.5, a_lead=-0.3, model_prob=0.99) + sm = make_sm(9.3, -0.3, -3.5, experimental_mode=experimental, + tracking_lead=False, **{lead_slot: lead}) + if scene: + setattr(sm["starpilotPlan"], scene, True) + captured = [] + update = planner.mpc.update + + def capture(*args, **kwargs): + captured.append(kwargs["tracking_lead"]) + return update(*args, **kwargs) + + monkeypatch.setattr(planner.mpc, "update", capture) + planner.update(sm, make_toggles()) + + assert captured == [expected] + assert planner.mode == ("blended" if experimental else "acc") + assert sm["selfdriveState"].experimentalMode is experimental + assert not sm["starpilotPlan"].trackingLead + if scene: + assert getattr(sm["starpilotPlan"], scene) + + +def test_city_follow_slew_damps_pulses_but_preserves_urgent_targets(): + cp = CarInterface.get_non_essential_params(CAR.KIA_NIRO_EV) + planner = LongitudinalPlanner(cp, init_v=9.3) + planner.lead_one = make_lead(status=True, d_rel=32.0, v_lead=8.5, model_prob=0.99) + planner.lead_two = make_lead(status=False) + + assert planner.far_follow_slew_min_speed == 5.0 + first = planner.get_vehicle_far_follow_slew_target(9.3, 0.0, -0.3, False, False) + braking = planner.get_vehicle_far_follow_slew_target(9.3, first, -0.8, False, False) + release = planner.get_vehicle_far_follow_slew_target(9.3, braking, 1.0, False, False) + assert braking == pytest.approx(first - 2.0 * planner.dt) + assert release == pytest.approx(braking + 1.25 * planner.dt) + assert planner.get_vehicle_far_follow_slew_target(9.3, release, -3.5, False, True) == -3.5 + assert planner.get_vehicle_far_follow_slew_target(9.3, release, -3.5, True, False) == -3.5 + planner.lead_one.dRel = 12.0 + assert planner.get_vehicle_far_follow_slew_target(9.3, release, -3.5, False, False) == -3.5 + planner.lead_one.dRel = 32.0 + planner.lead_one.vLead = 4.0 + assert planner.get_vehicle_far_follow_slew_target(9.3, release, -3.5, False, False) == -3.5 + + +def test_acc_follow_stays_active_across_raw_close_lead_threshold(monkeypatch): + cp = CarInterface.get_non_essential_params(CAR.KIA_NIRO_EV) + baseline = LongitudinalPlanner(cp, init_v=9.3) + candidate = LongitudinalPlanner(cp, init_v=9.3) + old_tracking, new_tracking = [], [] + + def capture(mpc, values): + update = mpc.update + + def record(*args, **kwargs): + values.append(kwargs["tracking_lead"]) + return update(*args, **kwargs) + + monkeypatch.setattr(mpc, "update", record) + + capture(baseline.mpc, old_tracking) + capture(candidate.mpc, new_tracking) + for frame in range(20): + monkeypatch.setattr(planner_module.time, "monotonic", lambda: 100.0 + frame * 0.05) + lead = make_lead(status=True, d_rel=32.0, + v_lead=8.5, a_lead=-0.3 if frame % 2 else -0.6, model_prob=0.99) + lead.aLeadTau = 0.3 + sm = make_sm(9.3, -0.3, -1.0, experimental_mode=False, tracking_lead=False, lead_one=lead) + with monkeypatch.context() as old_gate: + old_gate.setattr(planner_module, "is_kia_niro_ev_follow_lead", lambda *args: False) + baseline.update(sm, make_toggles()) + candidate.update(sm, make_toggles()) + + assert old_tracking == [True, False] * 10 + assert new_tracking == [True] * 20 diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 135d9decc7..a8622a2c46 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -161,6 +161,18 @@ "vehicle_makes": ["Tesla"], "settings_tier": "simple" }, + { + "key": "TeslaAOLScreenTap", + "label": "Three-Finger AOL Tap", + "description": "Toggle Always On Lateral by touching the Tesla screen with three fingers. Requires the Model 3/Y Vehicle CAN add-on cable, which is detected automatically. Does not engage or cancel cruise control. Restart the device after changing this setting or the AOL brake behavior when screen taps are enabled.", + "picker_description": "Toggle steering with a three-finger screen touch using the Vehicle CAN add-on cable.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "AlwaysOnLateral", + "vehicle_makes": ["Tesla"], + "requires_offroad": true, + "settings_tier": "simple" + }, { "key": "LaneChanges", "label": "Lane Changes", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 6c9e9d83b1..84f785c407 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -21,7 +21,7 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, EV_CAR as HYUNDAI_EV_ from opendbc.car.interfaces import TORQUE_SUBSTITUTE_PATH, CarInterfaceBase, GearShifter from opendbc.car.mock.values import CAR as MOCK from opendbc.car.subaru.values import SUBARU_REDNECK_CRUISE_CARS, SUBARU_STOP_START_CARS, SubaruFlags -from opendbc.car.tesla.values import CAR as TESLA_CAR +from opendbc.car.tesla.values import CAR as TESLA_CAR, TeslaFlags from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaStarPilotFlags from openpilot.common.basedir import BASEDIR from openpilot.common.constants import CV @@ -634,6 +634,8 @@ class StarPilotVariables: clear_update_flag = False # CarParams uses this value to select the matching Panda safety configuration. toggle.tesla_cooperative_steering = self.params.get_bool("TeslaCoopSteering") + toggle.tesla_aol_screen_tap_requested = self.params.get_bool("TeslaAOLScreenTap") and self.params.get_bool("AlwaysOnLateral") + toggle.tesla_aol_screen_brake_disengage_requested = self.params.get_bool("TeslaAOLDisengageOnBrake") toggle.rivian_angle_control = self.params.get_bool("RivianAngleControl") fallback_platform = GM_CAR.CHEVROLET_BOLT_ACC_2022_2023 if HARDWARE.get_device_type() == "pc" else MOCK.MOCK @@ -865,6 +867,10 @@ class StarPilotVariables: toggle.tesla_aol_disengage_on_brake = self.get_value( "TeslaAOLDisengageOnBrake", condition=toggle.always_on_lateral and toggle.car_make == "tesla" ) + toggle.tesla_aol_screen_tap = self.get_value( + "TeslaAOLScreenTap", condition=toggle.always_on_lateral and toggle.car_make == "tesla" and + toggle.car_model in (TESLA_CAR.TESLA_MODEL_3, TESLA_CAR.TESLA_MODEL_Y) and bool(CP.flags & TeslaFlags.AOL_SCREEN_BUTTON), + ) main_cruise_button_control = self.get_button_function("MainCruiseButtonControl") toggle.main_cruise_aol_toggle = _main_cruise_aol_allowed(main_cruise_button_control) diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 6a2a4903ac..6776b683ba 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -2,6 +2,7 @@ from opendbc.car import structs from opendbc.car.chrysler.values import pacifica_hybrid_aol_requires_set_press from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, HyundaiFlags +from opendbc.car.tesla.values import CAR as TESLA_CAR, TeslaFlags, TeslaSafetyFlags from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType, is_speed_limit_confirmation_pending @@ -85,6 +86,16 @@ class StarPilotCard: self.prev_brake_pressed = False self.prev_cruise_enabled = False self.tesla_aol_brake_disengaged = False + self.tesla_screen_button = ( + self.CP.brand == "tesla" and + getattr(self.CP, "carFingerprint", None) in (TESLA_CAR.TESLA_MODEL_3, TESLA_CAR.TESLA_MODEL_Y) and + bool(getattr(self.CP, "flags", 0) & TeslaFlags.AOL_SCREEN_BUTTON) + ) + self.tesla_screen_aol_override = None + self.tesla_screen_disengage_on_brake = any( + config.safetyParam & TeslaSafetyFlags.AOL_SCREEN_DISENGAGE_ON_BRAKE + for config in getattr(self.CP, "safetyConfigs", ()) + ) self.decel_pressed = False self.cancelPressed_previously = False self.cancel_pulse_glide_suppressed = False @@ -176,7 +187,8 @@ class StarPilotCard: return False tesla_disengage_on_brake = ( self.CP.brand == "tesla" and - getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False) + (self.tesla_screen_disengage_on_brake if self.tesla_screen_button else + getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False)) ) if tesla_disengage_on_brake and not self.always_on_lateral_allowed and carState.brakePressed: return False @@ -195,6 +207,8 @@ class StarPilotCard: self.controller_aol_override = self.always_on_lateral_allowed if tesla_disengage_on_brake and self.always_on_lateral_allowed: self.tesla_aol_brake_disengaged = False + if self.tesla_screen_button and getattr(starpilot_toggles, "tesla_aol_screen_tap", False): + self.tesla_screen_aol_override = self.always_on_lateral_allowed if carState.cruiseState.enabled or self.pause_lateral: self.pause_lateral = not self.always_on_lateral_allowed return True @@ -271,6 +285,7 @@ class StarPilotCard: for be in carState.buttonEvents ) pulse_glide_lkas_override = ( + not self.tesla_screen_button and (bool(getattr(sm["carControl"], "longActive", False)) or self.pulse_and_glide) and getattr(starpilot_toggles, "pulse_and_glide_via_lkas", False) ) @@ -311,7 +326,8 @@ class StarPilotCard: forte_main_cruise_aol_managed = self.kia_forte_non_scc and starpilot_toggles.main_cruise_aol_toggle tesla_disengage_on_brake = ( self.CP.brand == "tesla" and - getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False) + (self.tesla_screen_disengage_on_brake if self.tesla_screen_button else + getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False)) ) if not tesla_disengage_on_brake: self.tesla_aol_brake_disengaged = False @@ -404,6 +420,24 @@ class StarPilotCard: self.tesla_aol_brake_disengaged = False self.always_on_lateral_allowed = True + if self.tesla_screen_button and getattr(starpilot_toggles, "tesla_aol_screen_tap", False): + # Keep the existing cruise path until a gesture explicitly overrides it. + if engagement_started or (cruise_available_changed and not carState.cruiseState.available and not carState.brakePressed): + if self.tesla_screen_aol_override is not None: + self.pause_lateral = False + self.tesla_screen_aol_override = None + if self.tesla_screen_aol_override is not None: + self.always_on_lateral_allowed = self.tesla_screen_aol_override + if self.tesla_aol_brake_disengaged: + self.always_on_lateral_allowed = False + if cancel_pressed or getattr(carState, "steeringDisengage", False): + self.tesla_screen_aol_override = False + self.always_on_lateral_allowed = False + else: + for be in carState.buttonEvents: + if self._button_type_raw(be) == int(ButtonType.lkas) and be.pressed: + self._toggle_controller_aol(carState, starpilot_toggles) + if (tesla_disengage_on_brake and carState.brakePressed and not self.prev_brake_pressed and self.always_on_lateral_set): self.tesla_aol_brake_disengaged = True @@ -486,7 +520,7 @@ class StarPilotCard: elif not cancel_pressed and self.cancel_pulse_glide_suppressed: self.cancel_pulse_glide_suppressed = False - if lkas_pressed: + if lkas_pressed and not self.tesla_screen_button: if self.CP.brand != "ford" or carState.cruiseState.available: if self.CP.brand == "ford" and getattr(starpilot_toggles, "ford_lkas_aol_toggle", False): self.pause_lateral = not self.pause_lateral diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 49c90e0e16..7333cb4068 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -1396,6 +1396,120 @@ def test_tesla_aol_disengages_on_brake_until_deliberate_reengagement(monkeypatch assert ret.alwaysOnLateralEnabled is True +@pytest.fixture +def tesla_screen_card(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + return spc.StarPilotCard( + SimpleNamespace(brand="tesla", carFingerprint=spc.TESLA_CAR.TESLA_MODEL_3, pcmCruise=True, + flags=spc.TeslaFlags.HAS_VEHICLE_BUS | spc.TeslaFlags.AOL_SCREEN_BUTTON), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + + +def screen_toggles(**overrides): + return make_toggles(always_on_lateral=True, always_on_lateral_main=True, tesla_aol_screen_tap=True, **overrides) + + +def screen_state(**kwargs): + return make_car_state(button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)], **kwargs) + + +def test_tesla_screen_tap_toggles_without_engaging_cruise(tesla_screen_card): + card = tesla_screen_card + toggles, sm, fp_cs = screen_toggles(), make_sm(), SimpleNamespace(distancePressed=False) + cs = screen_state() + assert card.update(cs, fp_cs, sm, toggles).alwaysOnLateralEnabled + assert not cs.cruiseState.enabled + assert not cs.cruiseState.available + assert not sm["carControl"].longActive + for _ in range(10): + assert card.update(make_car_state(), fp_cs, sm, toggles).alwaysOnLateralEnabled + assert not card.update(screen_state(), fp_cs, sm, toggles).alwaysOnLateralEnabled + assert not card.update(make_car_state(), fp_cs, sm, toggles).alwaysOnLateralEnabled + + +def test_tesla_screen_tap_pauses_only_lateral_with_active_cruise(tesla_screen_card): + card = tesla_screen_card + toggles, sm, fp_cs = screen_toggles(pulse_and_glide_via_lkas=True), make_sm(), SimpleNamespace(distancePressed=False) + sm["selfdriveState"].active = True + sm["carControl"].longActive = True + card.update(make_car_state(available=True, enabled=True), fp_cs, sm, toggles) + cs = screen_state(available=True, enabled=True) + ret = card.update(cs, fp_cs, sm, toggles) + assert not ret.alwaysOnLateralEnabled + assert ret.pauseLateral + assert cs.cruiseState.enabled + assert sm["carControl"].longActive + assert not card.pulse_and_glide + assert card.update(make_car_state(available=True, enabled=True), fp_cs, sm, toggles).pauseLateral + ret = card.update(screen_state(available=True, enabled=True), fp_cs, sm, toggles) + assert ret.alwaysOnLateralEnabled + assert not ret.pauseLateral + + +def test_tesla_screen_tap_preserves_brake_and_stalk_paths(tesla_screen_card): + card = tesla_screen_card + toggles, sm, fp_cs = screen_toggles(), make_sm(), SimpleNamespace(distancePressed=False) + card.update(screen_state(), fp_cs, sm, toggles) + assert card.update(make_car_state(brake_pressed=True), fp_cs, sm, toggles).alwaysOnLateralAllowed + card.update(make_car_state(available=True, enabled=True), fp_cs, sm, toggles) + assert not card.update(make_car_state(), fp_cs, sm, toggles).alwaysOnLateralAllowed + sm["selfdriveState"].active = True + assert card.update(make_car_state(available=True, enabled=True), fp_cs, sm, toggles).alwaysOnLateralEnabled + + +def test_tesla_screen_tap_respects_brake_disengage_option(tesla_screen_card): + card = tesla_screen_card + card.tesla_screen_disengage_on_brake = True + toggles, sm, fp_cs = screen_toggles(tesla_aol_disengage_on_brake=True), make_sm(), SimpleNamespace(distancePressed=False) + card.update(screen_state(), fp_cs, sm, toggles) + assert not card.update(make_car_state(brake_pressed=True), fp_cs, sm, toggles).alwaysOnLateralAllowed + assert not card.update(screen_state(brake_pressed=True), fp_cs, sm, toggles).alwaysOnLateralAllowed + assert not card.update(make_car_state(), fp_cs, sm, toggles).alwaysOnLateralAllowed + assert card.update(screen_state(), fp_cs, sm, toggles).alwaysOnLateralAllowed + + +def test_tesla_screen_tap_reenables_after_brake_without_a_blocked_tap(tesla_screen_card): + card = tesla_screen_card + card.tesla_screen_disengage_on_brake = True + toggles, sm, fp_cs = screen_toggles(tesla_aol_disengage_on_brake=True), make_sm(), SimpleNamespace(distancePressed=False) + card.update(screen_state(), fp_cs, sm, toggles) + card.update(make_car_state(brake_pressed=True), fp_cs, sm, toggles) + card.update(make_car_state(), fp_cs, sm, toggles) + assert card.update(screen_state(), fp_cs, sm, toggles).alwaysOnLateralAllowed + + +def test_tesla_screen_tap_does_not_bypass_other_aol_gates(tesla_screen_card): + toggles, sm, fp_cs = screen_toggles(), make_sm(), SimpleNamespace(distancePressed=False) + sm["liveCalibration"].calPerc = 0 + assert not tesla_screen_card.update(screen_state(), fp_cs, sm, toggles).alwaysOnLateralEnabled + sm["liveCalibration"].calPerc = 100 + sm["starpilotPlan"].lateralCheck = False + assert not tesla_screen_card.update(make_car_state(), fp_cs, sm, toggles).alwaysOnLateralEnabled + sm["starpilotPlan"].lateralCheck = True + cs = make_car_state() + cs.steeringDisengage = True + assert not tesla_screen_card.update(cs, fp_cs, sm, toggles).alwaysOnLateralAllowed + assert not tesla_screen_card.update(make_car_state(), fp_cs, sm, toggles).alwaysOnLateralAllowed + + +@pytest.mark.parametrize(("brand", "candidate", "flags", "enabled"), ( + ("tesla", spc.TESLA_CAR.TESLA_MODEL_3, 0, True), + ("tesla", spc.TESLA_CAR.TESLA_MODEL_Y, spc.TeslaFlags.HAS_VEHICLE_BUS, True), + ("tesla", spc.TESLA_CAR.TESLA_MODEL_X, spc.TeslaFlags.AOL_SCREEN_BUTTON, True), + ("hyundai", spc.HYUNDAI_CAR.HYUNDAI_IONIQ_6, spc.TeslaFlags.AOL_SCREEN_BUTTON, True), + ("tesla", spc.TESLA_CAR.TESLA_MODEL_3, spc.TeslaFlags.AOL_SCREEN_BUTTON, False), +)) +def test_tesla_screen_toggle_cannot_affect_other_configs(monkeypatch, tmp_path, brand, candidate, flags, enabled): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + card = spc.StarPilotCard(SimpleNamespace(brand=brand, carFingerprint=candidate, flags=flags), + SimpleNamespace(alternativeExperience=32)) + toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True, tesla_aol_screen_tap=enabled) + assert not card.update(screen_state(), SimpleNamespace(distancePressed=False), make_sm(), toggles).alwaysOnLateralAllowed + + def test_tesla_aol_can_be_manually_reenabled_after_brake_release(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) 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 d23602a57a..5a84596a37 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -89,6 +89,16 @@ def test_tesla_aol_brake_disengage_is_tesla_only_and_opt_in(): assert setting["ui_type"] == "toggle" +def test_tesla_screen_tap_is_optional_parked_only_galaxy_setting(): + setting = _params_by_section(_layout())["Lateral (Steering)"]["TeslaAOLScreenTap"] + assert _declared_default("TeslaAOLScreenTap") == "0" + assert setting["vehicle_makes"] == ["Tesla"] + assert setting["parent_key"] == "AlwaysOnLateral" + assert setting["requires_offroad"] is True + assert setting["ui_type"] == "toggle" + assert "detected automatically" in setting["description"] + + def test_galaxy_new_ui_is_the_visible_default_choice(): galaxy_default = _params_by_section(_layout())["Developer"]["GalaxyMobileDefault"]