feat(long): physics decel cap

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -31,7 +31,9 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp
   COMFORT_STOP_ENABLED, COMFORT_STOP_V, COMFORT_STOP_LEAD_V, COMFORT_STOP_GAP, \
   COMFORT_STOP_MAX_DECEL, COMFORT_STOP_RELEASE_V, COMFORT_STOP_HOLD_GAP, \
   GAS_SUPPRESS_ENABLED, GAS_SUPPRESS_DREL, GAS_SUPPRESS_VREL, GAS_SUPPRESS_CLOSE, \
-  GAS_SUPPRESS_RECENT_T, GAS_SUPPRESS_BRAKE_THR
+  GAS_SUPPRESS_RECENT_T, GAS_SUPPRESS_BRAKE_THR, \
+  PHYSICS_CAP_ENABLED, PHYS_CAP_MIN_TTC, PHYS_CAP_MIN_DREL, PHYS_CAP_TGAP, PHYS_CAP_MIN_GAP, PHYS_CAP_VREL_MARGIN, \
+  PHYS_CAP_FORGET_T, PHYS_CAP_MIN_A
 
 _ZERO_ACCEL_EPS = 1e-6
 
@@ -56,6 +58,10 @@ class AccelController:
     self._stop_floor = 0.0       # comfort-stop floor latch (monotone within a stop episode, eased on release)
     self._comfort_stop_enabled = COMFORT_STOP_ENABLED
     self._gas_suppress_enabled = GAS_SUPPRESS_ENABLED
+    self._physics_cap_enabled = PHYSICS_CAP_ENABLED
+    self._cap_vrel = 0.0                                 # held worst-case (most-closing) lead for the physics cap
+    self._cap_dRel = 1e9
+    self._cap_vlead = 0.0
     self._since_brake_frames = 10 ** 6                   # frames since last brake output (gas-suppress recency)
     self._read_params()
 
@@ -99,6 +105,7 @@ class AccelController:
 
     out = self._shape(raw, should_stop, reset, speed_trajectory, t_idxs, stock_brake)
     out = self._comfort_stop(out, reset)   # low-speed monotone comfort decel-to-stop (replaces the self-releasing enforcer)
+    out = self._physics_decel_cap(out, reset)   # don't over-brake a closing lead that has room (brakes < stock)
     return self._finalize(out)
 
   def _shape(self, raw: float, should_stop: bool, reset: bool, speed_trajectory, t_idxs, stock_brake: bool) -> float:
@@ -128,6 +135,37 @@ class AccelController:
       return min(slewed, raw)                                  # blended/e2e: the model owns the brake -> strict never-weaker
     return self._onset_spread(slewed, raw)                     # non-emergency brake: bounded onset spread (<= ONSET_SPREAD_MAX weaker)
 
+  def _physics_decel_cap(self, out: float, reset: bool) -> float:
+    # On a closing lead with genuine room, cap the brake at the kinematic decel needed to settle at a comfortable
+    # gap -- the stock MPC over-brakes a slower lead at speed. Only softens (max(out, a_phys)). Uses a HELD
+    # worst-case closing (decaying ~PHYS_CAP_FORGET_T) so a benign lead-flicker frame cannot relax it, and only
+    # acts when the closing itself warrants a real brake (a_phys <= PHYS_CAP_MIN_A) so it never softens a brake
+    # meant for another cause (curve / vision / a closer lead). Guarded to TTC + distance, pessimistic vRel
+    # margin, self-disengaging as room shrinks (full stock brake returns).
+    if reset or not self._lead_status:
+      self._cap_vrel, self._cap_dRel, self._cap_vlead = 0.0, 1e9, 0.0
+    else:
+      vrel = self._lead_vlead - self._v_ego
+      if vrel < self._cap_vrel:                                # adopt a more-closing lead immediately
+        self._cap_vrel, self._cap_dRel, self._cap_vlead = vrel, self._lead_d, self._lead_vlead
+      else:                                                    # forget an old threat slowly
+        f = DT_MDL / PHYS_CAP_FORGET_T
+        self._cap_vrel += (vrel - self._cap_vrel) * f
+        self._cap_dRel += (self._lead_d - self._cap_dRel) * f
+        self._cap_vlead += (self._lead_vlead - self._cap_vlead) * f
+    if not self._enabled or not self._physics_cap_enabled or out >= 0.0 or not self._lead_status:
+      return out
+    hv, hd, hl = self._cap_vrel, self._cap_dRel, self._cap_vlead
+    if hv >= -0.5 or hd < PHYS_CAP_MIN_DREL or hd / -hv < PHYS_CAP_MIN_TTC:
+      return out
+    room = hd - max(PHYS_CAP_MIN_GAP, PHYS_CAP_TGAP * hl)
+    if room <= 1.0:
+      return out
+    a_phys = -((hv - PHYS_CAP_VREL_MARGIN) ** 2) / (2.0 * room)
+    if a_phys > PHYS_CAP_MIN_A:                                # lead-closing alone does not warrant a real brake
+      return out
+    return max(out, a_phys)                                    # only ever softens; never below the needed decel
+
   def _suppress_gas_near_lead(self, target: float, raw: float) -> float:
     # Coast instead of accelerating toward a close lead: T1 recent brake + lead not pulling away, or T2 clearly
     # closing. Only reduces accel, never a brake. Off => no-op.

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -91,6 +91,20 @@ COMFORT_STOP_HOLD_GAP = 2.0       # m: within this of the reference gap = final-
 # Gas suppression near a lead: coast instead of accelerating toward a close lead, in two cases (OR) --
 # T1 we braked for it within RECENT_T and it is still not pulling away (closing < VREL); T2 we are clearly
 # gaining on it (closing < CLOSE). Only reduces accel, never a brake; opening/far lead keeps its gas.
+# Physics decel cap: on a closing lead with genuine room, don't brake HARDER than kinematics require to settle
+# at a comfortable gap (stock MPC over-brakes a slower lead at high speed via its big comfort-distance cost).
+# The ONLY feature that brakes LESS than stock -- guarded to roomy situations (TTC + distance), pessimistic
+# vRel margin, self-disengaging as room shrinks (full stock brake returns). Gated OFF by default.
+PHYSICS_CAP_ENABLED = False
+PHYS_CAP_MIN_TTC = 4.0            # s: only cap when TTC above this (room to brake gently)
+PHYS_CAP_MIN_DREL = 30.0         # m: only cap when the lead is farther than this
+PHYS_CAP_TGAP = 1.6              # s: target time-gap to settle at
+PHYS_CAP_MIN_GAP = 20.0          # m: floor on the target gap
+PHYS_CAP_VREL_MARGIN = 1.5       # m/s: treat the lead as closing this much faster (pessimistic -> firmer cap)
+PHYS_CAP_FORGET_T = 1.0          # s: decay of the held worst-case closing (a benign flicker frame cannot relax the cap)
+PHYS_CAP_MIN_A = -0.5            # m/s^2: only cap if the closing lead itself warrants at least this brake
+                                 # (else the brake is for another cause -- curve / vision / a closer lead -- leave it)
+
 GAS_SUPPRESS_ENABLED = False
 GAS_SUPPRESS_DREL = 60.0          # m: lead within this distance
 GAS_SUPPRESS_VREL = 0.5           # m/s: "not pulling away" bound for the rebound trigger (vLead - vEgo)

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -39,11 +39,12 @@ def make_sm(v_ego=20.0, lead_status=False, lead_d=0.0, lead_vlead=0.0):
   return {'carState': SimpleNamespace(vEgo=v_ego), 'radarState': SimpleNamespace(leadOne=lead)}
 
 
-def make_controller(enabled=True, personality=NORMAL, crash_cnt=0, comfort_stop=False, gas_suppress=False):
+def make_controller(enabled=True, personality=NORMAL, crash_cnt=0, comfort_stop=False, gas_suppress=False, physics_cap=False):
   store = {"AccelPersonalityEnabled": enabled, "AccelPersonality": int(personality)}
   ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=crash_cnt), params=FakeParams(store))
   ctrl._comfort_stop_enabled = comfort_stop   # comfort_stop is gated off in production; opt in per-test
   ctrl._gas_suppress_enabled = gas_suppress   # gas-suppression is gated off in production; opt in per-test
+  ctrl._physics_cap_enabled = physics_cap     # physics decel cap is gated off in production; opt in per-test
   ctrl.update(make_sm())
   return ctrl
 
@@ -384,6 +385,39 @@ def test_gas_suppress_does_not_touch_brake():
   assert out <= 0.0                                            # suppression no-ops on a brake (never weakens it)
 
 
+# --- physics decel cap (no over-brake on a roomy closing lead) ---------------
+
+def _closing(ctrl, v_ego, lead_d, lead_vlead, raw):
+  ctrl.update(make_sm(v_ego=v_ego, lead_status=True, lead_d=lead_d, lead_vlead=lead_vlead))
+  return ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False)
+
+def test_physics_cap_softens_roomy_overbrake():
+  # the 0488 t600 case: lead 84m, 23.7 m/s, ego 34.7 (vRel -11, TTC 7.6s). -2.8 over-brakes; physics ~ -1.7.
+  out = _closing(make_controller(physics_cap=True), 34.7, 84.0, 23.7, -2.8)
+  assert -2.1 < out < -1.2
+
+def test_physics_cap_never_touches_close_lead():
+  out = _closing(make_controller(physics_cap=True), 20.0, 18.0, 9.0, -2.8)   # dRel < MIN_DREL
+  assert out == pytest.approx(-2.8, abs=_EPS)
+
+def test_physics_cap_never_touches_low_ttc():
+  out = _closing(make_controller(physics_cap=True), 30.0, 35.0, 5.0, -2.8)   # vRel -25 -> TTC ~1.4s
+  assert out == pytest.approx(-2.8, abs=_EPS)
+
+def test_physics_cap_off_passthrough():
+  out = _closing(make_controller(), 34.7, 84.0, 23.7, -2.8)                  # default off
+  assert out == pytest.approx(-2.8, abs=_EPS)
+
+def test_physics_cap_only_softens_never_hardens():
+  out = _closing(make_controller(physics_cap=True), 34.7, 84.0, 23.7, -0.5)  # already gentler than physics
+  assert out > -1.5                                                          # cap does not deepen it
+
+def test_physics_cap_skips_brake_not_from_lead():
+  # barely-closing far lead (vRel -1) but a real -1.5 brake (curve/vision/closer lead): a_phys ~ 0 -> don't cap.
+  out = _closing(make_controller(physics_cap=True), 34.0, 100.0, 33.0, -1.5)
+  assert out == pytest.approx(-1.5, abs=_EPS)
+
+
 def test_onset_spread_bounded_and_skipped_for_emergency():
   # Non-emergency brake onset is spread (lagged) but never by more than ONSET_SPREAD_MAX; an emergency brake
   # is instant full depth (no spread).