Skip to content
Merged
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
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.
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/selfdrived/events.py
Original file line number Diff line number Diff line change
Expand Up @@ -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: {
Expand Down
35 changes: 23 additions & 12 deletions selfdrive/selfdrived/preap_regen.py
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -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."""

Expand Down
22 changes: 13 additions & 9 deletions selfdrive/selfdrived/selfdrived.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
3 changes: 2 additions & 1 deletion selfdrive/selfdrived/tests/test_alerts.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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):
Expand Down
33 changes: 31 additions & 2 deletions selfdrive/selfdrived/tests/test_preap_regen.py
Original file line number Diff line number Diff line change
@@ -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.
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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,
)
Loading