diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 60fdad9c19..601fa3d2fc 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -26,6 +26,33 @@ ENABLE_BUTTONS = (Buttons.RES_ACCEL, Buttons.SET_DECEL, Buttons.CANCEL) BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: ButtonType.decelCruise, Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} + +class Ev6AolArmingState: + def __init__(self, lkas_on_engage: bool): + self.lkas_on_engage = lkas_on_engage + self.main_on = False + self.lkas_on = False + self.prev_main_button = False + self.prev_lkas_button = False + self.prev_cruise_button = Buttons.NONE + + def update(self, main_button: bool, lkas_button: bool, cruise_button: int): + if main_button and not self.prev_main_button: + self.main_on = not self.main_on + if lkas_button and not self.prev_lkas_button: + self.lkas_on = not self.lkas_on + if (self.lkas_on_engage and cruise_button != self.prev_cruise_button and + self.prev_cruise_button in (Buttons.SET_DECEL, Buttons.RES_ACCEL)): + self.lkas_on = True + self.prev_main_button = main_button + self.prev_lkas_button = lkas_button + self.prev_cruise_button = cruise_button + + @property + def authorized(self) -> bool: + return self.main_on or self.lkas_on + + IONIQ_6_BLINDSPOT_RIGHT_MASK = 0x08 IONIQ_6_BLINDSPOT_LEFT_MASK = 0x10 CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1 @@ -138,6 +165,13 @@ class CarState(CarStateBase): self.buttons_counter = 0 self.main_cruise_on = False self.main_cruise_tracking = bool(getattr(FPCP, "flags", 0) & HyundaiStarPilotFlags.MAIN_CRUISE_STATE_TRACKING) + self.ev6_aol_arming = None + if CP.carFingerprint == CAR.KIA_EV6 and CP.openpilotLongitudinalControl and not CP.pcmCruise: + lkas_on_engage = any( + config.safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE + for config in (*CP.safetyConfigs, *FPCP.safetyConfigs) + ) + self.ev6_aol_arming = Ev6AolArmingState(lkas_on_engage) if CP.carFingerprint == CAR.KIA_RAY_EV: self.ray_pedal_state = 5 self.ray_pedal_valid = False @@ -181,6 +215,10 @@ class CarState(CarStateBase): # Main button also can trigger an engagement on these cars return any(btn in ENABLE_BUTTONS for btn in self.cruise_buttons) or any(self.main_buttons) + @property + def ev6_aol_authorized(self) -> bool: + return self.ev6_aol_arming is not None and self.ev6_aol_arming.authorized + def update_main_cruise(self, ret: structs.CarState, button_events: list[structs.CarState.ButtonEvent] | None = None) -> bool: button_events = ret.buttonEvents if button_events is None else button_events @@ -640,6 +678,13 @@ class CarState(CarStateBase): *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}), *create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas}), *create_button_events(self.left_paddle, prev_left_paddle, {1: ButtonType.altButton2})] + if self.ev6_aol_arming is not None: + buttons = cp.vl_all[self.cruise_btns_msg_canfd] + for main, lkas, cruise in zip(buttons["ADAPTIVE_CRUISE_MAIN_BTN"], buttons["LDA_BTN"], buttons["CRUISE_BUTTONS"], strict=True): + self.ev6_aol_arming.update(bool(main), bool(lkas), int(cruise)) + if not ret.cruiseState.available: + self.ev6_aol_arming.main_on = False + if self.CP.openpilotLongitudinalControl and (self.CP.carFingerprint == CAR.KIA_EV9 or self.main_cruise_tracking): ret.cruiseState.available = self.update_main_cruise(ret) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 2cc658b90c..eec601dfd9 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -21,7 +21,7 @@ from opendbc.car.hyundai.carcontroller import CarController, CANCEL_BUTTON_DELAY preserve_stock_canfd_lfa_status, \ preserve_stock_canfd_lkas_status, \ suppress_redundant_gv70_brake_cancel -from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \ +from opendbc.car.hyundai.carstate import CarState, Ev6AolArmingState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state, \ get_canfd_cruise_available from opendbc.car.hyundai.interface import CarInterface, KIA_EV9_ACCEL_MAX, get_communication_control_request from opendbc.car.hyundai import hyundaican, hyundaicanfd @@ -130,6 +130,73 @@ def get_test_toggles() -> SimpleNamespace: class TestHyundaiFingerprint: + @pytest.mark.parametrize("candidate, alpha_long, needs_arming", ( + (CAR.KIA_EV6, True, True), (CAR.KIA_EV6, False, False), + (CAR.KIA_EV6_2025, True, False), (CAR.KIA_EV9, True, False), + (CAR.HYUNDAI_IONIQ_5, True, False), (CAR.HYUNDAI_IONIQ_6, True, False), + )) + def test_ev6_aol_arming_is_vehicle_specific(self, candidate, alpha_long, needs_arming): + toggles = get_test_toggles() + CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], alpha_long, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(candidate, gen_empty_fingerprint(), [], CP, toggles) + CS = CarState(CP, FPCP) + assert (CS.ev6_aol_arming is not None) == needs_arming + assert not CS.ev6_aol_authorized + + @pytest.mark.parametrize("alt_buttons", (False, True)) + @pytest.mark.parametrize("lkas_on_engage", (False, True)) + def test_ev6_aol_uses_all_physical_button_samples(self, alt_buttons, lkas_on_engage): + toggles = get_test_toggles() + toggles.always_on_lateral_lkas = lkas_on_engage + fingerprint = gen_empty_fingerprint() + if not alt_buttons: + fingerprint[0][0x1CF] = 8 + CP = CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], True, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.KIA_EV6, fingerprint, [], CP, toggles) + CS = CarState(CP, FPCP) + parsers = CS.get_can_parsers(CP) + packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) + buttons_msg = CS.cruise_btns_msg_canfd + frame = 0 + + def update(samples, acc_available=True): + nonlocal frame + frame += 1 + msgs = [packer.make_can_msg(buttons_msg, CanBus(CP).ECAN, { + "ADAPTIVE_CRUISE_MAIN_BTN": main, "LDA_BTN": lkas, "CRUISE_BUTTONS": cruise, + }) for main, lkas, cruise in samples] + msgs.append(packer.make_can_msg("TCS", CanBus(CP).ECAN, {"ACCEnable": 0 if acc_available else 1})) + parsers[Bus.pt].update([(frame * 10_000_000, msgs)]) + CS.update(parsers, toggles) + return CS.ev6_aol_authorized + + assert not update([(0, 0, Buttons.NONE)]) + assert update([(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)]) + assert update([]) + assert not update([(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)]) + assert update([(1, 0, Buttons.NONE), (1, 0, Buttons.NONE)]) + assert update([(0, 0, Buttons.NONE)]) + assert not update([(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)]) + assert update([(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)]) + assert not update([], acc_available=False) + assert not update([]) + assert not update([(0, 0, Buttons.SET_DECEL)]) + assert update([(0, 0, Buttons.NONE)]) == lkas_on_engage + assert update([]) == lkas_on_engage + + @pytest.mark.parametrize("button", (Buttons.SET_DECEL, Buttons.RES_ACCEL)) + @pytest.mark.parametrize("lkas_on_engage", (False, True)) + def test_ev6_aol_engagement_latch_matches_safety_flag(self, button, lkas_on_engage): + state = Ev6AolArmingState(lkas_on_engage) + state.update(False, False, button) + assert not state.authorized + state.update(False, False, Buttons.NONE) + assert state.authorized == lkas_on_engage + state.update(False, False, Buttons.CANCEL) + assert state.authorized == lkas_on_engage + state.update(False, True, Buttons.NONE) + assert state.authorized != lkas_on_engage + def test_egmp_communication_control_paths(self): stock_request = bytes([0x28, 0x83, 0x01]) radar_keepalive_request = bytes([0x28, 0x01, 0x01]) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 3f5083093c..f884c3a2e6 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -281,9 +281,10 @@ class CarController(CarControllerBase): self.doors_locked = False self.brake_hold_active = False self._brake_hold_counter = 0 + self._auto_hold_rearm_blocked = False def _compute_interceptor_gas_cmd(self, CC, CS): - if not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive): + if self.brake_hold_active or not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive): return 0.0 if CS.out.standstill: @@ -327,17 +328,31 @@ class CarController(CarControllerBase): self.last_standstill = CS.out.standstill def update_auto_hold_state(self, CS: structs.CarState, cancel_requested: bool = False, - activation_frames: int = TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES): + activation_frames: int = TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES, *, + long_active: bool = False, stopping: bool = False): brake_hold_allowed = (not cancel_requested and CS.out.standstill and CS.out.cruiseState.available and - not CS.out.gasPressed and not CS.out.cruiseState.enabled and + not CS.out.gasPressed and (not CS.out.cruiseState.enabled or long_active) and CS.out.gearShifter not in (PARK, REVERSE)) - if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed: - self._brake_hold_counter += 1 - self.brake_hold_active = self._brake_hold_counter > activation_frames - elif not brake_hold_allowed: - self._brake_hold_counter = 0 - self.brake_hold_active = False + if not brake_hold_allowed: + # A gas tap releases this stop, even if the pedal is lifted before the + # wheels start moving. Re-arm once moving or after another brake press. + rearm_blocked = CS.out.standstill and (self._auto_hold_rearm_blocked or CS.out.gasPressed) + self.reset_auto_hold_state() + self._auto_hold_rearm_blocked = rearm_blocked + elif not self.brake_hold_active: + if CS.out.brakePressed: + self._auto_hold_rearm_blocked = False + cruise_stop = long_active and stopping and CS.out.cruiseState.enabled and not self._auto_hold_rearm_blocked + if cruise_stop: + # Latch immediately at a cruise-controlled stop; planner resume requests + # must not release the brakes until the driver presses the gas. + self.brake_hold_active = True + elif CS.out.brakePressed and not CS.out.cruiseState.enabled: + self._brake_hold_counter += 1 + self.brake_hold_active = self._brake_hold_counter > activation_frames + else: + self._brake_hold_counter = 0 return self.brake_hold_active @@ -358,8 +373,11 @@ class CarController(CarControllerBase): return [] def reset_auto_hold_state(self): + if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS: + self.standstill_req = False self._brake_hold_counter = 0 self.brake_hold_active = False + self._auto_hold_rearm_blocked = False def update(self, CC, CS, now_nanos, starpilot_toggles): actuators = CC.actuators @@ -460,7 +478,9 @@ class CarController(CarControllerBase): if self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS: can_sends.extend(self.create_auto_brake_hold_messages(CS)) else: - self.update_auto_hold_state(CS, pcm_cancel_cmd) + self.update_auto_hold_state(CS, pcm_cancel_cmd, long_active=CC.longActive, stopping=stopping) + if self._auto_hold_rearm_blocked: + self.standstill_req = False else: self.reset_auto_hold_state() diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 550d9d7fcf..c48332a339 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -794,6 +794,7 @@ class TestToyotaCarController: controller.accel = 0.0 controller.brake_hold_active = False controller._brake_hold_counter = 0 + controller._auto_hold_rearm_blocked = False return controller @staticmethod @@ -1093,6 +1094,17 @@ class TestToyotaCarController: assert gas_cmd == 0.12 + def test_interceptor_does_not_apply_gas_during_auto_hold(self): + controller = self._make_controller() + controller.CP.enableGasInterceptorDEPRECATED = True + controller.accel = 1.5 + controller.brake_hold_active = True + + assert controller._compute_interceptor_gas_cmd( + SimpleNamespace(longActive=True), + SimpleNamespace(out=SimpleNamespace(standstill=True, vEgo=0.0)), + ) == 0.0 + def test_interceptor_non_stop_and_go_scales_with_accel_request(self): controller = self._make_controller() controller.CP.enableGasInterceptorDEPRECATED = True @@ -1225,6 +1237,181 @@ class TestToyotaCarController: assert CP.stopAccel == -1.5 +class TestToyotaAutoHoldCruise: + @staticmethod + def _make_car(*, candidate=CAR.TOYOTA_RAV4_TSS2, enabled=True, capability=True): + cp = CarInterface.get_non_essential_params(candidate) + cp.flags &= ~ToyotaFlags.AUTO_BRAKE_HOLD.value + if capability: + cp.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value + controller = CarController(DBC[candidate], cp) + cc = structs.CarControl(enabled=True, longActive=True) + cc.actuators.accel = -0.7 + cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.stopping + cc.hudControl.leadDistanceBars = 3 + cs = SimpleNamespace( + out=structs.CarState( + standstill=True, + gearShifter=structs.CarState.GearShifter.drive, + cruiseState=structs.CarState.CruiseState(available=True, enabled=True, standstill=True), + ), + gvc=0.0, + acc_type=1, + pcm_acc_status=7, + pcm_follow_distance=1, + lkas_hud={}, + pre_collision_2={}, + ) + toggles = SimpleNamespace(toyota_auto_hold=enabled, sng_hack=False, lock_doors=False, unlock_doors=False) + parser = CANParser(DBC[candidate][Bus.pt], [("ACC_CONTROL", 0)], 0) + return SimpleNamespace(controller=controller, cc=cc, cs=cs, toggles=toggles, parser=parser) + + @staticmethod + def _tick(car): + # Run through a complete ACC_CONTROL send interval and decode the actual + # controller output, including the ordinary standstill and PID paths. + for _ in range(3): + now_nanos = car.controller.frame * 10_000_000 + _, messages = car.controller.update(car.cc.as_reader(), car.cs, now_nanos, car.toggles) + car.parser.update([(now_nanos, messages)]) + return car.parser.vl["ACC_CONTROL"] + + @pytest.mark.parametrize("sng_hack", [False, True]) + def test_cruise_stop_blocks_planner_resume_until_gas(self, sng_hack): + car = self._make_car() + command = self._tick(car) + assert car.controller.brake_hold_active + assert command["ACCEL_CMD"] == -1.0 + + # Reproduce the report: no brake-pedal input, then the planner changes + # from stopping to starting and asks to resume while still stationary. + car.cc.actuators.accel = 1.5 + car.cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting + car.cc.cruiseControl.resume = True + car.toggles.sng_hack = sng_hack + for _ in range(10): + command = self._tick(car) + assert car.controller.brake_hold_active + assert command["ACCEL_CMD"] == -1.0 + assert command["PERMIT_BRAKING"] == 1 + assert command["RELEASE_STANDSTILL"] == 0 + + car.cs.out.gasPressed = True + car.cc.longActive = False + car.cc.actuators.accel = 0.0 + command = self._tick(car) + assert not car.controller.brake_hold_active + assert command["ACCEL_CMD"] == 0.0 + assert command["RELEASE_STANDSTILL"] == 1 + + def test_gas_tap_does_not_rehold_until_the_next_stop(self): + car = self._make_car() + self._tick(car) + assert car.controller.brake_hold_active + + car.cs.out.gasPressed = True + car.cc.longActive = False + car.cc.actuators.accel = 0.0 + self._tick(car) + + car.cs.out.gasPressed = False + car.cc.longActive = True + car.cc.actuators.accel = -0.7 + for _ in range(10): + command = self._tick(car) + assert not car.controller.brake_hold_active + assert command["RELEASE_STANDSTILL"] == 1 + + car.cs.out.standstill = False + car.cs.out.vEgo = car.cs.out.vEgoRaw = 1.0 + car.cs.out.cruiseState.standstill = False + self._tick(car) + + car.cs.out.standstill = True + car.cs.out.vEgo = car.cs.out.vEgoRaw = 0.0 + car.cs.out.cruiseState.standstill = True + command = self._tick(car) + assert car.controller.brake_hold_active + assert command["ACCEL_CMD"] == -1.0 + assert command["RELEASE_STANDSTILL"] == 0 + + def test_manual_stop_still_holds_with_only_aol_active(self): + car = self._make_car() + car.cc.enabled = False + car.cc.longActive = False + car.cc.latActive = True + car.cc.actuators.accel = 0.0 + car.cs.out.cruiseState.enabled = False + car.cs.out.brakePressed = True + # Include the first ACC_CONTROL transmission after the one-second latch. + for _ in range(35): + command = self._tick(car) + assert car.controller.brake_hold_active + assert command["ACCEL_CMD"] == -1.0 + assert command["RELEASE_STANDSTILL"] == 0 + + car.cs.out.brakePressed = False + command = self._tick(car) + assert car.controller.brake_hold_active + assert command["ACCEL_CMD"] == -1.0 + + car.cs.out.gasPressed = True + command = self._tick(car) + assert not car.controller.brake_hold_active + assert command["ACCEL_CMD"] == 0.0 + assert command["RELEASE_STANDSTILL"] == 1 + + @pytest.mark.parametrize("long_state", ["off", "pid", "starting"]) + def test_stationary_cruise_does_not_latch_without_a_stopping_request(self, long_state): + car = self._make_car() + car.cc.actuators.longControlState = getattr(structs.CarControl.Actuators.LongControlState, long_state) + car.cc.actuators.accel = 1.5 + self._tick(car) + assert not car.controller.brake_hold_active + + @pytest.mark.parametrize("enabled,capability,candidate", [ + (False, True, CAR.TOYOTA_RAV4_TSS2), + (True, False, CAR.TOYOTA_RAV4_TSS2), + (True, True, CAR.TOYOTA_CAMRY_TSS2), + ]) + def test_cruise_hold_requires_toggle_capability_and_acc_hold_path(self, enabled, capability, candidate): + car = self._make_car(candidate=candidate, enabled=enabled, capability=capability) + self._tick(car) + assert not car.controller.brake_hold_active + + car.cc.actuators.accel = 1.5 + car.cc.actuators.longControlState = structs.CarControl.Actuators.LongControlState.starting + car.cc.cruiseControl.resume = True + # Allow the ordinary acceleration rate limiter to ramp through zero. + for _ in range(20): + command = self._tick(car) + assert not car.controller.brake_hold_active + assert command["ACCEL_CMD"] > 0 + assert command["RELEASE_STANDSTILL"] == 1 + + @pytest.mark.parametrize("release", ["toggle", "cancel", "main", "park", "reverse", "moving", "long_inactive"]) + def test_cruise_hold_releases_when_conditions_change(self, release): + car = self._make_car() + self._tick(car) + assert car.controller.brake_hold_active + + if release == "toggle": + car.toggles.toyota_auto_hold = False + elif release == "cancel": + car.cc.cruiseControl.cancel = True + elif release == "main": + car.cs.out.cruiseState.available = False + elif release in ("park", "reverse"): + car.cs.out.gearShifter = getattr(structs.CarState.GearShifter, release) + elif release == "moving": + car.cs.out.standstill = False + else: + car.cc.longActive = False + + self._tick(car) + assert not car.controller.brake_hold_active + + class TestToyotaCarState: @pytest.mark.parametrize("candidate", [CAR.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_RETROFIT]) def test_legacy_prius_distance_button_generates_events(self, candidate): diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index d00a2be4a2..4955913ed1 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -502,6 +502,18 @@ class TestFordMachEExtendedCurvatureSafety(TestFordCANFDStockSafety): self._reset_curvature_measurement(0.02, 9.0) self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.02, 0.0))) + def test_mach_e_aol_keeps_curvature_without_path_angle_assist(self): + self.safety.set_alternative_experience(32) + self._rx(self._toggle_aol(True)) + self.assertTrue(self.safety.get_aol_allowed()) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertTrue(self._tx(self._extended_lka_msg())) + for sign in (-1, 1): + self._reset_curvature_measurement(sign * 0.02, 7.5) + self._set_prev_desired_angle(sign * 0.02) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, sign * 0.055, sign * 0.02, 0.0))) + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, sign * 0.02, 0.0))) + def test_other_canfd_fords_keep_original_error(self): self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD) self.safety.init_tests() diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index e47ac5e9b9..14f6ebcd8f 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -370,7 +370,10 @@ class Car: self.CP, self.CI.CS, self.sm['pandaStates'], self.sm.all_checks(['pandaStates']), ) self.CI.CS.preap_lateral_authorized = preap_authorized - FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles, preap_authorized=preap_authorized) + FPCS = self.starpilot_card.update( + CS, FPCS, self.sm, self.starpilot_toggles, preap_authorized=preap_authorized, + ev6_aol_authorized=getattr(self.CI.CS, 'ev6_aol_authorized', False), + ) return CS, RD, FPCS def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.StarPilotCarState): diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 27247ef3ff..6f5e1be119 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -38,8 +38,8 @@ MACH_E_TURN_IN_LOOKAHEAD_EXTRA = 0.80 MACH_E_LOW_SPEED_TURN_IN_LOOKAHEAD_EXTRA = 1.60 MACH_E_LOW_SPEED_TURN_IN_START_SPEED = 2.0 MACH_E_LOW_SPEED_TURN_IN_FULL_SPEED = 3.0 -MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED = 11.0 -MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED = 14.0 +MACH_E_LOW_SPEED_TURN_IN_MAX_SPEED = 12.0 +MACH_E_LOW_SPEED_TURN_IN_FADE_SPEED = 15.0 MACH_E_TURN_IN_MIN_CURVATURE = 0.002 MACH_E_TURN_IN_FULL_CURVATURE = 0.008 MACH_E_TURN_IN_LAG_CURVATURE = 0.006 @@ -491,8 +491,12 @@ class FordLateralController: self.manual_turn_direction = 0.0 return False - if (CS.out.steeringPressed or blinker_direction != 0.0 or - abs(CS.out.steeringAngleDeg) > MANUAL_TURN_RELEASE_ANGLE_DEG): + following_next_curve = ( + driver_assisting and not CS.out.leftBlinker and not CS.out.rightBlinker and + CS.out.vEgoRaw >= MACH_E_DIRECTION_CHANGE_MIN_SPEED and self.manual_turn_direction * desired < 0.0 + ) + if not following_next_curve and (CS.out.steeringPressed or blinker_direction != 0.0 or + abs(CS.out.steeringAngleDeg) > MANUAL_TURN_RELEASE_ANGLE_DEG): self.manual_turn_recovery_timer = 0.0 else: self.manual_turn_recovery_timer += STEER_DT @@ -604,6 +608,9 @@ class FordLateralController: applied = float(np.clip(applied, -max_curvature, max_curvature)) path_angle = self._path_angle_assist( requested, desired, applied, current, v_ego, driver_override, self._lane_change()[0]) + if path_angle != 0.0 and (not CC.enabled or CS.out.gasPressed or CS.out.brakePressed): + self.path_angle_last = 0.0 + path_angle = 0.0 self.curvature_samples.append(predicted) curvature_rate = 0.0 diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index d107bd0878..51bbfec195 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -33,7 +33,7 @@ def controller(monkeypatch): def car_state(speed=15.0, accel=0.0, curvature=0.0, steering_pressed=False, steering_angle=0.0, - steering_torque=0.0, left_blinker=False, right_blinker=False): + steering_torque=0.0, left_blinker=False, right_blinker=False, gas_pressed=False, brake_pressed=False): return SimpleNamespace(out=SimpleNamespace( vEgoRaw=speed, aEgo=accel, @@ -43,6 +43,8 @@ def car_state(speed=15.0, accel=0.0, curvature=0.0, steering_pressed=False, stee steeringTorque=steering_torque, leftBlinker=left_blinker, rightBlinker=right_blinker, + gasPressed=gas_pressed, + brakePressed=brake_pressed, )) @@ -337,6 +339,37 @@ def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller): assert encoded_angle == pytest.approx(-assist) +@pytest.mark.parametrize("sign", (-1, 1)) +@pytest.mark.parametrize("enabled,gas_pressed,brake_pressed", ( + (False, False, False), + (False, True, False), + (True, True, False), + (True, False, True), +)) +def test_mach_e_assist_falls_back_to_curvature_when_not_permitted( + controller, monkeypatch, sign, enabled, gas_pressed, brake_pressed): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.curvature_last = sign * 0.02 + controller.path_angle_last = sign * 0.16 + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.04) + state = car_state(speed=7.0, curvature=sign * 0.007, + gas_pressed=gas_pressed, brake_pressed=brake_pressed) + actuators = SimpleNamespace(curvature=sign * 0.03) + CC = SimpleNamespace(latActive=True, enabled=enabled) + for _ in range(10): + result = controller.update(CC, state, actuators) + assert result.active + assert result.curvature == pytest.approx(sign * 0.02) + assert result.path_angle == controller.path_angle_last == 0.0 + + CC.enabled = True + state.out.gasPressed = state.out.brakePressed = False + result = controller.update(CC, state, actuators) + assert result.curvature == pytest.approx(sign * 0.02) + assert result.path_angle == pytest.approx(sign * 0.055) + + @pytest.mark.parametrize("sign", (-1, 1)) @pytest.mark.parametrize("speed,expected", ((1.9, False), (2.0, True), (3.0, True), (7.0, True), (11.0, True), (14.9, True), (15.0, False))) @@ -387,7 +420,7 @@ def test_mach_e_driver_assistance_handoff_and_takeover(controller, monkeypatch, controller.CP.flags = FordFlags.CANFD controller.curvature_last = sign * 0.020 monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.030) - CC = SimpleNamespace(latActive=True) + CC = SimpleNamespace(latActive=True, enabled=True) actuators = SimpleNamespace(curvature=sign * 0.022) helping = car_state(speed=7.0, curvature=sign * 0.008, steering_pressed=True, steering_angle=-sign * 50.0, steering_torque=-sign * 2.0, @@ -441,7 +474,7 @@ def test_mach_e_driver_help_at_early_curve_entry(controller, monkeypatch, sign): state = car_state(speed=2.7, curvature=sign * 0.003, steering_pressed=True, steering_torque=-sign * 2.0, steering_angle=-sign * 15.0, left_blinker=sign < 0, right_blinker=sign > 0) - CC = SimpleNamespace(latActive=True) + CC = SimpleNamespace(latActive=True, enabled=True) assert controller.update(CC, state, SimpleNamespace(curvature=sign * 0.004)).active state.out.vEgoRaw = 3.5 state.out.yawRate = -sign * 0.004 * state.out.vEgoRaw @@ -683,10 +716,11 @@ def test_mach_e_turn_in_preview_is_not_carried_into_unwind(controller): (9.0, 1.60), (10.5, 1.60), (11.0, 1.60), - (12.0, 4.0 / 3.0), - (13.0, 16.0 / 15.0), - (14.0, 0.80), + (12.0, 1.60), + (13.0, 4.0 / 3.0), + (14.0, 16.0 / 15.0), (15.0, 0.80), + (16.0, 0.80), )) def test_mach_e_turn_in_lookahead_extra_fades_by_speed(controller, speed, expected): assert controller._turn_in_lookahead_extra(speed) == pytest.approx(expected) @@ -1334,6 +1368,90 @@ def test_mach_e_manual_turn_releases_for_opposite_path_request(controller): assert controller.update(CC, car_state(curvature=-0.001), CC.actuators).active +@pytest.mark.parametrize("sign", (-1.0, 1.0)) +@pytest.mark.parametrize("speed", (9.0, 9.5, 14.99)) +def test_mach_e_manual_turn_hands_off_to_driver_assisted_opposite_curve(controller, monkeypatch, sign, speed): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + CC = SimpleNamespace(latActive=True, enabled=True) + actuators = SimpleNamespace(curvature=sign * 0.006) + turning = car_state(speed=speed, curvature=sign * 0.004, steering_pressed=True, + steering_angle=-sign * 30.0, steering_torque=-sign * 2.0, + left_blinker=sign < 0.0, right_blinker=sign > 0.0) + assert not controller.update(CC, turning, actuators).active + assert controller.manual_turn_direction == sign + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: -sign * 0.010) + actuators.curvature = -sign * 0.006 + following = car_state(speed=speed, curvature=-sign * 0.004, steering_pressed=True, + steering_angle=sign * 20.0, steering_torque=sign * 2.0) + for _ in range(4): + assert not controller.update(CC, following, actuators).active + result = controller.update(CC, following, actuators) + assert result.active + assert -sign * result.curvature > 0.0 + assert abs(result.curvature) <= 0.0025 + assert result.path_angle == 0.0 + assert not controller.manual_turn_latched + assert controller.manual_turn_direction == 0.0 + assert controller.manual_turn_recovery_timer == 0.0 + + +@pytest.mark.parametrize("sign", (-1.0, 1.0)) +@pytest.mark.parametrize("speed,current,torque,preview,left,right,lane_change", ( + (4.5, -0.004, 2.0, -0.010, False, False, False), + (8.99, -0.004, 2.0, -0.010, False, False, False), + (15.0, -0.004, 2.0, -0.010, False, False, False), + (9.5, 0.004, 2.0, -0.010, False, False, False), + (9.5, -0.009, 2.0, -0.010, False, False, False), + (9.5, -0.004, -2.0, -0.010, False, False, False), + (9.5, -0.004, 3.6, -0.010, False, False, False), + (9.5, -0.004, 2.0, 0.010, False, False, False), + (9.5, -0.004, 2.0, -0.007, False, False, False), + (9.5, -0.004, 2.0, -0.010, True, False, False), + (9.5, -0.004, 2.0, -0.010, False, True, False), + (9.5, -0.004, 2.0, -0.010, True, True, False), + (9.5, -0.004, 2.0, -0.010, False, False, True), +)) +def test_mach_e_opposite_curve_handoff_preserves_manual_override( + controller, monkeypatch, sign, speed, current, torque, preview, left, right, lane_change): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.manual_turn_latched = True + controller.manual_turn_direction = sign + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * preview) + monkeypatch.setattr(controller, "_lane_change", lambda: (lane_change, 0)) + state = car_state(speed=speed, curvature=sign * current, steering_pressed=True, + steering_angle=sign * 25.0, steering_torque=sign * torque, + left_blinker=left, right_blinker=right) + for _ in range(8): + result = controller.update(SimpleNamespace(latActive=True, enabled=True), state, + SimpleNamespace(curvature=-sign * 0.006)) + assert not result.active + assert result.curvature == result.path_angle == 0.0 + assert controller.manual_turn_recovery_timer == 0.0 + + +def test_mach_e_opposite_curve_handoff_requires_uninterrupted_agreement(controller, monkeypatch): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + controller.manual_turn_latched = True + controller.manual_turn_direction = 1.0 + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: -0.010) + CC = SimpleNamespace(latActive=True, enabled=True) + actuators = SimpleNamespace(curvature=-0.006) + state = car_state(speed=9.5, curvature=-0.004, steering_pressed=True, + steering_angle=20.0, steering_torque=2.0) + for _ in range(4): + assert not controller.update(CC, state, actuators).active + state.out.steeringTorque = -2.0 + assert not controller.update(CC, state, actuators).active + assert controller.manual_turn_recovery_timer == 0.0 + state.out.steeringTorque = 2.0 + for _ in range(4): + assert not controller.update(CC, state, actuators).active + assert controller.update(CC, state, actuators).active + + def test_non_mach_e_signaled_turn_does_not_latch(controller): CC = SimpleNamespace(latActive=True) result = controller.update(CC, car_state( diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 76dd22ac37..135d9decc7 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -3652,8 +3652,8 @@ { "key": "ToyotaAutoHold", "label": "Toyota Auto Hold", - "description": "Hold the brakes at a stop on supported Toyota/Lexus vehicles when cruise main is available and cruise is not active.", - "picker_description": "Holds supported Toyota/Lexus brakes at stops when cruise is available.", + "description": "Hold the brakes after a manual stop with cruise main on. Supported models can also hold after a cruise-controlled stop until you press the gas pedal.", + "picker_description": "Holds supported Toyota/Lexus brakes at stops until you press the gas pedal.", "data_type": "bool", "ui_type": "toggle", "galaxy_only": true, diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index a05490f675..6a2a4903ac 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -73,6 +73,11 @@ class StarPilotCard: self.CP.brand == "hyundai" and not (hyundai_flags & HyundaiFlags.CANFD) and not hyundai_aol_before_engagement ) self.hyundai_aol_ready = False + self.ev6_aol_needs_arming = ( + getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.KIA_EV6 and + getattr(self.CP, "openpilotLongitudinalControl", False) and not getattr(self.CP, "pcmCruise", False) + ) + self.ev6_aol_authorized = False self.g70_main_cruise_aol_pending = False self.g70_main_cruise_aol_pending_frames = 0 self.prev_cruise_available = None @@ -167,6 +172,8 @@ class StarPilotCard: def _toggle_controller_aol(self, carState, starpilot_toggles, main_cruise_aol=False): if not self.always_on_lateral_supported or not getattr(starpilot_toggles, "always_on_lateral", False): return False + if self.ev6_aol_needs_arming and not self.ev6_aol_authorized: + return False tesla_disengage_on_brake = ( self.CP.brand == "tesla" and getattr(starpilot_toggles, "tesla_aol_disengage_on_brake", False) @@ -237,7 +244,8 @@ class StarPilotCard: else: self.params.put_bool_nonblocking("ExperimentalMode", not sm["selfdriveState"].experimentalMode) - def update(self, carState, starpilotCarState, sm, starpilot_toggles, *, preap_authorized=False): + def update(self, carState, starpilotCarState, sm, starpilot_toggles, *, preap_authorized=False, ev6_aol_authorized=False): + self.ev6_aol_authorized = ev6_aol_authorized self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled") self._handle_favorite_traffic_mode_action(sm) @@ -487,6 +495,9 @@ class StarPilotCard: self._handle_controller_actions(carState, sm, starpilot_toggles, main_cruise_aol) + if self.ev6_aol_needs_arming and not ev6_aol_authorized: + self.always_on_lateral_allowed = False + self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set if getattr(self.CP, "carFingerprint", None) == "TESLA_MODEL_S_PREAP": self.always_on_lateral_enabled &= preap_authorized diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 25ee4db5d0..49c90e0e16 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -295,6 +295,77 @@ def make_wrapped_button_event(button_type, pressed): return SimpleNamespace(type=SimpleNamespace(raw=int(button_type)), pressed=pressed) +@pytest.mark.parametrize("fingerprint", tuple(spc.HYUNDAI_CAR)) +@pytest.mark.parametrize("openpilot_long, pcm_cruise", ((True, False), (False, True), (True, True))) +def test_ev6_arming_gate_is_limited_to_ev6_openpilot_long(monkeypatch, tmp_path, fingerprint, openpilot_long, pcm_cruise): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + card = spc.StarPilotCard( + SimpleNamespace(brand="hyundai", carFingerprint=fingerprint, flags=spc.HyundaiFlags.CANFD, + openpilotLongitudinalControl=openpilot_long, pcmCruise=pcm_cruise), + SimpleNamespace(alternativeExperience=32), + ) + needs_arming = fingerprint == spc.HYUNDAI_CAR.KIA_EV6 and openpilot_long and not pcm_cruise + assert card.ev6_aol_needs_arming == needs_arming + toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True) + ret = card.update(make_car_state(available=True), SimpleNamespace(distancePressed=False), make_sm(), toggles) + assert ret.alwaysOnLateralEnabled == (card.always_on_lateral_supported and not needs_arming) + + +@pytest.mark.parametrize("lkas_mapping", (False, True)) +def test_ev6_aol_requires_physical_authorization_even_for_controller_actions(monkeypatch, tmp_path, lkas_mapping): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + card = spc.StarPilotCard( + SimpleNamespace(brand="hyundai", carFingerprint=spc.HYUNDAI_CAR.KIA_EV6, flags=spc.HyundaiFlags.CANFD, + openpilotLongitudinalControl=True, pcmCruise=False), + SimpleNamespace(alternativeExperience=32), + ) + toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=not lkas_mapping, + always_on_lateral_lkas=lkas_mapping) + sm = make_sm() + output = SimpleNamespace(distancePressed=False) + cs = make_car_state(available=True) + ret = card.update(cs, output, sm, toggles) + assert not ret.alwaysOnLateralAllowed + assert not ret.alwaysOnLateralEnabled + + counter = spc.CONTROLLER_ACTION_COUNTERS[spc.CONTROLLER_ACTION_TOGGLE_AOL] + card.params_memory.put_int(counter, 1) + ret = card.update(cs, output, sm, toggles) + assert not ret.alwaysOnLateralAllowed + assert not ret.alwaysOnLateralEnabled + + cs.buttonEvents = [make_wrapped_button_event(spc.ButtonType.lkas, True)] + ret = card.update(cs, output, sm, toggles, ev6_aol_authorized=True) + assert ret.alwaysOnLateralEnabled + cs.buttonEvents = [] + ret = card.update(cs, output, sm, toggles, ev6_aol_authorized=True) + assert ret.alwaysOnLateralEnabled + + ret = card.update(cs, output, sm, toggles, ev6_aol_authorized=False) + assert not ret.alwaysOnLateralAllowed + assert not ret.alwaysOnLateralEnabled + + +def test_ev6_lkas_experimental_mapping_still_runs_when_armed(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + card = spc.StarPilotCard( + SimpleNamespace(brand="hyundai", carFingerprint=spc.HYUNDAI_CAR.KIA_EV6, flags=spc.HyundaiFlags.CANFD, + openpilotLongitudinalControl=True, pcmCruise=False), + SimpleNamespace(alternativeExperience=32), + ) + toggles = make_toggles(always_on_lateral=True, always_on_lateral_main=True, experimental_mode_via_lkas=True, + experimental_mode_available=True) + sm = make_sm() + sm["carControl"].latActive = True + cs = make_car_state(available=True, button_events=[make_wrapped_button_event(spc.ButtonType.lkas, True)]) + ret = card.update(cs, SimpleNamespace(distancePressed=False), sm, toggles, ev6_aol_authorized=True) + assert ret.alwaysOnLateralEnabled + assert card.params.get_bool("ExperimentalMode") + + @pytest.mark.parametrize("pending_before_press", [False, True]) def test_slc_confirmation_release_does_not_republish_accel(monkeypatch, tmp_path, pending_before_press): monkeypatch.setattr(spc, "Params", FakeParams)