diff --git a/docs-nap/engagement.md b/docs-nap/engagement.md index 8b919b9858adf6..e43e42f0c80e1c 100644 --- a/docs-nap/engagement.md +++ b/docs-nap/engagement.md @@ -18,7 +18,7 @@ Pre-AP has two modes depending on whether a Comma Pedal is installed. Mode is se 2. `carcontroller.py` sends `GAS_COMMAND` (0x551) based on `actuators.accel` from the long planner 3. Zero-torque learning tracks the resting pedal position that produces zero torque at the current speed. On engage, we seed `prev_pedal_di` to this value so there is no regen spike 4. Brake rising edge → drops `enableLongControl=False`, keeps `cruiseEnabled=True` (steering stays on, pedal drops). `enableJustCC` flips true so the carcontroller knows to spoof stock CC cancel -5. Gas press → `OVERRIDE_LONGITUDINAL`. Pedal command passes through with `enable=0` — driver's foot controls throttle directly, NAP tracks position for smooth resume +5. Gas press → `OVERRIDE_LONGITUDINAL`. Pedal command passes through with `enable=0` — driver's foot controls throttle directly, NAP tracks position for smooth resume. Pedal cruise engage/disengage chimes follow the cruise session, not interceptor authority, so a gas override does not play "Pedal Cruise Disengaged" / "Pedal Cruise Engaged". 6. Stalk cancel (real, not spoof echo) → full disengage Interface flags for this mode: `openpilotLongitudinalControl=True`, `pcmCruise=False`. Long planner runs; accel goes to pedal. diff --git a/opendbc_repo b/opendbc_repo index 604b9c497c2c51..3c4da443534de3 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 604b9c497c2c51573c67379cd1cbbe20d2d3f9e2 +Subproject commit 3c4da443534de39ba019cc5a00d39afecb80d3f1 diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index b41f7e59f2ff62..3f45bf3f87d53e 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -1041,7 +1041,7 @@ def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messagin "Regen Limit Reached", "Press Brake to Slow Down", AlertStatus.userPrompt, AlertSize.mid, - Priority.HIGH, VisualAlert.brakePressed, AudibleAlert.prompt, .2), + Priority.HIGH, VisualAlert.brakePressed, AudibleAlert.promptRepeat, .2), }, EventName.teslaCCEngaged: { diff --git a/selfdrive/selfdrived/preap_regen.py b/selfdrive/selfdrived/preap_regen.py index 973711b2d489c3..6d03e952ba9fb7 100644 --- a/selfdrive/selfdrived/preap_regen.py +++ b/selfdrive/selfdrived/preap_regen.py @@ -1,12 +1,13 @@ -"""Pre-AP pedal-long regen demand check. - -The planner's deceleration request is clamped to the regen envelope -(get_preap_accel_limits) before the car ever sees it, so a demand the envelope -cannot cover is invisible to the pedal controller. This check reads the -unclamped plan and prompts the driver to add friction brake when the demand -sits meaningfully below the envelope floor. The carstate pedalMaxRegen flag -covers the other failure shape: a request inside the envelope that weak -battery regen fails to deliver. +"""Pre-AP pedal-long helpers used by selfdrived. + +RegenDemandCheck reads the unclamped plan and prompts when planned +deceleration sits below the regen envelope floor. The carstate +pedalMaxRegen flag covers the other failure shape: a request inside the +envelope that weak battery regen fails to deliver. + +update_pedal_cruise_session keeps the engage/disengage chime on the +driver's cruise session, not interceptor authority. Gas override drops +pedalLongActive while cruiseState.enabled stays true. """ import math @@ -15,13 +16,23 @@ # Evidence accumulates in a saturating up/down counter so a single MPC sample # cannot flash a driver prompt, while brief dropouts do not restart the clock. -REGEN_DEMAND_EVIDENCE_COUNT = int(0.3 / DT_CTRL) -REGEN_DEMAND_TRIGGER_MARGIN = 0.2 # m/s² below the envelope floor -REGEN_DEMAND_CLEAR_MARGIN = 0.05 # m/s² +REGEN_DEMAND_EVIDENCE_COUNT = int(0.2 / DT_CTRL) +REGEN_DEMAND_TRIGGER_MARGIN = 0.1 # m/s² below the envelope floor +REGEN_DEMAND_CLEAR_MARGIN = 0.03 # m/s² REGEN_DEMAND_MIN_SPEED = 2.0 # m/s; do not prompt for a stopped/settling car REGEN_DEMAND_CLEAR_SPEED = 1.0 # m/s +def update_pedal_cruise_session(*, cruise_enabled: bool, pedal_long_active: bool, + prev_session: bool) -> bool: + """True while this pedal-cruise engagement is still the driver's session.""" + if pedal_long_active: + return True + if not cruise_enabled: + return False + return prev_session + + class RegenDemandCheck: """Prompt when planned deceleration exceeds what the regen envelope allows.""" diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index e043ac84e5773d..dfc37e4416d045 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -18,7 +18,7 @@ from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.selfdrive.selfdrived.events import Events, ET from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck -from openpilot.selfdrive.selfdrived.preap_regen import RegenDemandCheck +from openpilot.selfdrive.selfdrived.preap_regen import RegenDemandCheck, update_pedal_cruise_session from openpilot.selfdrive.selfdrived.state import StateMachine from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert @@ -123,7 +123,7 @@ def __init__(self, CP=None): self.recalibrating_seen = False self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) - self.prev_pedal_long_active = False + self.prev_pedal_cruise_session = False self.preap_regen_demand = RegenDemandCheck() # Determine startup event @@ -191,19 +191,23 @@ def update_events(self, CS): self.events.add_from_msg(car_events) # Tesla Pre-AP pedal-long status alerts (banner + engage/disengage sounds). - # Track pedal-long mode via the explicit pedalLongActive flag from carstate, - # not cruiseState.speed (which reads DI_digitalSpeed when long is off and - # would wrongly trigger on lateral-only engagement while driving). + # Chime on the driver's cruise session, not interceptor authority: + # gas override drops pedalLongActive while cruiseState.enabled stays true. if (self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise): pedal_long_active = bool(CS.cruiseState.enabled and getattr(CS, 'pedalLongActive', False)) - if pedal_long_active and not self.prev_pedal_long_active: + pedal_cruise_session = update_pedal_cruise_session( + cruise_enabled=bool(CS.cruiseState.enabled), + pedal_long_active=pedal_long_active, + prev_session=self.prev_pedal_cruise_session, + ) + if pedal_cruise_session and not self.prev_pedal_cruise_session: self.events.add(EventName.pedalCruiseEnabled) - elif self.prev_pedal_long_active and not pedal_long_active: + elif self.prev_pedal_cruise_session and not pedal_cruise_session: self.events.add(EventName.pedalCruiseDisabled) - self.prev_pedal_long_active = pedal_long_active + self.prev_pedal_cruise_session = pedal_cruise_session # Two shapes of "regen is not enough, add friction brake": the carstate # flag covers weak regen under-delivering an in-envelope request; the @@ -218,7 +222,7 @@ def update_events(self, CS): if getattr(CS, 'pedalMaxRegen', False) or regen_demand_overflow: self.events.add(EventName.pedalMaxRegen) else: - self.prev_pedal_long_active = False + self.prev_pedal_cruise_session = False if self.CP.notCar: # wait for everything to init first diff --git a/selfdrive/selfdrived/tests/test_alerts.py b/selfdrive/selfdrived/tests/test_alerts.py index edb298993e07aa..cdefcaf3385121 100644 --- a/selfdrive/selfdrived/tests/test_alerts.py +++ b/selfdrive/selfdrived/tests/test_alerts.py @@ -9,7 +9,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.realtime import DT_CTRL -from openpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET +from openpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET, AudibleAlert from openpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS @@ -112,6 +112,7 @@ def test_preap_regen_alert_tells_driver_to_brake(self): assert alert.alert_text_2 == "Press Brake to Slow Down" assert alert.alert_size == AlertSize.mid assert alert.visual_alert == car.CarControl.HUDControl.VisualAlert.brakePressed + assert alert.audible_alert == AudibleAlert.promptRepeat assert alert.duration == int(0.2 / DT_CTRL) def test_preap_pedal_unavailable_alert_is_visible_without_disabling_lateral(self): diff --git a/selfdrive/selfdrived/tests/test_preap_regen.py b/selfdrive/selfdrived/tests/test_preap_regen.py index 9778c41fcbcb70..92bc77b2a354f2 100644 --- a/selfdrive/selfdrived/tests/test_preap_regen.py +++ b/selfdrive/selfdrived/tests/test_preap_regen.py @@ -1,6 +1,7 @@ from openpilot.selfdrive.selfdrived.preap_regen import ( REGEN_DEMAND_EVIDENCE_COUNT, RegenDemandCheck, + update_pedal_cruise_session, ) # get_preap_accel_limits floor is -1.5 m/s²; -2.0 clears the trigger margin. @@ -36,7 +37,8 @@ def test_demand_prompt_survives_single_sample_dropouts(): for _ in range(3 * REGEN_DEMAND_EVIDENCE_COUNT): for _ in range(9): fired = _update(check) or fired - fired = _update(check, a_target=-1.6) or fired + # -1.55 is inside the 0.10 trigger margin, outside the 0.03 clear margin. + fired = _update(check, a_target=-1.55) or fired if fired: break assert fired @@ -65,7 +67,7 @@ def test_demand_prompt_uses_hysteresis_before_clearing(): assert check.active # Back inside the trigger margin but still beyond the clear margin. - assert _update(check, a_target=-1.6, v_ego=1.5) + assert _update(check, a_target=-1.55, v_ego=1.5) # Demand returns to the envelope: prompt clears. assert not _update(check, a_target=-1.5) @@ -80,3 +82,30 @@ def test_demand_prompt_resets_when_pedal_long_inactive(): assert not _update(check, pedal_long_active=False) assert not check.active + + +def test_pedal_cruise_session_starts_when_authority_is_accepted(): + assert update_pedal_cruise_session( + cruise_enabled=True, pedal_long_active=True, prev_session=False, + ) + + +def test_pedal_cruise_session_survives_gas_override(): + # Gas override drops pedalLongActive while cruise stays enabled. + assert update_pedal_cruise_session( + cruise_enabled=True, pedal_long_active=False, prev_session=True, + ) + + +def test_pedal_cruise_session_ends_on_cancel(): + assert not update_pedal_cruise_session( + cruise_enabled=False, pedal_long_active=False, prev_session=True, + ) + + +def test_pedal_cruise_session_ignores_lateral_only_gas(): + # Lateral-only engagement never accepted pedal authority, so a gas press + # must not look like a pedal-cruise session. + assert not update_pedal_cruise_session( + cruise_enabled=True, pedal_long_active=False, prev_session=False, + )