diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index e70f6fa39a..a6ae75ab30 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -7,6 +7,8 @@ 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, and when the tracked lead is itself braking hard; * 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). @@ -21,8 +23,36 @@ 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, 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, JERK_SCALE_BP, JERK_SCALE_V, ONSET_DEADBAND, ONSET_RAMP_S, \ + ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_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. + def __init__(self): + self._prev_sign = 0 + self._ramp = 1.0 + + def reset(self) -> None: + self._prev_sign = 0 + self._ramp = 1.0 + + def update(self, a_ego: float, floor: float) -> float: + sign = 0 + if a_ego > ONSET_DEADBAND: + sign = 1 + elif a_ego < -ONSET_DEADBAND: + sign = -1 + + if sign != 0 and sign != self._prev_sign: + self._ramp = floor + self._prev_sign = sign + else: + self._ramp = min(1.0, self._ramp + (1.0 - floor) * (DT_MDL / ONSET_RAMP_S)) + return self._ramp class AccelController: @@ -36,6 +66,9 @@ class AccelController: self._a_ego = 0.0 self._widen = 0.0 # current slewed follow-gap widen (s), add-only self._t_follow = 0.0 # last t_follow handed to the MPC (telemetry) + self._onset_relax = _OnsetRelax() + self._onset_factor = 1.0 + self._lead_brake_factor = 1.0 self._read_params() def _read_params(self) -> None: @@ -50,11 +83,28 @@ class AccelController: self._read_params() self._v_ego = float(sm['carState'].vEgo) self._a_ego = float(sm['carState'].aEgo) + + if self._enabled: + self._onset_factor = self._onset_relax.update(self._a_ego, ONSET_FLOOR[self._personality]) + self._lead_brake_factor = self._get_lead_brake_factor(sm['radarState'].leadOne) + else: + self._onset_relax.reset() + self._onset_factor = 1.0 + self._lead_brake_factor = 1.0 + self._frame += 1 + def _get_lead_brake_factor(self, lead) -> float: + if not lead.status: + return 1.0 + return float(np.interp(lead.aLeadK, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V[self._personality])) + def reset(self) -> None: # Drop the accumulated widen (e.g. on disengage / standstill re-init) so it re-ramps cleanly. self._widen = 0.0 + self._onset_relax.reset() + self._onset_factor = 1.0 + self._lead_brake_factor = 1.0 def get_max_accel(self, v_ego: float) -> float: # Disabled -> stock ceiling (off == stock, independent of the NORMAL profile so NORMAL is free to differ). @@ -69,11 +119,13 @@ class AccelController: 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: relaxes the core MPC's jerk_factor near a stop - # (tier-scaled), ramping back to 1.0 (stock) by the v=5 knot so cruise/follow jerk is unchanged. + # Disabled -> 1.0 -> byte-stock jerk cost. Enabled: takes the most-relaxed of three tier-scaled factors + # -- near a stop (v_ego), a fresh accel<->decel onset (any speed), and a hard-braking lead -- each never + # exceeding 1.0 (stock), so this only ever relaxes jerk cost, never tightens it beyond stock. if not self._enabled: return 1.0 - return float(np.interp(v_ego, JERK_SCALE_BP, JERK_SCALE_V[self._personality])) + near_stop = float(np.interp(v_ego, JERK_SCALE_BP, JERK_SCALE_V[self._personality])) + return min(near_stop, self._onset_factor, self._lead_brake_factor) def get_t_follow(self, t_follow: float, v_ego: float) -> float: # MPC t_follow hook. Adds a slewed, decel-held, speed-dependent comfort widen on top of the stock diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 25e1ecb1ef..46f50b3521 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -58,6 +58,25 @@ JERK_SCALE_V = { SPORT: [0.30, 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_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} + +# --- 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_FACTOR_V = { + ECO: [0.75, 1.0], + NORMAL: [0.60, 1.0], + SPORT: [0.45, 1.0], +} + # --- 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: 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 0ae1aaa1a8..f6baf3457d 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 @@ -19,8 +19,9 @@ from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \ ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE_V, \ - STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, \ - TF_WIDEN_TIER, TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A, AccelerationPersonality + STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, ONSET_DEADBAND, ONSET_RAMP_S, \ + ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, \ + TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A, AccelerationPersonality _EPS = 1e-6 _TF_STOCK = 1.45 # a representative stock t_follow (standard personality); the widen is add-only on top @@ -41,8 +42,13 @@ class FakeParams: self.store[key] = val -def make_sm(v_ego=20.0, a_ego=0.0): - return {'carState': SimpleNamespace(vEgo=v_ego, aEgo=a_ego)} +def make_lead(status=False, aLeadK=0.0): + return SimpleNamespace(status=status, aLeadK=aLeadK) + + +def make_sm(v_ego=20.0, a_ego=0.0, lead=None): + return {'carState': SimpleNamespace(vEgo=v_ego, aEgo=a_ego), + 'radarState': SimpleNamespace(leadOne=lead or make_lead())} def make_controller(enabled=True, personality=NORMAL): @@ -178,6 +184,114 @@ def test_jerk_scale_table_matches_constants(): assert ctrl.get_jerk_scale(v) == pytest.approx(np.interp(v, JERK_SCALE_BP, JERK_SCALE_V[personality])) +# --- onset relax: fresh accel<->decel direction change, any speed ------------------------------------------ + +def test_onset_disabled_is_stock(): + ctrl = make_controller(enabled=False, personality=SPORT) + ctrl.update(make_sm(v_ego=20.0, a_ego=2.0)) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) + + +def test_onset_no_effect_at_steady_state(): + ctrl = make_controller(personality=NORMAL) + for _ in range(10): + ctrl.update(make_sm(v_ego=20.0, a_ego=0.0)) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) # within the deadband, no direction to flip from + + +def test_onset_drops_to_floor_on_fresh_direction_change(): + ctrl = make_controller(personality=NORMAL) + ctrl.update(make_sm(v_ego=20.0, a_ego=1.0)) # establish "accelerating" + ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0)) # fresh flip to decelerating + assert ctrl.get_jerk_scale(20.0) == pytest.approx(ONSET_FLOOR[NORMAL]) + + +def test_onset_eases_back_to_stock_over_ramp_s(): + ctrl = make_controller(personality=NORMAL) + ctrl.update(make_sm(v_ego=20.0, a_ego=1.0)) + ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0)) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(ONSET_FLOOR[NORMAL]) + n = int(ONSET_RAMP_S / DT_MDL) + for _ in range(n): + ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0)) # sustained decel, no further flip + assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0, abs=1e-3) + + +def test_onset_never_relaxes_below_its_own_floor(): + ctrl = make_controller(personality=SPORT) + ctrl.update(make_sm(v_ego=20.0, a_ego=1.0)) + for _ in range(50): + ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0 if _ % 2 == 0 else -1.2)) # repeated same-direction wiggle + assert ctrl.get_jerk_scale(20.0) >= ONSET_FLOOR[SPORT] - _EPS + + +def test_onset_tier_ordering(): + ordered = [] + for personality in (ECO, NORMAL, SPORT): + ctrl = make_controller(personality=personality) + ctrl.update(make_sm(v_ego=20.0, a_ego=1.0)) + ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0)) + ordered.append(ctrl.get_jerk_scale(20.0)) + assert ordered[2] < ordered[1] < ordered[0] # SPORT relaxes most, ECO least + + +def test_onset_ignores_deadband_noise(): + ctrl = make_controller(personality=NORMAL) + ctrl.update(make_sm(v_ego=20.0, a_ego=0.3)) + for _ in range(10): + ctrl.update(make_sm(v_ego=20.0, a_ego=ONSET_DEADBAND - 0.02)) # tiny wiggle, never crosses -deadband + assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0, abs=1e-3) + + +# --- lead-braking relax: hard-braking lead relaxes jerk cost regardless of speed --------------------------- + +def test_lead_brake_no_lead_is_stock(): + ctrl = make_controller(personality=SPORT) + ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=False, aLeadK=-5.0))) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) + + +def test_lead_brake_relaxes_with_lead_decel(): + for personality, floor in ((ECO, 0.75), (NORMAL, 0.60), (SPORT, 0.45)): + ctrl = make_controller(personality=personality) + ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, aLeadK=-3.0))) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(floor) + + +def test_lead_brake_gentle_lead_decel_is_stock(): + ctrl = make_controller(personality=SPORT) + ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, aLeadK=-0.2))) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) # above the -0.5 gate -> no relax + + +def test_lead_brake_matches_constants_table(): + for personality in (ECO, NORMAL, SPORT): + ctrl = make_controller(personality=personality) + for a_lead in (-0.5, -1.5, -3.0): + ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, aLeadK=a_lead))) + expected = np.interp(a_lead, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V[personality]) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(expected) + + +# --- combined: get_jerk_scale takes the most-relaxed of all three factors ---------------------------------- + +def test_combined_takes_most_relaxed_factor(): + ctrl = make_controller(personality=NORMAL) + ctrl.update(make_sm(v_ego=20.0, a_ego=1.0)) + # onset flip (floor=0.65) + hard-braking lead (floor=0.60 at aLeadK=-3.0) -> min of the two, near-stop is 1.0 at v=20 + ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0, lead=make_lead(status=True, aLeadK=-3.0))) + assert ctrl.get_jerk_scale(20.0) == pytest.approx(min(ONSET_FLOOR[NORMAL], LEAD_BRAKE_FACTOR_V[NORMAL][0])) + + +def test_reset_clears_onset_and_lead_brake_state(): + ctrl = make_controller(personality=SPORT) + ctrl.update(make_sm(v_ego=20.0, a_ego=1.0)) + ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0, lead=make_lead(status=True, aLeadK=-3.0))) + assert ctrl.get_jerk_scale(20.0) < 1.0 - _EPS + ctrl.reset() + assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0) + + # --- t_follow: add-only speed widen ----------------------------------------------------------------------- def test_t_follow_zero_below_gate():