mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-10-01 01:43:41 +08:00
fix(long): remove onset jerk-cap
This commit is contained in:
rav4kumar
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
+116
-209
@@ -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),
|
||||
|
||||
Reference in New Issue
Block a user