diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index e54ae29ec4..9b3af89c7c 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -3,6 +3,12 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. + +Acceleration personality: per-profile launch/cruise accel ceiling (ECO/NORMAL/SPORT) plus an +anticipatory brake front-load. SAFETY INVARIANT: on the brake side the output is NEVER WEAKER than the +plan -- it can only be EQUAL or DEEPER (front-load). It never softens, delays, or rate-limits a brake, +so it can never under-brake a closing lead. Hard brakes, stops and low speed pass the plan straight +through (stock). Disabled => byte-stock. """ from collections.abc import Sequence @@ -15,13 +21,11 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \ - 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, 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 + NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, \ + STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, SMOOTH_DECEL_BP, 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, OVERBITE_CAP, STOP_PASSTHROUGH_V, \ + STOP_IMMINENT_VEGO, STOP_IMMINENT_LOOKAHEAD_T _ZERO_ACCEL_EPS = 1e-6 @@ -40,12 +44,6 @@ 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) - self._soft_active = False # True iff the convex shaper governed this frame's output (bypasses min(.,raw)) - self._soft_episode = False # True while a soft onset is open (incl. closing its gap) -> own deepening, no snap self._read_params() def _read_params(self) -> None: @@ -80,49 +78,32 @@ class AccelController: self._brake_need = self._compute_brake_need(raw, accel_trajectory, t_idxs) 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 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() - - # --- Full stock passthroughs (no shaping at all) --- + # --- Full stock passthroughs (output is exactly the plan, no shaping) --- if reset or not self._enabled: - return self._passthrough(raw) # disabled / reset + 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) --- + # Stop/creep regime: braking is stock so the stop distance matches OFF exactly (no coast-in). + return self._passthrough(raw) self._bypassed = self._emergency_bypass(raw, should_stop) - if self._bypassed: - 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) + if self._bypassed or self._stop_imminent(speed_trajectory, t_idxs): + # Hard brake (closing lead / FCW / deep plan) or a coming stop: hand the plan straight through at + # FULL strength and rate -- never delay or rate-limit it (a delayed hard brake is a near-crash). + return self._passthrough(raw) - # --- 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. --- + # --- Anticipatory front-load. NEVER weaker than the plan: min(., raw) guarantees the output is only + # ever EQUAL or DEEPER than the plan, so the controller can never under-brake. --- + target = raw if self._brake_need >= MIN_SMOOTH_BRAKE_NEED: self._smooth_active = True # 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)) - - 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)) + target = min(raw, self._decel_target) + slewed = self._slew(target) + return self._finalize(min(slewed, raw) if raw < 0.0 else slewed) def _stop_imminent(self, speed_trajectory: Sequence[float] | None, t_idxs: Sequence[float]) -> bool: # plan predicts a near-stop within the lookahead -> a stop is coming (lead or light/sign). @@ -143,79 +124,18 @@ class AccelController: raw_target_accel <= HARD_BRAKE_TARGET_ACCEL or self._brake_need >= HARD_BRAKE_NEED) def _slew(self, target_accel: float) -> float: + # Jerk-limit the brake DEEPENING (smooths the front-load's extra depth). On the brake side the caller + # clamps with min(., raw), so this NEVER delays a real brake -- when the plan is deeper than the slewed + # value, min(.) picks the plan and the brake passes through at full rate. target_accel = float(target_accel) - p = self._personality - jmax = BRAKE_DEEPENING_JERK[p] - deepening = target_accel <= self._last_target_accel - if not deepening: - # genuine release / coast: close any soft episode, advance the re-arm debounce, unlatch after a - # sustained release. - self._soft_episode = False - self._onset_release += 1 - if self._onset_release >= SOFT_ONSET_REARM_FRAMES: - self._onset_latched = False - return self._slew_up(target_accel) - self._onset_release = 0 - # NORMAL (and disabled, forced to NORMAL) -> stock constant-jerk linear deepening, byte-exact. - if p == NORMAL: + if target_accel <= self._last_target_accel: + jmax = BRAKE_DEEPENING_JERK[self._personality] return self._clean_accel(max(target_accel, self._last_target_accel - jmax * DT_MDL)) - return self._slew_convex(target_accel, jmax) - - 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). 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) - - def _slew_convex(self, target_accel: float, jmax: float) -> float: - # target_accel is the effective plan to track (raw, or min(raw, decel_target) on the smooth branch). - # Dispatch: armed -> gentle bite; firm zone with an open soft gap -> fast hand-back; else stock. - last = self._last_target_accel - gap = max(0.0, last - target_accel) # m/s^2 currently shallower than the plan (last,target both <=0) - soft_armed = self._onset_soft_armed(target_accel) - if soft_armed and not self._onset_latched: - return self._onset_bite(target_accel, last, gap) - if soft_armed: # firm/deep zone -> latch off further (re)arming - self._onset_latched = True - if self._soft_episode and gap > _ZERO_ACCEL_EPS: - return self._onset_handback(target_accel) - self._soft_episode = False # no open gap: NEVER soften a fresh firm brake - return self._clean_accel(max(target_accel, last - jmax * DT_MDL)) # stock; caller does min(.,raw) - - def _onset_bite(self, target_accel: float, last: float, gap: float) -> float: - # Gentle convex onset. Depth-proportional jerk: gentle ONSET_JERK0 at the bite (a~0), growing with - # current decel depth -- da/dt = j0 + k*a integrates to a(t) = (j0/k)*(exp(k*t)-1), the exponential- - # growth profile. A stateless instantaneous-gap catch-up adds bounded jerk once realized lags the plan - # by more than ONSET_GAP_SOFT, hard-capped at ONSET_JERK_MAX so even the catch is never a grab. - p = self._personality - self._soft_episode = True - jerk = ONSET_JERK0[p] + ONSET_JERK_GAIN[p] * abs(last) - jerk = min(jerk + ONSET_GAP_GAIN[p] * max(0.0, gap - ONSET_GAP_SOFT[p]), ONSET_JERK_MAX[p]) - out = max(last - jerk * DT_MDL, target_accel) # never deeper than the plan -> only softer-or-equal - if out <= target_accel + _ZERO_ACCEL_EPS: # gap closed -> episode complete - self._soft_episode = False - self._soft_active = True - return self._clean_accel(out) - - def _onset_handback(self, target_accel: float) -> float: - # Plan left the gentle zone but a soft gap is still open: close it FAST (firm, jerk-limited so it is - # not a snap) so the output catches the plan before braking gets firm -> no late-brake lag. - out = max(self._last_target_accel - ONSET_HANDBACK_JERK[self._personality] * DT_MDL, target_accel) - if out <= target_accel + _ZERO_ACCEL_EPS: - self._soft_episode = False - self._soft_active = True - return self._clean_accel(out) - - def _reset_onset(self) -> None: - self._onset_latched = False - self._onset_release = 0 - self._soft_active = False - self._soft_episode = False + return self._slew_up(target_accel) def _slew_up(self, target_accel: float) -> float: + # Releasing the brake / accelerating: rate-limit the rise (release jerk on the brake side, the + # personality accel-rise jerk on the throttle side). if self._last_target_accel < 0.0: released = min(target_accel, self._last_target_accel + BRAKE_RELEASE_JERK * DT_MDL) if released <= 0.0: @@ -226,28 +146,8 @@ class AccelController: def _passthrough(self, target_accel: float) -> float: self._smooth_active = False - self._soft_active = False return self._finalize(target_accel) - def _stand_down(self, target_accel: float) -> float: - # clear shaper state and hand the plan straight through (true emergency / FCW) - self._reset_onset() - return self._passthrough(target_accel) - - def _stand_down_jerk_limited(self, target_accel: float) -> float: - # Like _stand_down but caps the DEEPENING rate of the onset at HARD_BRAKE_ONSET_JERK. One-sided: - # releasing or accel passes straight through, and depth is never reduced (output rejoins the plan - # within ~65ms), so the firm brake is never weaker or meaningfully later -- only the onset is smoothed. - if not (self._enabled and self._personality != NORMAL): # off / NORMAL -> stock passthrough (off==stock) - return self._stand_down(target_accel) - self._reset_onset() - self._smooth_active = False - self._soft_active = False - raw = float(target_accel) - last = self._last_target_accel - out = max(raw, last - HARD_BRAKE_ONSET_JERK * DT_MDL) if raw < last else raw # limit deepening only - return self._finalize(out) - def _finalize(self, target_accel: float) -> float: target_accel = self._clean_accel(target_accel) self._last_target_accel = target_accel diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 3f973b7c13..6abc1c2a05 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -30,12 +30,11 @@ A_CRUISE_MAX_V = { } RISE_RATE = {ECO: 0.10, NORMAL: 0.15, SPORT: 0.22} # ceiling open-rate: all >> stock 0.05 for fast take-off -# Early soft braking: predicted brake need (m/s^2) -> early decel target (m/s^2). Front-loads a gentle -# decel as soon as the 3s plan lookahead predicts a brake, so decel is spread out instead of arriving as -# one late firm onset. The old ECO row was near-flat (-0.07 at brake_need~1.0 vs an eventual ~-0.88 plan -# brake) so it barely front-loaded -> late, jerky onsets on route 00000456. Deepened toward (but kept -# gentler than) NORMAL. Hard brakes (brake_need>=HARD_BRAKE_NEED or raw<=HARD_BRAKE_TARGET_ACCEL) still -# bypass to stock, and min(.,raw) keeps it never weaker than the plan. +# Anticipatory front-load: predicted brake need (m/s^2) -> early decel target (m/s^2). When the 3s plan +# lookahead predicts a brake, start a gentle decel EARLY so braking is spread out instead of arriving as +# one late firm onset (route 00000456). It is one-sided: min(., raw) keeps the output NEVER weaker than the +# plan, so it can only brake EQUAL or EARLIER/DEEPER, never softer. Hard brakes (brake_need>=HARD_BRAKE_NEED +# or raw<=HARD_BRAKE_TARGET_ACCEL) pass straight through at full strength. SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4] SMOOTH_DECEL_V = { ECO: [0.00, -0.08, -0.20, -0.35, -0.55, -0.78, -1.00], @@ -55,24 +54,16 @@ MIN_SMOOTH_BRAKE_NEED = 0.2 # 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: at/below this accel, or this predicted brake_need within the lookahead, the controller hands +# the plan straight through at full strength and rate (no front-load, no rate limit) -- a firm/closing-lead +# brake must never be delayed, softened or rate-limited. HARD_BRAKE_TARGET_ACCEL = -1.5 HARD_BRAKE_NEED = 2.6 -# Hard-brake onset jerk cap. Firm/closing-lead brakes (raw<=HARD_BRAKE_TARGET_ACCEL or brake_need>= -# HARD_BRAKE_NEED) used to fully stand the shaper down -> raw stock MPC onset, which lands as a grab -# (felt jerk on routes 45c/45d, both user bookmarks). This caps ONLY the deepening RATE of such onsets; -# the full plan depth is always reached (one-sided), so the brake is never weaker or later in magnitude -- -# only the rate of getting there is smoothed. Verified on the 45d@767 closing lead (ego 28.6 m/s, lead -# braking aLeadK -2.9): cap=2.0 adds <=65ms to reach 80% of target and <=0.22m extra gap, inside the -# 150ms / 2.0m safety budget. Do NOT lower below 2.0 (1.8 breaches the lag gate). True emergencies -# (mpc.crash_cnt>0, i.e. FCW) skip the cap and stay pure passthrough. -HARD_BRAKE_ONSET_JERK = 2.0 # m/s^3, deepening-only onset rate cap on firm (non-crash) hard brakes - -# Stop-imminent stand-down. The shaper's gentle bite is softer than the plan, so on a STOP approach it -# coasts the car in -> halts too close / "stop-roll-stop" creep. When the plan predicts a near-stop -# within the lookahead, stand the shaper down (full stock decel) so it stops at the proper gap with no -# coast. Keyed on the PREDICTED speed reaching ~0 (covers lead AND light/sign stops), NOT raw ego speed -# -- so non-stop low-speed braking (slowing to a moving follow) keeps the gentle onset at every speed. +# Stop-imminent stand-down. When the plan predicts a near-stop within the lookahead, hand the plan straight +# through (stock decel) so the car stops at the proper gap with no front-load coast-in. Keyed on the +# PREDICTED speed reaching ~0 (covers lead AND light/sign stops), not raw ego speed. STOP_IMMINENT_VEGO = 1.0 # m/s plan-predicted speed below this within the lookahead == stop coming STOP_IMMINENT_LOOKAHEAD_T = 3.0 # s @@ -81,41 +72,3 @@ STOP_IMMINENT_LOOKAHEAD_T = 3.0 # s # 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. -# Fix: jerk-limit the deepening with a DEPTH-PROPORTIONAL jerk -# jerk(a) = ONSET_JERK0 + ONSET_JERK_GAIN * abs(a_current), capped at ONSET_JERK_MAX -# At the bite (a~0) the jerk is ONSET_JERK0 (gentle); it grows with decel depth, so the decel magnitude -# follows da/dt = j0 + k*a => a(t) = (j0/k)*(exp(k*t) - 1) -- the exponential-growth reference. The -# output is never deeper than the plan (only ever softer-or-equal during the bite) and converges to it. -# No velocity-debt feedback: it carried stale state across closely-spaced stop-and-go brakes and -# over-braked the next onset (verified). NORMAL omitted -> shaper never runs. -ONSET_JERK0 = {ECO: 0.15, SPORT: 0.25} # m/s^3 initial gentle jerk at the bite (target band 0.15-0.25) -ONSET_JERK_GAIN = {ECO: 0.9, SPORT: 1.5} # 1/s depth-proportional growth rate k (lowered: gentler jerk-build = smoother decel, less "jerky") - -# Bounded softening: the gentle bite lags the plan (brakes shallower) at the very start. To keep the -# softening modest (so it never feels like "no brakes"), an INSTANTANEOUS-gap catch-up adds jerk when -# realized lags the plan by more than ONSET_GAP_SOFT, hard-capped at ONSET_JERK_MAX. This uses the -# current accel gap only (no integrated state) so nothing carries across closely-spaced brakes. Steady -# softening then settles near ONSET_GAP_SOFT; the hard cap keeps the catch from ever being a grab. -ONSET_GAP_SOFT = {ECO: 0.30, SPORT: 0.25} # m/s^2 tolerated shallower-than-plan gap before catch-up -ONSET_GAP_GAIN = {ECO: 4.0, SPORT: 5.0} # 1/s extra jerk per m/s^2 of gap beyond ONSET_GAP_SOFT -ONSET_JERK_MAX = {ECO: 1.1, SPORT: 1.4} # m/s^3 hard ceiling on convex-path jerk (lowered: smoother catch-up) -# Fast hand-back: once the plan leaves the gentle zone (no longer armed) but a soft gap is still open, -# close it at this FIRM jerk so the output catches the plan BEFORE braking gets firm -> no late-brake lag -# into the [-1.5,-1.0] band. Jerk-limited (not a snap), and never deeper than the plan, so not a grab. -ONSET_HANDBACK_JERK = {ECO: 2.2, SPORT: 3.0} # m/s^3 gap-close rate (lowered: gentler hand-back = less jounce/jerk) - -# Arm gates (conservative). Only shape genuinely gentle onsets; firm/deep onsets fall to the stock -# never-weaker slew (they SHOULD bite). Two independent safety layers against late braking: (1) the -# PREDICTIVE brake_need gate declines to start a gentle bite when a firmer brake is seen within 3s, so -# we don't soften ahead of one; (2) the fast hand-back (ONSET_HANDBACK_JERK) closes any open soft gap -# before the plan reaches firm braking. Together: 0 firm-band ([-1.5,-1.0]) lag on the verified windows. -SOFT_ONSET_MAX_BRAKE_NEED = 0.9 # do NOT soften if a firmer brake is predicted within 3s -SOFT_ONSET_MAX_INSTANT_ACCEL = -0.7 # m/s^2 stop softening (fast hand-back) once raw is this deep -# Sticky re-arm: once an onset goes firm (instantaneously too deep) it latches OFF; require this many -# consecutive released/flat frames before a NEW soft window may open, so lead/SnG jitter cannot re-arm -# the bite every few hundred ms (flicker guard). Controller runs at the model rate (DT_MDL = 0.05 s). -SOFT_ONSET_REARM_FRAMES = 10 # frames (~0.5 s at 20 Hz model rate) of release before re-arm 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 2175854063..c97e7a4b4c 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 @@ -11,18 +11,10 @@ import numpy as np import pytest from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController -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, 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 -# bounds how far it can lag, and it converges to the plan. The integrated velocity deficit over an -# armed brake stays under this conservative cap (no permanent offset; added stopping distance bounded). -_ONSET_VDEBT_BOUND = {ECO: 0.55, SPORT: 0.50} # raised for the firm-bypass onset jerk cap (still bounded -> no runaway) + STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, HARD_BRAKE_TARGET_ACCEL, OVERBITE_CAP, \ + STOP_PASSTHROUGH_V, AccelerationPersonality T_IDXS = [0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.25, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0] _EPS = 1e-6 @@ -57,6 +49,8 @@ def flat_traj(value): return [float(value)] * len(T_IDXS) +# --- Profiles / off==stock --------------------------------------------------- + def test_enum_source_parity(): assert (ECO, NORMAL, SPORT) == (AccelerationPersonality.eco, AccelerationPersonality.normal, AccelerationPersonality.sport) assert (PERSONALITY_MIN, PERSONALITY_MAX) == (0, 2) @@ -73,22 +67,19 @@ def test_disabled_forces_normal_and_stock_ceiling(): def test_disabled_passes_brake_through(): ctrl = make_controller(enabled=False) - for raw in (-1.5, -0.5, 0.0, 1.0): + for raw in (-3.0, -1.5, -0.5, 0.0, 1.0): out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) assert out == pytest.approx(raw, abs=_EPS) def test_normal_is_distinct_from_stock(): - # off==stock is enforced via the disabled path (see test_disabled_forces_normal_and_stock_ceiling), NOT by - # NORMAL==stock. So the enabled NORMAL tier is free to differ from stock -- and now does. + # off==stock is enforced via the disabled path, NOT by NORMAL==stock, so enabled NORMAL is free to differ. ctrl = make_controller(personality=NORMAL) assert ctrl.get_max_accel(0.0) != pytest.approx(np.interp(0.0, A_CRUISE_MAX_BP, STOCK_A_CRUISE_MAX_V)) assert ctrl.get_rise_rate() != STOCK_RISE_RATE def test_ceiling_ordering_eco_lt_normal_lt_sport(): - # All three tiers are distinct: ECO < NORMAL < SPORT at every speed (launch and cruise). Each launches - # promptly (peak + rise rate above stock), stepped by tier. eco, normal, sport = (make_controller(personality=p) for p in (ECO, NORMAL, SPORT)) for v in (0.0, 14.0, 25.0, 40.0): assert eco.get_max_accel(v) < normal.get_max_accel(v) < sport.get_max_accel(v) @@ -96,26 +87,119 @@ def test_ceiling_ordering_eco_lt_normal_lt_sport(): def test_rise_rate_ordering(): - # ECO rise == stock (prompt launch ramp) by design; SPORT firmer than both. - assert RISE_RATE[ECO] <= RISE_RATE[NORMAL] < RISE_RATE[SPORT] + assert RISE_RATE[ECO] < RISE_RATE[NORMAL] < RISE_RATE[SPORT] -def test_early_soft_braking_brakes_before_plan(): - ctrl = make_controller(personality=NORMAL) +# --- SAFETY: never weaker than the plan, hard brakes never delayed -------------- + +@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) +def test_never_weaker_than_plan_sustained(personality): + # Core safety invariant: on the brake side the output is NEVER weaker than the plan (only equal or deeper). + ctrl = make_controller(personality=personality) + for raw in [0.0, -0.2, -0.5, -0.9, -1.2, -1.5, -2.0] + [-2.0] * 20: + out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) + if raw < 0.0: + assert out <= raw + _EPS + + +@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) +def test_never_weaker_random_walk(personality): + rng = np.random.default_rng(0) + ctrl = make_controller(personality=personality) + for _ in range(500): + raw = float(rng.uniform(-2.5, 1.5)) + traj = flat_traj(raw - float(rng.uniform(0.0, 0.6))) + out = ctrl.smooth_target_accel(raw, traj, T_IDXS, should_stop=False) + if raw < 0.0: + assert out <= raw + _EPS + + +@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) +def test_hard_brake_passes_through_immediately(personality): + # Regression for route 00000466 near-crash: a sudden hard brake (plan steps deep) must reach FULL depth + # on the FIRST frame -- never rate-limited / delayed, or the car under-brakes into a closing lead. + ctrl = make_controller(personality=personality) + out = ctrl.smooth_target_accel(-3.5, flat_traj(-3.5), T_IDXS, should_stop=False) + assert out == pytest.approx(-3.5, abs=_EPS) + assert ctrl.bypassed() + + +def test_sudden_lead_no_brake_delay(): + # The exact 466 shape: cruising (plan +1.7, no brake) then a fast lead appears and the plan steps to max + # brake. The commanded brake must hit full depth immediately, not ramp in over time. + ctrl = make_controller(personality=ECO) + for _ in range(5): + ctrl.smooth_target_accel(1.7, flat_traj(1.7), T_IDXS, should_stop=False) # cruising, no lead + out = ctrl.smooth_target_accel(-3.5, flat_traj(-3.5), T_IDXS, should_stop=False) # lead appears + assert out == pytest.approx(-3.5, abs=_EPS) # full brake, zero delay + + +def test_should_stop_passes_through(): + ctrl = make_controller(personality=ECO) + out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=True) + assert out == pytest.approx(-1.0, abs=_EPS) + assert ctrl.bypassed() + + +def test_fcw_crash_passes_through(): + ctrl = make_controller(personality=ECO, crash_cnt=3) + out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False) + assert out == pytest.approx(-1.0, abs=_EPS) + assert ctrl.bypassed() + + +def test_blended_never_weaker(): + # Blended/e2e (stock_brake): never weaker than the plan (may anticipate via the never-weaker front-load). + ctrl = make_controller(personality=ECO) + 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 + + +# --- Anticipatory front-load (never weaker, capped) ------------------------------ + +def test_front_load_brakes_before_plan(): + # A deeper brake is predicted ahead (brake_need=1.0) while the live plan is still flat -> front-load + # brakes early (output goes negative), but the smooth branch keeps it never weaker than the plan. + ctrl = make_controller(personality=ECO) out = ctrl.smooth_target_accel(0.0, flat_traj(-1.0), T_IDXS, should_stop=False) assert out < 0.0 assert ctrl.smooth_active() assert ctrl.brake_need() == pytest.approx(1.0) +def test_front_load_anticipates_below_live_plan(): + # When the live plan is gently braking and a deeper brake is predicted, the front-load deepens below the + # live plan (anticipatory early brake), settling within OVERBITE_CAP of it. + ctrl = make_controller(personality=ECO) + out = 0.0 + for _ in range(20): + out = ctrl.smooth_target_accel(-0.2, flat_traj(-1.5), T_IDXS, should_stop=False) + assert out < -0.2 - _EPS # deeper than the live -0.2 plan + assert out >= -0.2 - OVERBITE_CAP - _EPS # but never more than the cap below it + + +def test_overbite_cap_limits_frontload_vs_live_plan(): + # Cut-in/merge: plan still wants throttle (+0.5) while a deep brake is predicted -> front-load may not + # settle more than OVERBITE_CAP below the live plan (no abrupt early over-bite). + ctrl = make_controller(personality=ECO) + traj = [0.5, 0.3, 0.0, -0.5, -1.5, -2.0] + [-2.0] * (len(T_IDXS) - 6) + out = 0.0 + for _ in range(10): + 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) + + +# --- Stop / low-speed neutrality ------------------------------------------------- + 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. + # Stop/creep regime (vEgo < STOP_PASSTHROUGH_V): braking is stock so the stop distance matches OFF. 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 + for raw in (-0.3, -1.0): 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 out == pytest.approx(raw, abs=_EPS) assert not ctrl.smooth_active() @@ -123,208 +207,31 @@ 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 + ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False) 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 + assert out < 1.5 # rise-rate limited (shaped) -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 - # at every speed -> the gentle-brake goal is not regressed. +def test_stop_imminent_passthrough_but_moving_follow_shapes(): + # Stop coming (plan speed -> ~0): stock passthrough (no coast-in). Slowing to a moving follow: front-load + # stays active so the early-brake goal is preserved. ctrl = make_controller(personality=ECO) stopping = [3.0, 2.0, 1.0, 0.4, 0.0] + [0.0] * (len(T_IDXS) - 5) out = ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False, speed_trajectory=stopping) assert not ctrl.smooth_active() - assert out == pytest.approx(-0.1, abs=_EPS) # stock passthrough into the stop, no softening - moving = [8.0] * len(T_IDXS) # slowing to a moving follow, not a stop + assert out == pytest.approx(-0.1, abs=_EPS) + moving = [8.0] * len(T_IDXS) ctrl.smooth_target_accel(-0.1, flat_traj(-1.0), T_IDXS, should_stop=False, speed_trajectory=moving) - assert ctrl.smooth_active() # gentle onset preserved (not stop-imminent) - - -@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) -def test_never_weaker_than_plan_sustained_closing(personality): - # NORMAL/off: strict never-weaker (route 000003da regression guard). ECO/SPORT: the convex onset may - # lag the plan during the bite, but the INTEGRATED velocity deficit vs the plan stays bounded. This - # sequence also crosses the -1.5 emergency bypass (bit-exact raw thereafter). - ctrl = make_controller(personality=personality) - vdebt = 0.0 - for raw in [0.0, -0.2, -0.5, -0.9, -1.2, -1.5] + [-1.5] * 40: - out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) - if personality == NORMAL: - assert out <= raw + _EPS - else: - vdebt = max(0.0, vdebt + (out - raw) * DT_MDL) - assert vdebt <= _ONSET_VDEBT_BOUND[personality] - - -@pytest.mark.parametrize("personality", [ECO, NORMAL, SPORT]) -def test_never_weaker_random_walk(personality): - # NORMAL strict never-weaker; ECO/SPORT integrated velocity deficit stays bounded even under an - # unrate-limited random plan (the deficit can never run away). - rng = np.random.default_rng(0) - ctrl = make_controller(personality=personality) - vdebt = 0.0 - for _ in range(500): - raw = float(rng.uniform(-1.9, 1.5)) - traj = flat_traj(raw - float(rng.uniform(0.0, 0.6))) - out = ctrl.smooth_target_accel(raw, traj, T_IDXS, should_stop=False) - if personality == NORMAL: - if raw < 0.0: - assert out <= raw + _EPS - else: - vdebt = max(0.0, vdebt + (out - raw) * DT_MDL) - assert vdebt <= _ONSET_VDEBT_BOUND[personality] - - -def test_normal_brake_bit_exact_vs_legacy_slew(): - # NORMAL must be byte-identical to the legacy constant-jerk slew + min(slewed,raw) on a deepening ramp. - ctrl = make_controller(personality=NORMAL) - jmax = BRAKE_DEEPENING_JERK[NORMAL] - last = 0.0 - for raw in [0.0, -0.1, -0.3, -0.45, -0.6, -0.6, -0.6, -0.5, -0.3, 0.0, 0.5]: - out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) - if raw <= last: # deepening: legacy step-limited then min(.,raw) - legacy = min(max(raw, last - jmax * DT_MDL), raw) if raw < 0.0 else max(raw, last - jmax * DT_MDL) - assert out == pytest.approx(legacy, abs=_EPS) - last = out - - -@pytest.mark.parametrize("personality", [ECO, SPORT]) -def test_convex_onset_gentle_bite(personality): - # At a real brake onset the plan deepens gradually, so the gap stays within ONSET_GAP_SOFT and the - # first deepening tick must use only the gentle initial jerk ONSET_JERK0 (the soft bite), NOT bite hard. - ctrl = make_controller(personality=personality) - ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False) # seed last=0 - raw = -0.5 * ONSET_GAP_SOFT[personality] # within the gap budget -> gentle bite, no catch-up - out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) - j_init = abs(out - 0.0) / DT_MDL # realized first-tick jerk == ONSET_JERK0 (gentle bite) - assert j_init == pytest.approx(ONSET_JERK0[personality], abs=1e-6) - assert out > raw # softened: shallower than the plan, NOT passed through - - -@pytest.mark.parametrize("personality", [ECO, SPORT]) -def test_convex_onset_velocity_deficit_bounded_and_converges(personality): - # Integrated velocity deficit vs plan over a sustained moderate brake stays bounded, and the controller - # CONVERGES to the plan (no permanent velocity offset -> added stopping distance is a bounded transient). - ctrl = make_controller(personality=personality) - vdebt = 0.0 - last = 0.0 - raw = -0.9 # armed (raw > SOFT_ONSET_MAX_INSTANT_ACCEL = -1.0) - for _ in range(500): - out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) - vdebt = max(0.0, vdebt + (out - raw) * DT_MDL) # accrue only shallower-than-plan deficit - last = out - assert vdebt <= _ONSET_VDEBT_BOUND[personality] - assert last == pytest.approx(raw, abs=1e-3) # converged to the plan, no permanent offset - - -@pytest.mark.parametrize("personality", [ECO, SPORT]) -def test_convex_onset_no_jerk_snap(personality): - # A gentle armed bite opens a soft gap; when the plan then deepens past the gentle zone the gap is - # closed by the FAST hand-back -- still jerk-limited, never a 1-frame snap to the plan. Max realized - # jerk anywhere on the convex path must not exceed the hand-back ceiling. - ctrl = make_controller(personality=personality) - ctrl.smooth_target_accel(0.0, flat_traj(0.0), T_IDXS, should_stop=False) - prev = 0.0 - worst = 0.0 - for raw in [-0.3] * 10 + [-0.9] * 40: # gentle onset, then deepen past the gentle zone - out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) - worst = max(worst, abs(out - prev) / DT_MDL) - prev = out - assert worst <= ONSET_HANDBACK_JERK[personality] + _EPS - - -def test_hard_brake_bypass(): - # Firm (non-crash) hard brake: the DEEPENING RATE is jerk-limited (no raw stock grab), but depth is - # never reduced (out never deeper than raw) and full depth is reached within a bounded ramp. - ctrl = make_controller(personality=ECO) - raw = HARD_BRAKE_TARGET_ACCEL - 0.5 # -2.0 - last = 0.0 - reached = False - for _ in range(40): - out = ctrl.smooth_target_accel(raw, flat_traj(raw), T_IDXS, should_stop=False) - assert ctrl.bypassed() - assert out >= raw - _EPS # never deeper than the plan - assert out <= last - HARD_BRAKE_ONSET_JERK * DT_MDL + _EPS # deepening rate capped - last = out - if abs(out - raw) < _EPS: - reached = True - break - assert reached # full plan depth reached - - -def test_hard_brake_onset_jerk_limited_vs_crash(): - # The firm bypass is rate-limited; a true emergency (crash_cnt>0 / FCW) is NOT -> instant full depth. - firm = make_controller(personality=ECO) - out_firm = firm.smooth_target_accel(-3.0, flat_traj(-3.0), T_IDXS, should_stop=False) - assert out_firm == pytest.approx(-HARD_BRAKE_ONSET_JERK * DT_MDL, abs=_EPS) # first tick capped from last=0 - crash = make_controller(personality=ECO, crash_cnt=3) - out_crash = crash.smooth_target_accel(-3.0, flat_traj(-3.0), T_IDXS, should_stop=False) - assert out_crash == pytest.approx(-3.0, abs=_EPS) # FCW: instant, never rate-limited - - -def test_should_stop_bypass(): - # should_stop firm brake: rate-limited onset, never deeper than plan, reaches full depth. - ctrl = make_controller(personality=ECO) - last = 0.0 - reached = False - for _ in range(40): - out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=True) - assert ctrl.bypassed() - assert out >= -1.0 - _EPS - last = out - if abs(out + 1.0) < _EPS: - reached = True - break - assert reached + assert ctrl.smooth_active() def test_disabled_hard_brake_is_instant_stock(): - # off == stock: a disabled controller must pass the hard brake straight through (no rate cap). ctrl = make_controller(enabled=False, personality=ECO) out = ctrl.smooth_target_accel(-3.0, flat_traj(-3.0), T_IDXS, should_stop=False) assert out == pytest.approx(-3.0, abs=_EPS) -def test_fcw_crash_cnt_bypass(): - ctrl = make_controller(personality=ECO, crash_cnt=3) - out = ctrl.smooth_target_accel(-1.0, flat_traj(-1.0), T_IDXS, should_stop=False) - assert out == pytest.approx(-1.0, abs=_EPS) - assert ctrl.bypassed() - - -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) - 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 - +# --- Misc ------------------------------------------------------------------------ def test_out_of_range_personality_clamps(): ctrl = AccelController(CP=SimpleNamespace(), mpc=SimpleNamespace(crash_cnt=0),