diff --git a/opendbc_repo b/opendbc_repo index 2282f562ec73b0..6840f8f22e209f 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 2282f562ec73b0513661db0f622251b5083098ad +Subproject commit 6840f8f22e209f20a6093ac705de7b805456185d diff --git a/selfdrive/selfdrived/preap_regen.py b/selfdrive/selfdrived/preap_regen.py new file mode 100644 index 00000000000000..973711b2d489c3 --- /dev/null +++ b/selfdrive/selfdrived/preap_regen.py @@ -0,0 +1,62 @@ +"""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. +""" +import math + +from openpilot.common.realtime import DT_CTRL +from opendbc.car.tesla.preap.interface import get_preap_accel_limits + +# 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_MIN_SPEED = 2.0 # m/s; do not prompt for a stopped/settling car +REGEN_DEMAND_CLEAR_SPEED = 1.0 # m/s + + +class RegenDemandCheck: + """Prompt when planned deceleration exceeds what the regen envelope allows.""" + + def __init__(self): + self.active = False + self.evidence_updates = 0 + + def reset(self): + self.active = False + self.evidence_updates = 0 + + def update(self, *, pedal_long_active: bool, brake_pressed: bool, + a_target: float, v_ego: float) -> bool: + if not pedal_long_active or brake_pressed or not math.isfinite(a_target): + self.reset() + return False + + accel_floor, _ = get_preap_accel_limits(v_ego) + + if self.active: + keep_prompting = ( + v_ego > REGEN_DEMAND_CLEAR_SPEED + and a_target <= accel_floor - REGEN_DEMAND_CLEAR_MARGIN + ) + if not keep_prompting: + self.reset() + return self.active + + demanding = ( + v_ego >= REGEN_DEMAND_MIN_SPEED + and a_target <= accel_floor - REGEN_DEMAND_TRIGGER_MARGIN + ) + if demanding: + self.evidence_updates = min(self.evidence_updates + 1, REGEN_DEMAND_EVIDENCE_COUNT) + else: + self.evidence_updates = max(self.evidence_updates - 1, 0) + self.active = self.evidence_updates >= REGEN_DEMAND_EVIDENCE_COUNT + return self.active diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 2b541b09fb1b3c..2d35ad8b307485 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -18,6 +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.state import StateMachine from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert @@ -122,6 +123,7 @@ def __init__(self, CP=None): self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) self.prev_pedal_long_active = False + self.preap_regen_demand = RegenDemandCheck() # Determine startup event self.startup_event = EventName.startup if build_metadata.openpilot.comma_remote and build_metadata.tested_channel else EventName.startupMaster @@ -202,8 +204,17 @@ def update_events(self, CS): self.events.add(EventName.pedalCruiseDisabled) self.prev_pedal_long_active = pedal_long_active - # Sustained regen under-delivery: the driver needs to add friction brake. - if getattr(CS, 'pedalMaxRegen', False): + # Two shapes of "regen is not enough, add friction brake": the carstate + # flag covers weak regen under-delivering an in-envelope request; the + # demand check covers a planned deceleration the envelope cannot cover, + # which the clamped actuator request hides from the car entirely. + regen_demand_overflow = self.preap_regen_demand.update( + pedal_long_active=pedal_long_active, + brake_pressed=CS.brakePressed, + a_target=float(self.sm['longitudinalPlan'].aTarget), + v_ego=CS.vEgo, + ) + if getattr(CS, 'pedalMaxRegen', False) or regen_demand_overflow: self.events.add(EventName.pedalMaxRegen) else: self.prev_pedal_long_active = False diff --git a/selfdrive/selfdrived/tests/test_preap_regen.py b/selfdrive/selfdrived/tests/test_preap_regen.py new file mode 100644 index 00000000000000..9778c41fcbcb70 --- /dev/null +++ b/selfdrive/selfdrived/tests/test_preap_regen.py @@ -0,0 +1,82 @@ +from openpilot.selfdrive.selfdrived.preap_regen import ( + REGEN_DEMAND_EVIDENCE_COUNT, + RegenDemandCheck, +) + +# get_preap_accel_limits floor is -1.5 m/s²; -2.0 clears the trigger margin. +OVERFLOW_TARGET = -2.0 + + +def _update(check, *, a_target=OVERFLOW_TARGET, v_ego=15.0, + pedal_long_active=True, brake_pressed=False): + return check.update( + pedal_long_active=pedal_long_active, + brake_pressed=brake_pressed, + a_target=a_target, + v_ego=v_ego, + ) + + +def test_demand_prompt_requires_sustained_overflow(): + check = RegenDemandCheck() + for _ in range(REGEN_DEMAND_EVIDENCE_COUNT - 1): + assert not _update(check) + assert _update(check) + + +def test_demand_prompt_silent_when_plan_fits_envelope(): + check = RegenDemandCheck() + for _ in range(3 * REGEN_DEMAND_EVIDENCE_COUNT): + assert not _update(check, a_target=-1.5) + + +def test_demand_prompt_survives_single_sample_dropouts(): + check = RegenDemandCheck() + fired = False + 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 + if fired: + break + assert fired + + +def test_demand_prompt_does_not_fire_at_standstill(): + check = RegenDemandCheck() + for _ in range(2 * REGEN_DEMAND_EVIDENCE_COUNT): + assert not _update(check, v_ego=0.0) + + +def test_demand_prompt_clears_when_driver_brakes(): + check = RegenDemandCheck() + for _ in range(REGEN_DEMAND_EVIDENCE_COUNT): + _update(check) + assert check.active + + assert not _update(check, brake_pressed=True) + assert not check.active + + +def test_demand_prompt_uses_hysteresis_before_clearing(): + check = RegenDemandCheck() + for _ in range(REGEN_DEMAND_EVIDENCE_COUNT): + _update(check) + 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) + + # Demand returns to the envelope: prompt clears. + assert not _update(check, a_target=-1.5) + assert not check.active + + +def test_demand_prompt_resets_when_pedal_long_inactive(): + check = RegenDemandCheck() + for _ in range(REGEN_DEMAND_EVIDENCE_COUNT): + _update(check) + assert check.active + + assert not _update(check, pedal_long_active=False) + assert not check.active