diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index c14ad131ce..d7854bdac5 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/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. diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 6002014def..cc00eb99c5 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index 6e792535c7..32d2db9686 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/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).