Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion docs-nap/engagement.md
Original file line number Diff line number Diff line change
Expand Up @@ -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. 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".
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 `enableLongControl` edges, not interceptor authority, so a gas override or pedal-up does not play "Pedal Cruise Disengaged" / "Pedal Cruise Engaged". Brake and first-pull drop `enableLongControl` while cruise stays and do chime.
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.
Expand Down
1 change: 1 addition & 0 deletions selfdrive/car/tests/test_car_specific_tesla_preap.py
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,7 @@ def test_pedal_acquisition_failure_keeps_lateral_and_surfaces_alert():

public_state = interface.update(drive_packet)
assert public_state.cruiseState.enabled
assert not public_state.enableLongControl
assert not public_state.pedalLongActive
assert public_state.pedalAuthorityFailed

Expand Down
25 changes: 14 additions & 11 deletions selfdrive/selfdrived/preap_regen.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,10 @@
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.
pedal_long_chime binds engage/disengage to enableLongControl edges.
Gas override drops interceptor authority (pedalLongActive) but keeps
enableLongControl, so a pedal press/release must not chime. Brake and
first-pull drop enableLongControl while cruise stays, and those must.
"""
import math

Expand All @@ -22,15 +23,17 @@
REGEN_DEMAND_MIN_SPEED = 2.0 # m/s; do not prompt for a stopped/settling car
REGEN_DEMAND_CLEAR_SPEED = 1.0 # m/s

PEDAL_CHIME_ENABLED = "enabled"
PEDAL_CHIME_DISABLED = "disabled"

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

def pedal_long_chime(*, enable_long_control: bool, prev_enable_long_control: bool) -> str | None:
"""Engage/disengage chime follows the FSM long-control flag, not authority."""
if enable_long_control and not prev_enable_long_control:
return PEDAL_CHIME_ENABLED
if prev_enable_long_control and not enable_long_control:
return PEDAL_CHIME_DISABLED
return None


class RegenDemandCheck:
Expand Down
31 changes: 19 additions & 12 deletions selfdrive/selfdrived/selfdrived.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,12 @@
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, update_pedal_cruise_session
from openpilot.selfdrive.selfdrived.preap_regen import (
PEDAL_CHIME_DISABLED,
PEDAL_CHIME_ENABLED,
RegenDemandCheck,
pedal_long_chime,
)
from openpilot.selfdrive.selfdrived.state import StateMachine
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert

Expand Down Expand Up @@ -123,7 +128,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_cruise_session = False
self.prev_enable_long_control = False
self.preap_regen_demand = RegenDemandCheck()

# Determine startup event
Expand Down Expand Up @@ -191,23 +196,25 @@ def update_events(self, CS):
self.events.add_from_msg(car_events)

# Tesla Pre-AP pedal-long status alerts (banner + engage/disengage sounds).
# Chime on the driver's cruise session, not interceptor authority:
# gas override drops pedalLongActive while cruiseState.enabled stays true.
# Chime on enableLongControl edges, not interceptor authority or the
# long-lived cruise session. Gas override drops pedalLongActive while
# enableLongControl stays true; brake / first-pull drop long and keep
# cruise. Do not treat pedal-up (authority re-accepted) as engage.
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))
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,
enable_long_control = bool(getattr(CS, 'enableLongControl', False))
chime = pedal_long_chime(
enable_long_control=enable_long_control,
prev_enable_long_control=self.prev_enable_long_control,
)
if pedal_cruise_session and not self.prev_pedal_cruise_session:
if chime == PEDAL_CHIME_ENABLED:
self.events.add(EventName.pedalCruiseEnabled)
elif self.prev_pedal_cruise_session and not pedal_cruise_session:
elif chime == PEDAL_CHIME_DISABLED:
self.events.add(EventName.pedalCruiseDisabled)
self.prev_pedal_cruise_session = pedal_cruise_session
self.prev_enable_long_control = enable_long_control

# Two shapes of "regen is not enough, add friction brake": the carstate
# flag covers weak regen under-delivering an in-envelope request; the
Expand All @@ -222,7 +229,7 @@ def update_events(self, CS):
if getattr(CS, 'pedalMaxRegen', False) or regen_demand_overflow:
self.events.add(EventName.pedalMaxRegen)
else:
self.prev_pedal_cruise_session = False
self.prev_enable_long_control = False

if self.CP.notCar:
# wait for everything to init first
Expand Down
58 changes: 38 additions & 20 deletions selfdrive/selfdrived/tests/test_preap_regen.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
from openpilot.selfdrive.selfdrived.preap_regen import (
PEDAL_CHIME_DISABLED,
PEDAL_CHIME_ENABLED,
REGEN_DEMAND_EVIDENCE_COUNT,
RegenDemandCheck,
update_pedal_cruise_session,
pedal_long_chime,
)

# get_preap_accel_limits floor is -1.5 m/s²; -2.0 clears the trigger margin.
Expand Down Expand Up @@ -84,28 +86,44 @@ def test_demand_prompt_resets_when_pedal_long_inactive():
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 _chimes(enable_long_frames):
prev = False
events = []
for enable_long in enable_long_frames:
chime = pedal_long_chime(
enable_long_control=enable_long,
prev_enable_long_control=prev,
)
if chime is not None:
events.append(chime)
prev = enable_long
return events


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_brake_or_single_pull_to_lat_only_fires_disengage():
# enableLongControl 1→0 while cruise stays (brake or first-pull).
# Interceptor authority also drops, but the chime is the FSM edge.
assert _chimes([True, False]) == [PEDAL_CHIME_ENABLED, PEDAL_CHIME_DISABLED]


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_double_pull_long_on_fires_engage():
# Lat-only (cruise on, long off) then double-pull: enableLongControl 0→1.
assert _chimes([False, True]) == [PEDAL_CHIME_ENABLED]


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,
)
def test_pedal_override_does_not_use_pedal_up_as_engage():
# Gas override drops pedalLongActive while enableLongControl stays true.
# Pedal-up re-accepts authority on the same still-true flag — not a 0→1.
assert pedal_long_chime(enable_long_control=True, prev_enable_long_control=True) is None
assert _chimes([True, True, True]) == [PEDAL_CHIME_ENABLED]


def test_full_off_then_on_still_chimes():
assert _chimes([True, False, True]) == [
PEDAL_CHIME_ENABLED, PEDAL_CHIME_DISABLED, PEDAL_CHIME_ENABLED,
]


def test_authority_drop_is_not_a_disengage_when_long_stays_on():
# The 55704413 failure mode: chiming pedalLongActive 1→0. Long stays on.
assert pedal_long_chime(enable_long_control=True, prev_enable_long_control=True) is None
Loading