selfdrived: prompt max regen when planned decel exceeds envelope

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

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

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