From 0a68face78165fd27fde3fe8234fa113caee3076 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 21 Jun 2026 12:48:26 -0700 Subject: [PATCH] long refactor --- .../lib/accel_personality/accel_controller.py | 51 ++++++++++------- .../lib/accel_personality/constants.py | 13 +++++ .../tests/test_accel_controller.py | 56 +++++++++++++++++-- .../lib/radar_distance/radar_distance.py | 53 ++++++------------ .../tests/test_radar_distance.py | 31 +++++----- 5 files changed, 129 insertions(+), 75 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index 6e719378ba..e54ae29ec4 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -18,7 +18,8 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants imp NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, SMOOTH_DECEL_BP, \ STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, \ SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, \ - MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, HARD_BRAKE_ONSET_JERK, STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \ + MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, HARD_BRAKE_ONSET_JERK, OVERBITE_CAP, \ + STOP_PASSTHROUGH_V, STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T, \ ONSET_JERK0, ONSET_JERK_GAIN, ONSET_GAP_SOFT, ONSET_GAP_GAIN, ONSET_JERK_MAX, ONSET_HANDBACK_JERK, \ SOFT_ONSET_MAX_BRAKE_NEED, SOFT_ONSET_MAX_INSTANT_ACCEL, SOFT_ONSET_REARM_FRAMES @@ -39,6 +40,7 @@ class AccelController: self._decel_target = 0.0 self._smooth_active = False self._bypassed = False + self._no_soften = False # blended/e2e: anticipate (front-load) but never soften the onset # convex brake-onset shaper state self._onset_latched = False # sticky: True once an onset goes firm -> no re-soften until sustained release self._onset_release = 0 # consecutive non-deepening frames (sticky re-arm debounce) @@ -79,37 +81,45 @@ class AccelController: self._decel_target = 0.0 self._smooth_active = False self._soft_active = False + self._bypassed = False + # Blended/e2e (stock_brake): the model owns the brake, so never SOFTEN it (no convex soft-onset), + # but still allow the never-weaker front-load so blended anticipates like ACC and brakes enough. + self._no_soften = bool(stock_brake) - # The convex onset shaper runs ONLY for ECO/SPORT (NORMAL and disabled are stock). Reset its state - # whenever it cannot run so nothing leaks across a personality toggle or a passthrough interlude. - if not (self._enabled and self._personality != NORMAL): + # The convex soft-onset runs ONLY for an enabled non-NORMAL, non-no-soften personality. Reset its + # state whenever it cannot run so nothing leaks across a toggle or a passthrough interlude. + if not (self._enabled and self._personality != NORMAL and not self._no_soften): self._reset_onset() - # Passthroughs (hand the plan straight through, no shaping): - if reset or not self._enabled or (stock_brake and (raw < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED)): - self._bypassed = False # disabled / reset / blended-e2e braking - return self._passthrough(raw) + # --- Full stock passthroughs (no shaping at all) --- + if reset or not self._enabled: + return self._passthrough(raw) # disabled / reset + if self._v_ego < STOP_PASSTHROUGH_V and raw <= 0.0: + # Stop/creep regime: braking is stock so the stop distance matches OFF exactly (no softened crawl / + # coast-in). Launch (positive accel) is unaffected. Mirrors radar_distance's low-speed neutrality. + return self._stand_down(raw) + + # --- Hard brake / stop: never soften the DEPTH (onset rate-limited, full plan depth always reached) --- self._bypassed = self._emergency_bypass(raw, should_stop) if self._bypassed: - # A hard brake's DEPTH is never softened. True emergencies (FCW / crash imminent) pass straight - # through; other firm brakes get a deepening-only onset rate cap so the firm brake arrives smoothly - # instead of as a raw stock grab (full plan depth still reached -> never weaker or later in size). - if self._mpc.crash_cnt > 0: + if self._mpc.crash_cnt > 0: # true emergency / FCW -> pure passthrough return self._stand_down(raw) return self._stand_down_jerk_limited(raw) if self._stop_imminent(speed_trajectory, t_idxs): # stop coming -> stock decel, no coast/creep return self._stand_down_jerk_limited(raw) - # Front-load a gentle early brake when a deeper brake is predicted ahead. The convex shaper owns the - # output when it governed this frame (soft_active); otherwise never weaker than the plan. + # --- Smooth shaping. min(slewed, raw) keeps the output NEVER WEAKER than the plan; the only softener + # is the convex soft-onset, which is gated off above for no-soften (blended) mode. --- if self._brake_need >= MIN_SMOOTH_BRAKE_NEED: self._smooth_active = True - self._decel_target = self.get_decel_target(self._brake_need) + # Front-load a gentle early brake when a deeper brake is predicted ahead, but never bite more than + # OVERBITE_CAP below the LIVE plan (a cut-in/merge spikes brake_need while the plan still wants + # throttle -> abrupt over-bite). Once the plan itself brakes, the table wins -> anticipation preserved. + self._decel_target = max(self.get_decel_target(self._brake_need), raw - OVERBITE_CAP) slewed = self._slew(min(raw, self._decel_target)) return self._finalize(slewed if self._soft_active else min(slewed, raw)) - # Below the smooth-brake threshold: track the plan, never weaker than it while braking. - slewed = self._slew(raw) + slewed = self._slew(raw) # below the smooth-brake threshold: track the plan if self._soft_active or raw >= 0.0: return self._finalize(slewed) return self._finalize(min(slewed, raw)) @@ -153,9 +163,10 @@ class AccelController: def _onset_soft_armed(self, target_accel: float) -> bool: # Gentle non-emergency onset. Armed from the FIRST deepening tick (no brake_need lower gate, so the - # gentle bite lands on the actual onset, not after the plan has already deepened). Two upper gates - # keep it off firm/deep braking: the 3s-lookahead brake_need ceiling AND the instantaneous raw depth. - return (self._enabled and self._personality != NORMAL and + # gentle bite lands on the actual onset, not after the plan has already deepened). Off in no-soften + # (blended/e2e) mode. Two upper gates keep it off firm/deep braking: the 3s-lookahead brake_need + # ceiling AND the instantaneous raw depth. + return (self._enabled and self._personality != NORMAL and not self._no_soften and 0.0 < self._brake_need < SOFT_ONSET_MAX_BRAKE_NEED and target_accel > SOFT_ONSET_MAX_INSTANT_ACCEL) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 9942bf8fb2..3f973b7c13 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -48,6 +48,13 @@ ACCEL_RISE_JERK = {ECO: 1.0, NORMAL: 1.5, SPORT: 2.2} # accel-onset jerk: high SMOOTH_DECEL_LOOKAHEAD_T = 3.0 MIN_SMOOTH_BRAKE_NEED = 0.2 + +# Front-load over-bite cap. The SMOOTH_DECEL front-load is allowed to brake at most this much DEEPER than +# the live raw plan. On a cut-in/merge the 3s brake_need spikes and the table would front-load a firm brake +# while the plan still wants throttle/coast -> an abrupt early bite (the "worse on merge" feel; routes +# 45e/460). This binds only in that contradictory case; once the plan itself brakes, raw-OVERBITE_CAP sits +# below the table value so the table wins and the anticipatory early brake (route 456 fix) is preserved. +OVERBITE_CAP = 0.30 # m/s^2 max front-load depth below the live plan HARD_BRAKE_TARGET_ACCEL = -1.5 HARD_BRAKE_NEED = 2.6 @@ -69,6 +76,12 @@ HARD_BRAKE_ONSET_JERK = 2.0 # m/s^3, deepening-only onset rate cap on firm (no STOP_IMMINENT_VEGO = 1.0 # m/s plan-predicted speed below this within the lookahead == stop coming STOP_IMMINENT_LOOKAHEAD_T = 3.0 # s +# Stop/creep stop-neutrality. Below this ego speed, the brake side hands the plan straight through (stock), +# so the controller cannot soften the final crawl and let the car coast in closer than stock. Matches the +# radar_distance low-speed stop-neutrality so ON == OFF near stops. Positive-accel (launch) shaping is +# unaffected (the launch profiles still apply via the accel ceiling). +STOP_PASSTHROUGH_V = 5.0 # m/s ego speed below which braking is stock passthrough + # --- Convex brake-onset shaper (param-gated; ECO/SPORT only, NORMAL = stock passthrough) --- # The grabby bite is the raw MPC plan: stock deepening uses a CONSTANT jerk (integrates to a LINEAR # accel ramp) and min(slewed,raw) lets the deep raw plan win, so the bite passes through untouched. 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 181546b061..2175854063 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 @@ -15,7 +15,8 @@ from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController as _AC # noqa: F401 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \ ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE, \ - STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_ONSET_JERK, AccelerationPersonality, \ + STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_ONSET_JERK, OVERBITE_CAP, \ + STOP_PASSTHROUGH_V, AccelerationPersonality, \ BRAKE_DEEPENING_JERK, ONSET_JERK0, ONSET_GAP_SOFT, ONSET_HANDBACK_JERK # The convex onset brakes shallower than the plan during the bite, but the instantaneous-gap catch-up @@ -107,6 +108,48 @@ def test_early_soft_braking_brakes_before_plan(): assert ctrl.brake_need() == pytest.approx(1.0) +def test_low_speed_brake_is_stock_passthrough(): + # Stop/creep regime (vEgo < STOP_PASSTHROUGH_V): braking is handed through unshaped (stock) so the + # controller cannot soften the crawl and let the car coast in closer than stock. + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=STOP_PASSTHROUGH_V - 0.1)) + for raw in (-0.3, -1.0): # a braking plan that would normally front-load + out = ctrl.smooth_target_accel(raw, flat_traj(-1.5), T_IDXS, should_stop=False) + assert out == pytest.approx(raw, abs=_EPS) # exact stock passthrough, no front-load + assert not ctrl.smooth_active() + + +def test_low_speed_launch_still_shapes(): + # The low-speed brake passthrough must NOT neutralize positive-accel (launch) shaping. + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=STOP_PASSTHROUGH_V - 0.1)) + ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False) # seed + out = ctrl.smooth_target_accel(1.5, flat_traj(1.5), T_IDXS, should_stop=False) + assert out < 1.5 # rise-rate limited (shaped), not raw passthrough + + +def test_overbite_cap_limits_frontload_vs_live_plan(): + # Cut-in/merge: raw plan still wants throttle (+0.5) while a deep brake is predicted ahead (brake_need + # high). The front-load must not settle more than OVERBITE_CAP below the LIVE plan -> no abrupt early grab. + ctrl = make_controller(personality=ECO) + traj = [0.5, 0.3, 0.0, -0.5, -1.5, -2.0] + [-2.0] * (len(T_IDXS) - 6) # throttle now, hard brake later + out = 0.0 + for _ in range(8): # let the accel rise-rate slew settle + out = ctrl.smooth_target_accel(0.5, traj, T_IDXS, should_stop=False) + assert ctrl.smooth_active() + assert out == pytest.approx(0.5 - OVERBITE_CAP, abs=1e-3) # front-load clamped to exactly cap below plan + + +def test_overbite_cap_preserves_anticipation_when_plan_braking(): + # Once the live plan is itself braking, raw-OVERBITE_CAP sits below the table, so the cap does NOT bind + # and the anticipatory front-load (route 456 fix) is preserved (output deeper than the shallow raw). + ctrl = make_controller(personality=ECO) + out = 0.0 + for _ in range(4): + out = ctrl.smooth_target_accel(-0.2, flat_traj(-1.5), T_IDXS, should_stop=False) + assert out < -0.2 - _EPS # still front-loads below the live -0.2 plan + + def test_stop_imminent_stands_down_but_moving_follow_shapes(): # Stop coming (plan speed -> ~0): stand down to stock decel so the gentle bite can't coast into the # stop (creep). Slowing to a MOVING follow (plan stays > STOP_IMMINENT_VEGO): gentle onset stays active @@ -273,11 +316,14 @@ def test_fcw_crash_cnt_bypass(): assert ctrl.bypassed() -def test_e2e_brake_passthrough(): +def test_blended_never_softens_brake(): + # Blended/e2e (stock_brake): the brake is NEVER softened -> output is never weaker than the plan and + # the convex soft-onset never engages. (It may still anticipate via the never-weaker front-load.) ctrl = make_controller(personality=ECO) - out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False, stock_brake=True) - assert out == pytest.approx(-1.0, abs=_EPS) - assert not ctrl.smooth_active() + for raw in [0.0, -0.3, -0.6, -0.9, -1.0, -1.0, -1.0]: + out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False, stock_brake=True) + assert out <= raw + _EPS # never weaker than the plan (no softening) + assert not ctrl._soft_active # convex soft-onset disabled in blended def test_out_of_range_personality_clamps(): diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index a3506fda02..5166134a32 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -7,6 +7,10 @@ See the LICENSE.md file in the root directory for more details. Radar Distance: hold a just-dropped, recently-sustained lead alive through radar flicker so the MPC does not lose+regain it. Obstacle-monotone (held obstacle <= last real <= stock) -> braking is always >= stock, never less. Wall-clock bounded, flicker-proof. Default off => stock passthrough. + +Stop-neutral: at/below LOW_SPEED_PASSTHROUGH_V (the stop/creep regime) it returns the RAW radarstate +unchanged so the stop distance is byte-identical to stock (RadarDistance OFF). The flicker-hold only +governs above that speed, where lose+regain actually matters. """ from opendbc.car import structs @@ -19,12 +23,13 @@ DROPOUT_DREL = 1.0 FCW_PROB_CAP = 0.9 # held lead can't reach the FCW gate (>0.9) -> no false FCW MIN_HELD_DREL = 0.5 -# Roomier stops. Stock long MPC targets STOP_DISTANCE (6m) but soft-coasts in to ~3.5-4m on a real stop, -# which feels close. We pull the reported lead in by STOP_MARGIN as the car slows so the MPC settles that -# much farther back. Scoped to the stop regime by a v_ego ramp (full at/below STOP_MARGIN_V[0], zero at/above -# STOP_MARGIN_V[1]) so it never touches mid/high-speed following. Set STOP_MARGIN=0 to disable. -STOP_MARGIN = 1.5 # m extra standstill/low-speed gap -STOP_MARGIN_V = (1.0, 4.0) # m/s ramp: full <=1.0, none >=4.0 +# Stop-neutrality gate. At/below this speed we are in the stop/creep regime, where the car's stop +# distance must match stock openpilot exactly (and radard's low_speed_override already supplies a robust +# closest-track lead). So below this speed smooth_radarstate returns the RAW radarstate unchanged -- +# byte-identical to RadarDistance OFF -- so neither the flicker-hold's dead-reckoned dRel nor any other +# transform can move the lead the MPC sees near a stop. The hold is still stepped to keep its state warm +# for when speed rises back above the gate. Above this speed the highway flicker-hold runs (its purpose). +LOW_SPEED_PASSTHROUGH_V = 5.0 # m/s (~18 km/h): covers stop + creep, below highway following class _HeldLead: @@ -50,23 +55,6 @@ class _RadarStateProxy: self.leadTwo = lead_two -class _LeadView: - # mirror the fields the MPC reads from a lead, with dRel pulled in by `pull` (>=0 m) so the car keeps that - # much extra gap. Only used in the low-speed stop regime (see STOP_MARGIN); high-speed leads pass through. - __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') - - def __init__(self, src, pull): - self.status = src.status - self.dRel = max(MIN_HELD_DREL, src.dRel - pull) - self.yRel = src.yRel - self.vRel = src.vRel - self.vLead = src.vLead - self.vLeadK = src.vLeadK - self.aLeadK = src.aLeadK - self.aLeadTau = src.aLeadTau - self.modelProb = src.modelProb - - class _LeadHold: def __init__(self): self._last = None @@ -130,23 +118,14 @@ class RadarDistanceController: def enabled(self) -> bool: return self._enabled - def _stop_margin(self) -> float: - if STOP_MARGIN <= 0.0: - return 0.0 - lo, hi = STOP_MARGIN_V - frac = (hi - self._v_ego) / (hi - lo) - frac = 0.0 if frac < 0.0 else (1.0 if frac > 1.0 else frac) - return STOP_MARGIN * frac - def smooth_radarstate(self, radarstate): if not self._enabled: return radarstate + # Step the holds every frame to keep their flicker state warm, but in the stop/creep regime return the + # RAW radarstate unchanged so the lead the MPC sees -- and therefore the stop distance -- is byte-stock + # (identical to RadarDistance OFF). Above the gate the highway flicker-hold governs (its real purpose). one = self._one.step(radarstate.leadOne) two = self._two.step(radarstate.leadTwo) - pull = self._stop_margin() - if pull > 0.0: - if one.status: - one = _LeadView(one, pull) - if two.status: - two = _LeadView(two, pull) + if self._v_ego < LOW_SPEED_PASSTHROUGH_V: + return radarstate return _RadarStateProxy(one, two) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py index 518d006e80..fad9760bd4 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py @@ -10,7 +10,7 @@ from types import SimpleNamespace import pytest from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \ - RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, STOP_MARGIN, STOP_MARGIN_V + RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V COMFORT_BRAKE = 2.5 @@ -38,7 +38,7 @@ def obstacle(ld): def ctrl(enabled=True): c = RadarDistanceController(CP=SimpleNamespace(), params=FakeParams({'RadarDistance': enabled})) - c._v_ego = STOP_MARGIN_V[1] + 10.0 # default to a no-margin (cruise) speed so hold-logic tests are isolated + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0 # default above the gate so hold-logic tests exercise the flicker-hold return c @@ -85,21 +85,26 @@ def test_low_speed_override_lead_arms_hold(): assert held.status is True # armed off the prob=0 lead, holds through dropout -def test_stop_margin_pulls_lead_in_near_stop(): - # at standstill the reported lead is pulled in by STOP_MARGIN so the MPC keeps extra real gap (roomier stop) +def test_low_speed_returns_raw_object(): + # Stop/creep regime: ENABLED returns the EXACT raw radarstate object (byte-identical to OFF), so the + # lead the MPC sees -- and thus the stop distance -- is stock. This is the core stop-neutrality guarantee. c = ctrl() - c._v_ego = STOP_MARGIN_V[0] # full-margin regime - out = c.smooth_radarstate(rs(lead(status=True, dRel=6.0, vRel=0.0, vLead=0.0))) - assert out.leadOne.dRel == pytest.approx(6.0 - STOP_MARGIN, abs=1e-6) + c._v_ego = LOW_SPEED_PASSTHROUGH_V - 0.1 + r = rs(lead(status=True, dRel=6.0, vRel=0.0, vLead=0.0)) + assert c.smooth_radarstate(r) is r # object identity == stock -def test_stop_margin_off_at_speed(): - # no margin at cruise speed -> untouched passthrough (full-fidelity real lead to the MPC) +def test_low_speed_passthrough_but_hold_warmed_for_highway(): + # At low speed the raw radarstate is returned, but the hold is still stepped (state kept warm) so the + # flicker-hold engages the moment speed rises above the gate. c = ctrl() - c._v_ego = STOP_MARGIN_V[1] + 5.0 - one = lead(status=True, dRel=40.0) - out = c.smooth_radarstate(rs(one)) - assert out.leadOne is one + for _ in range(3): # sustain a real lead while in the low-speed regime + c._v_ego = LOW_SPEED_PASSTHROUGH_V - 0.1 + r = rs(lead(dRel=30.0, vRel=-4.0, vLead=16.0)) + assert c.smooth_radarstate(r) is r # returned object stays raw at low speed + c._v_ego = LOW_SPEED_PASSTHROUGH_V + 10.0 # rise above the gate -> dropout now held (proxy, not raw) + out = c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0))) + assert out.leadOne.status is True def test_obstacle_monotone_during_hold():