mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-10 20:33:12 +08:00
fix(long): ceiling rise-rate
This commit is contained in:
rav4kumar
@@ -5,21 +5,11 @@ 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 (ECO / NORMAL / SPORT). Tunes only MPC INPUTS, never the output:
|
||||
* positive-accel ceiling + speed-dependent per-cycle open-rate -> tier-scaled take-off from a stop
|
||||
(the open-rate is fast near v=0 so launch is never delayed, tapering to a steady-state rate at speed);
|
||||
* jerk-cost relaxation (scales the core MPC's jerk_factor) -> smooth accel/decel onset: near a stop, on
|
||||
any fresh accel<->decel direction change, when the tracked lead is itself braking hard, or when the gap
|
||||
is closing fast for any other reason (cut-in, ego overtaking a slower lead) -- the last one is the only
|
||||
proactive trigger keyed on an MPC INPUT (vRel) rather than a_ego's own realized sign flip, so it can
|
||||
soften the very first brake jab instead of only the recovery after it. The lead-keyed factors are
|
||||
TRANSIENT (dip on a fresh/escalating trigger, then ease back to 1.0 over a fixed window regardless of
|
||||
whether the raw trigger is still active) rather than a level-level pin -- a sustained low jerk_scale
|
||||
destabilizes the MPC's real-time-iteration re-solve into an oscillation (closed-loop-verified: a pinned
|
||||
0.6 factor produced a 30+ m/s^3 jerk whipsaw where stock stayed at 0), so only the onset gets softened,
|
||||
matching the "first jab" intent, never the sustained middle of a real braking episode;
|
||||
* add-only, speed-dependent follow-gap widen on the MPC t_follow -> earlier/gentler braking, roomier gap;
|
||||
* sticky should_stop hysteresis -> no stop-and-go gas-brake-gas-brake.
|
||||
Add-only gap => desired distance >= stock => braking >= stock. Disabled => stock everywhere (byte-stock).
|
||||
* positive-accel ceiling + its per-cycle open-rate -> tier-scaled take-off from a stop;
|
||||
* jerk-cost relaxation -> smooth accel/decel onset near a stop, on a fresh direction change, a
|
||||
hard-braking lead, or a fast-closing gap;
|
||||
* add-only, speed-dependent follow-gap widen on the MPC t_follow.
|
||||
Disabled => stock everywhere (byte-stock).
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
@@ -31,15 +21,14 @@ 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, STOCK_A_CRUISE_MAX_V, \
|
||||
RISE_RATE_BP, RISE_RATE_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, ONSET_DEADBAND, ONSET_RAMP_S, \
|
||||
ONSET_FLOOR, RELAX_RAMP_S, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, CLOSING_VREL_BP, CLOSING_FACTOR_V, \
|
||||
TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A
|
||||
RISE_RATE_BP, RISE_RATE_V, STOCK_RISE_RATE, RISE_RATE_LEAD_VREL_GATE, JERK_SCALE_BP, JERK_SCALE_V, \
|
||||
ONSET_DEADBAND, ONSET_RAMP_S, ONSET_FLOOR, RELAX_RAMP_S, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, \
|
||||
CLOSING_VREL_BP, CLOSING_FACTOR_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, \
|
||||
TF_SLEW_PER_S, TF_DECEL_HOLD_A
|
||||
|
||||
|
||||
class _OnsetRelax:
|
||||
# Detects a fresh accel<->decel direction change on aEgo (a real, causal signal -- never the MPC's own
|
||||
# solved target) and relaxes toward a tier-scaled floor immediately, then eases linearly back to 1.0 over
|
||||
# ONSET_RAMP_S. Feeds the MPC's cost weights for the cycles that follow; never touches this cycle's output.
|
||||
# Snaps to a tier-scaled floor on a fresh accel<->decel direction change, eases back to 1.0 over ONSET_RAMP_S.
|
||||
def __init__(self):
|
||||
self._prev_sign = 0
|
||||
self._ramp = 1.0
|
||||
@@ -64,15 +53,9 @@ class _OnsetRelax:
|
||||
|
||||
|
||||
class _TransientRelax:
|
||||
# Same shape as _OnsetRelax's ramp, but triggered by a level (a raw tier-scaled factor computed fresh each
|
||||
# cycle) instead of a sign flip: on a fresh dip (factor newly below 1.0) or a further escalation (factor
|
||||
# drops below the lowest value already seen this episode), snap down to it; otherwise ease back toward 1.0
|
||||
# over RELAX_RAMP_S REGARDLESS of whether the raw factor is still low. Without this, a raw factor that
|
||||
# stays pinned at its floor for the duration of a real, sustained closing/braking episode (not just the
|
||||
# first instant) keeps jerk_scale low for seconds at a time -- closed-loop-verified to destabilize the
|
||||
# MPC's real-time-iteration re-solve into an oscillation (30+ m/s^3 jerk vs 0 for the identical scenario
|
||||
# with the factor disabled). Ramping back after the initial dip keeps the "soften the first jab" intent
|
||||
# without holding a low jerk cost through the sustained middle of a real episode.
|
||||
# Same shape as _OnsetRelax, but triggered by a level instead of a sign flip: snap to a fresh/escalating
|
||||
# factor, then ease back to 1.0 over ramp_s regardless of whether the raw factor is still low. A pinned
|
||||
# floor destabilizes the MPC's re-solve over a sustained episode, so only the onset gets softened.
|
||||
def __init__(self):
|
||||
self._was_active = False
|
||||
self._episode_floor = 1.0
|
||||
@@ -113,6 +96,7 @@ class AccelController:
|
||||
self._onset_factor = 1.0
|
||||
self._lead_brake_factor = 1.0
|
||||
self._closing_factor = 1.0
|
||||
self._closing_on_lead = False
|
||||
self._read_params()
|
||||
|
||||
def _read_params(self) -> None:
|
||||
@@ -133,6 +117,7 @@ class AccelController:
|
||||
self._onset_factor = self._onset_relax.update(self._a_ego, ONSET_FLOOR[self._personality])
|
||||
self._lead_brake_factor = self._lead_brake_relax.update(self._get_lead_brake_factor(lead), RELAX_RAMP_S)
|
||||
self._closing_factor = self._closing_relax.update(self._get_closing_factor(lead), RELAX_RAMP_S)
|
||||
self._closing_on_lead = lead.status and lead.vRel < RISE_RATE_LEAD_VREL_GATE
|
||||
else:
|
||||
self._onset_relax.reset()
|
||||
self._lead_brake_relax.reset()
|
||||
@@ -140,6 +125,7 @@ class AccelController:
|
||||
self._onset_factor = 1.0
|
||||
self._lead_brake_factor = 1.0
|
||||
self._closing_factor = 1.0
|
||||
self._closing_on_lead = False
|
||||
|
||||
self._frame += 1
|
||||
|
||||
@@ -173,17 +159,13 @@ class AccelController:
|
||||
return float(np.interp(v_ego, A_CRUISE_MAX_BP, table))
|
||||
|
||||
def get_rise_rate(self, v_ego: float) -> float:
|
||||
# Disabled -> stock ceiling open-rate (off == stock, independent of the NORMAL profile).
|
||||
# Speed-dependent: fast near a stop (non-binding, no launch delay), tapering to the steady-state rate.
|
||||
if not self._enabled:
|
||||
# Disabled, or still closing on a lead -> stock ceiling open-rate.
|
||||
if not self._enabled or self._closing_on_lead:
|
||||
return STOCK_RISE_RATE
|
||||
return float(np.interp(v_ego, RISE_RATE_BP, RISE_RATE_V[self._personality]))
|
||||
|
||||
def get_jerk_scale(self, v_ego: float) -> float:
|
||||
# Disabled -> 1.0 -> byte-stock jerk cost. Enabled: takes the most-relaxed of four tier-scaled factors --
|
||||
# near a stop (v_ego), a fresh accel<->decel onset (any speed), a hard-braking lead, and a fast-closing
|
||||
# gap (any cause) -- each never exceeding 1.0 (stock), so this only ever relaxes jerk cost, never tightens
|
||||
# it beyond stock.
|
||||
# Most-relaxed of: near a stop, a fresh onset, a hard-braking lead, a fast-closing gap. Never > 1.0 (stock).
|
||||
if not self._enabled:
|
||||
return 1.0
|
||||
near_stop = float(np.interp(v_ego, JERK_SCALE_BP, JERK_SCALE_V[self._personality]))
|
||||
|
||||
@@ -21,43 +21,30 @@ PERSONALITY_MIN = min(AccelerationPersonality.schema.enumerants.values())
|
||||
PERSONALITY_MAX = max(AccelerationPersonality.schema.enumerants.values())
|
||||
|
||||
# --- Positive-accel ceiling (launch/cruise) + its upward open-rate ---------------------------------------
|
||||
# off == stock: get_max_accel/get_rise_rate fall back to the STOCK_* values (upstream get_max_accel table
|
||||
# and +0.05 ceiling slew), independent of the NORMAL profile so NORMAL is free to differ.
|
||||
# ACCEL_MAX (opendbc) hard-caps the ceiling at 2.0 m/s^2, so the launch knots are set at/below it.
|
||||
# Disabled -> falls back to STOCK_* (byte-stock). ACCEL_MAX (opendbc) hard-caps the ceiling at 2.0 m/s^2.
|
||||
A_CRUISE_MAX_BP = [0., 10., 25., 40.] # m/s (matches upstream A_CRUISE_MAX_BP)
|
||||
STOCK_A_CRUISE_MAX_V = [1.6, 1.2, 0.8, 0.6] # upstream A_CRUISE_MAX_VALS -> off == byte-stock ceiling
|
||||
STOCK_A_CRUISE_MAX_V = [1.6, 1.2, 0.8, 0.6] # upstream A_CRUISE_MAX_VALS
|
||||
STOCK_RISE_RATE = 0.05 # upstream ceiling open-rate (m/s^2 per cycle)
|
||||
A_CRUISE_MAX_V = {
|
||||
ECO: [1.55, 0.75, 0.35, 0.20], # responsive off the line, LAZY at highway speed (mileage)
|
||||
NORMAL: [2.00, 1.40, 0.95, 0.70], # brisk launch, balanced cruise
|
||||
SPORT: [2.00, 1.70, 1.20, 0.90], # strong launch (ACCEL_MAX caps the 0 m/s knot), assertive cruise
|
||||
ECO: [1.55, 0.75, 0.35, 0.20],
|
||||
NORMAL: [2.00, 1.40, 0.95, 0.70],
|
||||
SPORT: [2.00, 1.70, 1.20, 0.90],
|
||||
}
|
||||
# Ceiling open-rate: how fast the accel ceiling may rise per cycle, speed-dependent (decoupled from the
|
||||
# ceiling magnitude above). Near a stop (v=0) the rate is set high enough to be non-binding within ~2 cycles
|
||||
# (DT_MDL=0.05s @ 20Hz) so launch is never delayed waiting on the ceiling to open, regardless of personality.
|
||||
# By the v=5 knot the rate settles to the telemetry-verified steady-state value (unchanged from before) so
|
||||
# cruise/resume behavior at speed is preserved exactly. The MPC's own jerk/a_change cost still smooths the
|
||||
# actual accel.
|
||||
# Ceiling open-rate (m/s^2 per cycle): fast near v=0 so launch isn't delayed, tapers by v=5.
|
||||
RISE_RATE_BP = [0., 5.] # m/s
|
||||
RISE_RATE_V = {
|
||||
ECO: [0.80, 0.07],
|
||||
NORMAL: [1.00, 0.16],
|
||||
SPORT: [1.20, 0.24],
|
||||
}
|
||||
# The fast near-stop rise-rate above only looks at v_ego, so it also fires while still closing on a lead
|
||||
# that hasn't cleared yet, letting the ceiling snap open faster than the actual situation calls for. Fall
|
||||
# back to the stock rate whenever still closing (vRel this negative or more).
|
||||
RISE_RATE_LEAD_VREL_GATE = -0.5 # m/s
|
||||
|
||||
# --- Launch jerk-cost relaxation (MPC INPUT: scales the core MPC's own jerk_factor) -----------------------
|
||||
# The ceiling open-rate above stops binding once the ceiling is already open (the normal case while
|
||||
# following a lead), so it can't pace the launch ramp. That ramp is paced by A_CHANGE_COST/J_EGO_COST via
|
||||
# upstream get_jerk_factor(personality) -- stock 1.0 for relaxed/standard, 0.5 for aggressive. JERK_SCALE
|
||||
# multiplies into that same upstream factor (same lever stock's aggressive tier already uses), bounded to
|
||||
# near a stop and ramped back to 1.0 (stock) by cruise speed.
|
||||
# The v=0 knot is NOT monotone with personality -- verified via a closed-loop MPC harness (dead stop +
|
||||
# departing lead, 3 scenarios): 0.60/0.45 both measurably beat stock 1.0 (0.3-0.65s faster to cross the
|
||||
# should_stop 0.1 m/s^2 gate, 0 solver resets), but pushing lower is NOT "more relaxed = faster" -- 0.30 came
|
||||
# back SLOWER than stock in all 3 scenarios (the MPC back-loads the ramp instead of front-loading it once
|
||||
# A_CHANGE_COST/J_EGO_COST get too cheap to bother avoiding), and below ~0.15 the solver itself destabilizes
|
||||
# (46-68% QP resets). SPORT's knot is pinned to the verified-good value (tied with NORMAL) rather than pushed
|
||||
# lower for tier-consistency -- lower is not safe or effective here, unlike every other tier table in this file.
|
||||
# Bounded near a stop, ramped back to 1.0 (stock) by cruise speed. v=0 knot closed-loop verified; SPORT tied
|
||||
# to NORMAL rather than pushed lower (lower destabilizes the solver at this knot).
|
||||
JERK_SCALE_BP = [0., 5.] # m/s
|
||||
JERK_SCALE_V = {
|
||||
ECO: [0.60, 1.0],
|
||||
@@ -65,29 +52,17 @@ JERK_SCALE_V = {
|
||||
SPORT: [0.45, 1.0],
|
||||
}
|
||||
|
||||
# --- Onset jerk-cost relaxation (MPC INPUT: general accel<->decel-direction change, not just launch) ------
|
||||
# The v_ego-indexed table above only relaxes near a stop. This relaxes on ANY fresh accel<->decel direction
|
||||
# change (aEgo crossing the deadband in a new sign) regardless of speed: drop to a tier-scaled floor right
|
||||
# away, then ease linearly back to 1.0 (stock) over ONSET_RAMP_S. Same shape as the launch ramp, general to
|
||||
# any onset (e.g. releasing off a lead, starting to brake). Disabled -> 1.0.
|
||||
# --- Onset jerk-cost relaxation (MPC INPUT: any accel<->decel direction change, not just launch) ----------
|
||||
ONSET_DEADBAND = 0.15 # m/s^2: ignore aEgo noise this small around a zero-crossing
|
||||
ONSET_RAMP_S = 0.4 # s: ease back to stock over this long
|
||||
ONSET_FLOOR = {ECO: 0.75, NORMAL: 0.65, SPORT: 0.50}
|
||||
|
||||
# --- Transient-relax ramp for the two LEVEL-triggered factors below (lead-braking, closing-rate) -----------
|
||||
# Both factors are eased through _TransientRelax (accel_controller.py), not applied as a level-pinned floor:
|
||||
# closed-loop-verified (selfdrive/test/longitudinal_maneuvers/plant.py-based) that holding jerk_scale at a
|
||||
# fixed low value for the duration of a sustained closing/braking episode -- not just its first instant --
|
||||
# destabilizes the MPC's real-time-iteration re-solve into an oscillation (30+ m/s^3 jerk vs 0 with the
|
||||
# factor disabled, for an identical scenario). Same ramp duration as ONSET_RAMP_S -- same "soften the first
|
||||
# jab, then get out of the way" shape -- kept as its own constant since the two mechanisms are conceptually
|
||||
# distinct (direction-change vs a proactive level) and may need to diverge under future tuning.
|
||||
RELAX_RAMP_S = 0.4 # s: ease back to stock over this long, regardless of whether the raw factor is still low
|
||||
# Ramp shared by the two level-triggered factors below (lead-braking, closing-rate): a sustained pinned
|
||||
# floor destabilizes the MPC's re-solve, so both ease back to stock over this window instead.
|
||||
RELAX_RAMP_S = 0.4
|
||||
|
||||
# --- Lead-braking jerk-cost relaxation (MPC INPUT: react faster to a hard-braking lead) --------------------
|
||||
# When the tracked lead is itself decelerating hard, relax jerk cost so the MPC's reaction isn't paced by a
|
||||
# jerk budget tuned for routine following. No lead, or lead not braking -> 1.0. Disabled -> 1.0.
|
||||
LEAD_BRAKE_ALEAD_BP = [-3.0, -0.5] # m/s^2, lead's own aLeadK (ascending, as np.interp requires)
|
||||
LEAD_BRAKE_ALEAD_BP = [-3.0, -0.5] # m/s^2, lead's own aLeadK
|
||||
LEAD_BRAKE_FACTOR_V = {
|
||||
ECO: [0.75, 1.0],
|
||||
NORMAL: [0.60, 1.0],
|
||||
@@ -95,13 +70,7 @@ LEAD_BRAKE_FACTOR_V = {
|
||||
}
|
||||
|
||||
# --- Closing-rate jerk-cost relaxation (MPC INPUT: react faster to a fast-closing gap, any cause) ----------
|
||||
# Complements LEAD_BRAKE_FACTOR_V, which keys off the LEAD's own deceleration: a gap can close quickly for
|
||||
# reasons aLeadK never reflects (a cut-in, or ego simply catching up faster than the lead is slowing). Onset
|
||||
# relax (above) only reacts the cycle AFTER a_ego has already crossed its deadband -- reactive on a realized
|
||||
# signal, so it structurally can't soften the very first jab into a fresh, fast-closing gap. vRel is an MPC
|
||||
# INPUT (causal, known before any brake is commanded), so keying off it directly closes that gap. No lead, or
|
||||
# not closing past the gate -> 1.0. Disabled -> 1.0.
|
||||
CLOSING_VREL_BP = [-6.0, -1.5] # m/s, closing rate (negative = closing), ascending for np.interp
|
||||
CLOSING_VREL_BP = [-6.0, -1.5] # m/s, closing rate (negative = closing)
|
||||
CLOSING_FACTOR_V = {
|
||||
ECO: [0.75, 1.0],
|
||||
NORMAL: [0.60, 1.0],
|
||||
@@ -109,14 +78,9 @@ CLOSING_FACTOR_V = {
|
||||
}
|
||||
|
||||
# --- Follow-gap widen (add-only, fed to the MPC t_follow) ------------------------------------------------
|
||||
# Add a small speed-dependent widen to the stock t_follow (the driver's gap-button value). Wider gap ->
|
||||
# MPC brakes earlier + gentler onto a slowing lead and settles a roomier cruise gap. Invariants:
|
||||
# * add-only -> desired distance >= stock -> braking >= stock;
|
||||
# * zero below TF_WIDEN_V_BP[0] -> low-speed & standstill gap stay stock (stock stop distance preserved);
|
||||
# * slewed per cycle -> no rubber-band; decel-hold -> gap won't shrink while braking (stays committed).
|
||||
TF_WIDEN_V_BP = [14.0, 28.0] # m/s: widen ramps in across this band, flat above
|
||||
TF_WIDEN_BASE_V = [0.0, 0.30] # s: base follow-time added at the band ends (pre-tier)
|
||||
TF_WIDEN_TIER = {ECO: 1.30, NORMAL: 1.00, SPORT: 0.50} # ECO roomiest/smoothest, SPORT tightest/snappiest
|
||||
TF_WIDEN_MAX = 0.45 # s: absolute cap on the added gap (never explodes)
|
||||
TF_WIDEN_TIER = {ECO: 1.30, NORMAL: 1.00, SPORT: 0.50}
|
||||
TF_WIDEN_MAX = 0.45 # s: absolute cap on the added gap
|
||||
TF_SLEW_PER_S = 0.50 # s per second: max rate the widen may open/close
|
||||
TF_DECEL_HOLD_A = -0.20 # m/s^2: at/below this a_ego (braking) the widen won't shrink
|
||||
TF_DECEL_HOLD_A = -0.20 # m/s^2: at/below this a_ego the widen won't shrink
|
||||
|
||||
@@ -131,6 +131,26 @@ def test_rise_rate_fast_near_stop_tapers_to_steady_state():
|
||||
assert ctrl.get_rise_rate(20.0) == pytest.approx(steady_state) # flat above the v=5 knot
|
||||
|
||||
|
||||
def test_rise_rate_falls_back_to_stock_while_closing_on_a_lead():
|
||||
ctrl = make_controller(personality=NORMAL)
|
||||
ctrl.update(make_sm(v_ego=2.0, lead=make_lead(status=True, vRel=-2.0)))
|
||||
assert ctrl.get_rise_rate(2.0) == STOCK_RISE_RATE
|
||||
|
||||
|
||||
def test_rise_rate_unaffected_by_a_departing_or_absent_lead():
|
||||
ctrl = make_controller(personality=NORMAL)
|
||||
ctrl.update(make_sm(v_ego=2.0, lead=make_lead(status=True, vRel=0.5)))
|
||||
assert ctrl.get_rise_rate(2.0) != STOCK_RISE_RATE
|
||||
ctrl.update(make_sm(v_ego=2.0, lead=make_lead(status=False)))
|
||||
assert ctrl.get_rise_rate(2.0) != STOCK_RISE_RATE
|
||||
|
||||
|
||||
def test_rise_rate_lead_gate_disabled_is_still_stock():
|
||||
ctrl = make_controller(enabled=False, personality=SPORT)
|
||||
ctrl.update(make_sm(v_ego=2.0, lead=make_lead(status=True, vRel=-2.0)))
|
||||
assert ctrl.get_rise_rate(2.0) == STOCK_RISE_RATE
|
||||
|
||||
|
||||
def test_normal_is_distinct_from_stock():
|
||||
nrm = make_controller(personality=NORMAL)
|
||||
# enabled NORMAL differs from stock (so NORMAL is a real profile, not a stock alias)
|
||||
|
||||
Reference in New Issue
Block a user