diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 45c8f595aa..5c7766ed02 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -311,8 +311,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { enabled @0 :Bool; active @1 :Bool; profile @2 :Profile; - tFollowMultiplier @3 :Float32; - + reserved3 @3 :Void; enum Profile { eco @0; normal @1; diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 324af4b92e..0a0722fbc6 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -307,10 +307,8 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, personality=log.LongitudinalPersonality.standard, t_follow_multiplier=None): + def update(self, radarstate, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) - if t_follow_multiplier is not None: - t_follow *= t_follow_multiplier lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 183a43b0cf..dc6c164728 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -135,8 +135,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality, - t_follow_multiplier=self.get_t_follow_multiplier(sm, v_ego)) + self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) diff --git a/openpilot/selfdrive/ui/layouts/settings/toggles.py b/openpilot/selfdrive/ui/layouts/settings/toggles.py index f3d0edd3dd..a3b52e8a26 100644 --- a/openpilot/selfdrive/ui/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/layouts/settings/toggles.py @@ -28,8 +28,8 @@ DESCRIPTIONS = { "your steering wheel distance button." ), "AccelPersonalityEnabled": tr_noop( - "Sets your preferred acceleration ceiling by profile, and gives extra following distance when a lead is braking for an earlier, " - "smoother response. Stock braking and stopping logic remain in control at all times." + "Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain " + + "independent of this setting." ), "AccelPersonality": tr_noop( "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles." diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py index 5450f26d82..64e050b748 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -31,31 +31,12 @@ MIN_ACCEL_BREAKPOINTS = [3., 4.5, 7., 9., 25.] ACCEL_SMOOTH_ALPHA = 0.90 DECEL_SMOOTH_ALPHA = 0.40 -LEAD_GAP_WIDEN_PROFILES = { - AccelProfile.eco: 0.30, - AccelProfile.normal: 0.20, - AccelProfile.sport: 0.10, -} -LEAD_DECEL_FOR_MAX_WIDEN = 3.0 # m/s^2, lead decel (aLeadK) that saturates the widen amount -# aLeadK is a differentiated, filtered estimate -- it can lag several tenths of a second behind -# the lead actually closing. vRel is measured directly every frame, so closing speed alone (even -# before aLeadK has caught up) can independently trigger the same widen. -LEAD_CLOSING_FOR_MAX_WIDEN = 5.0 # m/s, closing speed (-vRel) that alone saturates the widen amount -GAP_WIDEN_ONSET_ALPHA = 0.12 -GAP_WIDEN_RELEASE_ALPHA = 0.08 -# Taper out below city speed so the lever only shapes higher-speed anticipation. -GAP_WIDEN_TAPER_LOW_SPEED = 3.0 # m/s, widen fully tapered out at/below this speed -GAP_WIDEN_TAPER_HIGH_SPEED = 8.0 # m/s, widen fully active at/above this speed - - class AccelController: def __init__(self): self.params = Params() self.frame = 0 self.last_max_accel = 2.0 self.last_min_accel = -0.01 - self.last_t_follow_widen = 0.0 - self._last_t_follow_multiplier = 1.0 self.first_run = True self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params) self._enabled = self.params.get_bool("AccelPersonalityEnabled") @@ -91,20 +72,3 @@ class AccelController: self.last_min_accel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel self.last_min_accel = min(self.last_min_accel, self.last_max_accel - 0.1) return float(self.last_min_accel) - - def get_t_follow_multiplier(self, lead_present: bool, lead_accel: float, v_ego: float, lead_v_rel: float = 0.0) -> float: - max_widen = LEAD_GAP_WIDEN_PROFILES[self._profile] - decel_widen = min(-lead_accel / LEAD_DECEL_FOR_MAX_WIDEN, 1.0) if lead_accel < 0.0 else 0.0 - closing_widen = min(-lead_v_rel / LEAD_CLOSING_FOR_MAX_WIDEN, 1.0) if lead_v_rel < 0.0 else 0.0 - target_widen = max(decel_widen, closing_widen) * max_widen if lead_present else 0.0 - - alpha = GAP_WIDEN_ONSET_ALPHA if target_widen > self.last_t_follow_widen else GAP_WIDEN_RELEASE_ALPHA - self.last_t_follow_widen += alpha * (target_widen - self.last_t_follow_widen) - - taper = np.clip((v_ego - GAP_WIDEN_TAPER_LOW_SPEED) / (GAP_WIDEN_TAPER_HIGH_SPEED - GAP_WIDEN_TAPER_LOW_SPEED), 0.0, 1.0) - self._last_t_follow_multiplier = 1.0 + self.last_t_follow_widen * taper - return self._last_t_follow_multiplier - - @property - def t_follow_multiplier(self) -> float: - return self._last_t_follow_multiplier diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py index abce01e9dd..bceeffde65 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -4,20 +4,16 @@ 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. -Scope is deliberately narrow: a v_ego-keyed acceleration ceiling and decel floor per -profile, plus one pre-solve lead-follow lever (widening the MPC's own t_follow when a -lead is braking). The floor only ever softens the no-lead cruise candidate (slowing for -a lower cruise speed, a curve, a speed limit) -- it is excluded during forceDecel and -e2e, and min() against the untouched mpc_accel candidate means a real lead can always -still force full ACCEL_MIN braking regardless. Lead-relevance checks, an SLC-shaped -floor beyond that, and controller-internal Params writes are NOT ported from the -reference designs this was built from - do not backfill them here without revisiting -scope. +Scope is deliberately narrow: a v_ego-keyed acceleration ceiling and cruise-deceleration +floor per profile. The controller does not modify lead following distance or the MPC lead +candidate. The floor only ever softens the no-lead cruise candidate (slowing for a lower +cruise speed, a curve, or a speed limit); it is excluded during forceDecel and e2e, and +min() against the untouched MPC candidate means a real lead can always still force full +ACCEL_MIN braking. -Ceiling vs floor apply on different policies: ACC (non-e2e) uses the controller's -ceiling AND floor; blended (e2e) uses the controller's ceiling but always the stock -floor (A_CRUISE_MIN), never the controller's -- final, don't try to make the floor -work under blended again. +Ceiling vs floor apply on different policies: ACC (non-e2e) uses the controller's ceiling +and floor; blended (e2e) uses the controller's ceiling but always the stock floor +(A_CRUISE_MIN). """ import unittest @@ -27,8 +23,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import ( - AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES, - LEAD_GAP_WIDEN_PROFILES, LEAD_DECEL_FOR_MAX_WIDEN, LEAD_CLOSING_FOR_MAX_WIDEN, + AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_PROFILES, ) @@ -198,147 +193,19 @@ class TestOffEqualsStock(OpenpilotTestCase): # controller's floor -- this is the "acc policy = controller min+max, blended policy = # controller max + stock min" split, final per product decision. from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel, A_CRUISE_MIN - args = dict(v_cruise=-100.0, v_ego=20.0, a_cruise_prev=0.0, angle_steers=0.0, CP=_fake_cp(), - dt=DT_MDL, accel_coast=1.0, allow_throttle=True) + args = {"v_cruise": -100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(), + "dt": DT_MDL, "accel_coast": 1.0, "allow_throttle": True} target = get_cruise_accel(True, **args, min_accel_override=-0.3) self.assertAlmostEqual(target, A_CRUISE_MIN, places=6) def test_blended_max_accel_uses_controller_override(self): from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel - args = dict(v_cruise=100.0, v_ego=20.0, a_cruise_prev=0.0, angle_steers=0.0, CP=_fake_cp(), - dt=DT_MDL, accel_coast=1.0, allow_throttle=True) + args = {"v_cruise": 100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(), + "dt": DT_MDL, "accel_coast": 1.0, "allow_throttle": True} target = get_cruise_accel(True, **args, max_accel_override=0.4) self.assertAlmostEqual(target, 0.4, places=6) -class TestLeadGapWiden(OpenpilotTestCase): - def setUp(self): - self.params = Params() - self.params.put_bool("AccelPersonalityEnabled", True, block=True) - self.params.put("AccelPersonality", AccelProfile.normal, block=True) - self.controller = AccelController() - - def test_no_lead_never_widens(self): - for _ in range(50): - multiplier = self.controller.get_t_follow_multiplier(lead_present=False, lead_accel=-5.0, v_ego=20.0) - self.assertEqual(multiplier, 1.0) - - def test_accelerating_lead_never_widens(self): - for _ in range(50): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=1.5, v_ego=20.0) - self.assertEqual(multiplier, 1.0) - - def test_braking_lead_widens_and_saturates(self): - for _ in range(200): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, v_ego=20.0) - self.assertAlmostEqual(multiplier, 1.0 + LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal], places=2) - - def test_closing_lead_widens_even_with_zero_lead_accel(self): - # aLeadK is a laggy, differentiated estimate -- vRel (measured directly) must be able to - # trigger the same widen on its own, before aLeadK has caught up to a real closing event. - for _ in range(200): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0, - lead_v_rel=-LEAD_CLOSING_FOR_MAX_WIDEN * 2) - self.assertAlmostEqual(multiplier, 1.0 + LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal], places=2) - - def test_opening_lead_v_rel_never_widens(self): - for _ in range(50): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0, lead_v_rel=3.0) - self.assertEqual(multiplier, 1.0) - - def test_widen_takes_whichever_signal_is_more_urgent(self): - # a laggy aLeadK=0 (not yet updated) shouldn't suppress an already-strong closing-rate signal - for _ in range(200): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=0.0, v_ego=20.0, - lead_v_rel=-LEAD_CLOSING_FOR_MAX_WIDEN * 2) - from_v_rel_only = multiplier - for _ in range(200): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, - v_ego=20.0, lead_v_rel=0.0) - from_a_lead_only = multiplier - self.assertAlmostEqual(from_v_rel_only, from_a_lead_only, places=2) - - def test_lead_v_rel_default_matches_pre_existing_callers(self): - # regression guard: existing call sites that don't pass lead_v_rel must be unaffected - controller_a, controller_b = AccelController(), AccelController() - for _ in range(50): - a = controller_a.get_t_follow_multiplier(lead_present=True, lead_accel=-1.0, v_ego=20.0) - b = controller_b.get_t_follow_multiplier(lead_present=True, lead_accel=-1.0, v_ego=20.0, lead_v_rel=0.0) - self.assertEqual(a, b) - - def test_widen_never_shrinks_below_stock(self): - for lead_accel in [-0.5, -1.5, -3.0, -6.0, 0.5, 0.0]: - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=lead_accel, v_ego=20.0) - self.assertGreaterEqual(multiplier, 1.0) - - def test_onset_is_faster_than_release(self): - for _ in range(5): - self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN, v_ego=20.0) - onset_multiplier = self.controller.t_follow_multiplier - for _ in range(5): - self.controller.get_t_follow_multiplier(lead_present=False, lead_accel=0.0, v_ego=20.0) - release_multiplier = self.controller.t_follow_multiplier - onset_progress = onset_multiplier - 1.0 - release_progress = (1.0 + LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal]) - onset_multiplier - self.assertGreater(onset_progress, 0.0) - self.assertLess(release_multiplier, onset_multiplier) - self.assertGreater(release_progress, 0.0) - - def test_profile_scales_max_widen(self): - controllers = {} - for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): - self.params.put("AccelPersonality", profile, block=True) - controllers[profile] = AccelController() - for profile, controller in controllers.items(): - for _ in range(500): - multiplier = controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, v_ego=20.0) - self.assertAlmostEqual(multiplier, 1.0 + LEAD_GAP_WIDEN_PROFILES[profile], places=2) - self.assertGreater(LEAD_GAP_WIDEN_PROFILES[AccelProfile.eco], LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal]) - self.assertGreater(LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal], LEAD_GAP_WIDEN_PROFILES[AccelProfile.sport]) - - def test_widen_tapers_out_at_low_speed(self): - # Widening t_follow only matters for higher-speed anticipation -- the MPC's own - # comfort-distance reference collapses to a t_follow-independent floor as v_ego -> 0, - # so widening during the final stopping approach only forces a bigger gap to close - # later and settles the car closer, not farther. Confirmed empirically via closed-loop - # scoring (sunnypilot/selfdrive/test/longitudinal_maneuvers/): must taper to a no-op - # at low speed even under hard lead braking. - for _ in range(500): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, v_ego=2.0) - self.assertEqual(multiplier, 1.0) - - def test_widen_scales_between_taper_speeds(self): - for _ in range(500): - multiplier = self.controller.get_t_follow_multiplier(lead_present=True, lead_accel=-LEAD_DECEL_FOR_MAX_WIDEN * 2, v_ego=5.5) - full_speed_widen = LEAD_GAP_WIDEN_PROFILES[AccelProfile.normal] - self.assertGreater(multiplier, 1.0) - self.assertLess(multiplier, 1.0 + full_speed_widen) - - def test_mpc_update_none_multiplier_matches_no_kwarg(self): - from unittest import mock - from openpilot.cereal import log - from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, get_T_FOLLOW - radarstate = log.RadarState.new_message() - mpc = LongitudinalMpc() - with mock.patch.object(mpc, "run", return_value=None): - mpc.update(radarstate, personality=log.LongitudinalPersonality.standard) - t_follow_no_kwarg = mpc.params[0, 4] - mpc.update(radarstate, personality=log.LongitudinalPersonality.standard, t_follow_multiplier=None) - t_follow_explicit_none = mpc.params[0, 4] - self.assertEqual(t_follow_no_kwarg, t_follow_explicit_none) - self.assertAlmostEqual(t_follow_no_kwarg, get_T_FOLLOW(log.LongitudinalPersonality.standard), places=6) - - def test_mpc_update_multiplier_scales_t_follow(self): - from unittest import mock - from openpilot.cereal import log - from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, get_T_FOLLOW - radarstate = log.RadarState.new_message() - mpc = LongitudinalMpc() - with mock.patch.object(mpc, "run", return_value=None): - mpc.update(radarstate, personality=log.LongitudinalPersonality.standard, t_follow_multiplier=1.5) - stock_t_follow = get_T_FOLLOW(log.LongitudinalPersonality.standard) - self.assertAlmostEqual(mpc.params[0, 4], stock_t_follow * 1.5, places=6) - def _fake_cp(): class _CP: diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 67c6019cae..6e342a5251 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -54,12 +54,6 @@ class LongitudinalPlannerSP: return None return self.accel_controller.get_min_accel(v_ego) - def get_t_follow_multiplier(self, sm: messaging.SubMaster, v_ego: float) -> float | None: - if not self.accel_controller.is_enabled(): - return None - lead = sm['radarState'].leadOne - return self.accel_controller.get_t_follow_multiplier(lead.present, lead.aLeadK, v_ego, lead.vRel) - def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: CS = sm['carState'] v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX) @@ -117,7 +111,6 @@ class LongitudinalPlannerSP: accel_controller.enabled = self.accel_controller.is_enabled() accel_controller.active = self.accel_controller_active accel_controller.profile = self.accel_controller.profile - accel_controller.tFollowMultiplier = float(self.accel_controller.t_follow_multiplier) # Smart Cruise Control smartCruiseControl = longitudinalPlanSP.smartCruiseControl diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py index 0451835b51..58389842f6 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -384,7 +384,6 @@ class PlantSP(Plant): "mpc_source": self.planner.mpc.source, "dec_mode": self.planner.dec.mode(), "controller_active": self.planner.accel_controller_active, - "t_follow_multiplier": self.planner.accel_controller.t_follow_multiplier, "model_action": { "desiredAcceleration": float(model_acceleration), "shouldStop": bool(model_should_stop), diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py index 55b7b06de1..e99e578101 100644 --- a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py @@ -1,17 +1,12 @@ -from collections import deque from collections.abc import Callable import math from typing import cast -from unittest import mock from openpilot.common.parameterized import parameterized -from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import accel_controller as accel_controller_module -from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelProfile -from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw") @@ -167,146 +162,3 @@ class TestPlantSP(OpenpilotTestCase): def test_invalid_actuator_dynamics(self, delay, lag): with self.assertRaises(ValueError): PlantSP(actuator_delay=delay, actuator_lag=lag) - - def test_v_rel_widen_anticipates_lagged_a_lead_k_without_safety_regression(self): - # Real radar's aLeadK is a filtered/differentiated estimate that lags the lead actually - # closing (confirmed on a recorded route: vRel escalated ~1s before aLeadK caught up). - # get_t_follow_multiplier's vRel trigger exists to widen t_follow before aLeadK updates -- - # this drives that scenario through the real closed-loop MPC while keeping dRel/vRel live. - params = Params() - params.put_bool("AccelPersonalityEnabled", True, block=True) - params.put("AccelPersonality", AccelProfile.normal, block=True) - - lead_accel_lag = 1.0 - brake_start = 3.0 # let the lead/MPC state settle for well over two seconds first - brake_end = 4.4 - lever_window_end = 4.45 # isolate pre-aLeadK widening while both synthetic arms remain solver-valid - full_event_end = 10.0 # include the complete brake and recovery response - lag_steps = round(lead_accel_lag / DT_MDL) - assert brake_start >= 2.0 - real_get_t_follow_multiplier = AccelController.get_t_follow_multiplier - - def run(strip_v_rel_widen: bool, onset_alpha: float, end_time: float): - # PlantSP publishes leadOne and leadTwo every frame. A shared history would advance - # twice per frame and silently turn this intended 1.0 s lag into 0.5 s. - histories = {lead_name: deque([0.0] * lag_steps, maxlen=lag_steps + 1) - for lead_name in ("leadOne", "leadTwo")} - lag_transitions = {lead_name: [] for lead_name in histories} - - def lagged_a_lead(current_time, lead_name, truth): - history = histories[lead_name] - history.append(truth["aLeadK"]) - out = dict(truth) - out["aLeadK"] = history[0] - lag_transitions[lead_name].append((current_time, truth["aLeadK"], out["aLeadK"])) - return out - - plant = PlantSP(lead_relevancy=True, speed=14.0, distance_lead=30.0, e2e=False, - lead_observation_fn=lagged_a_lead, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True) - v_lead = 14.0 - plant.v_lead_prev = v_lead # do not manufacture a 280 m/s^2 acceleration on the first frame - - solver_failures = 0 - original_reset = plant.planner.mpc.reset - - def counting_reset(*args, **kwargs): - nonlocal solver_failures - if plant.planner.mpc.solution_status != 0: - solver_failures += 1 - return original_reset(*args, **kwargs) - - plant.planner.mpc.reset = counting_reset - - def patched_t_follow(self, lead_present, lead_accel, v_ego, lead_v_rel=0.0): - return real_get_t_follow_multiplier(self, lead_present, lead_accel, v_ego, - 0.0 if strip_v_rel_widen else lead_v_rel) - - trace = [] - with mock.patch.object(accel_controller_module, "GAP_WIDEN_ONSET_ALPHA", onset_alpha), \ - mock.patch.object(AccelController, "get_t_follow_multiplier", patched_t_follow): - for _ in range(round(end_time / DT_MDL)): - current_time = plant.current_time - a_lead_cmd = -2.5 if brake_start <= current_time < brake_end else 0.0 - v_lead = max(0.0, v_lead + a_lead_cmd * plant.ts) - result = plant.step(v_lead=v_lead, v_cruise=22.0) - truth_lead = result["truth_lead"] - ttc = truth_lead["dRel"] / max(-truth_lead["vRel"], 1e-3) if truth_lead["vRel"] < 0.0 else None - trace.append({ - "time": current_time, - "realized_acceleration": result["realized_acceleration"], - "target_acceleration": result["a_target"], - "gap": result["distance_lead"] - result["distance"], - "ttc": ttc, - "fcw": result["fcw"], - "t_follow": result["t_follow_multiplier"], - }) - - event_index = round(brake_start / DT_MDL) - pre_event_accel = trace[event_index - 1]["realized_acceleration"] - onset_time = next(row["time"] for row in trace[event_index:] - if row["realized_acceleration"] <= pre_event_accel - 0.1) - measurement = trace[event_index - 1:] - realized_jerks = [(after["realized_acceleration"] - before["realized_acceleration"]) / DT_MDL - for before, after in zip(measurement, measurement[1:], strict=False)] - target_jerks = [(after["target_acceleration"] - before["target_acceleration"]) / DT_MDL - for before, after in zip(measurement, measurement[1:], strict=False)] - - measured_lags = {} - for lead_name, transitions in lag_transitions.items(): - true_onset = next(t for t, true_a, _ in transitions if true_a < -0.1) - observed_onset = next(t for t, _, observed_a in transitions if observed_a < -0.1) - measured_lags[lead_name] = observed_onset - true_onset - - return { - "solver_failures": solver_failures, - "lag_s": measured_lags, - "onset_s": onset_time, - "worst_negative_realized_jerk_mps3": min(realized_jerks), - "worst_negative_target_jerk_mps3": min(target_jerks), - "worst_positive_realized_jerk_mps3": max(realized_jerks), - "peak_realized_decel_mps2": min(row["realized_acceleration"] for row in measurement), - "peak_target_decel_mps2": min(row["target_acceleration"] for row in measurement), - "min_gap_m": min(row["gap"] for row in trace), - "end_gap_m": trace[-1]["gap"], - "min_ttc_s": min(row["ttc"] for row in trace if row["ttc"] is not None), - "max_t_follow_pre_lag": max(row["t_follow"] for row in trace - if brake_start <= row["time"] < brake_start + lead_accel_lag), - "fcw": any(row["fcw"] for row in trace), - } - - production_alpha = accel_controller_module.GAP_WIDEN_ONSET_ALPHA - reference_alpha = 0.15 - # Only isolate the controller's vRel-based t-follow widening in the short comparison. - # The real MPC still receives the same live radar vRel in every arm. - without_v_rel_widen = run(strip_v_rel_widen=True, onset_alpha=production_alpha, end_time=lever_window_end) - reference_onset = run(strip_v_rel_widen=False, onset_alpha=reference_alpha, end_time=full_event_end) - production = run(strip_v_rel_widen=False, onset_alpha=production_alpha, end_time=full_event_end) - diagnostics = f"without_v_rel_widen={without_v_rel_widen}, reference_onset={reference_onset}, production={production}" - - for metrics in (without_v_rel_widen, reference_onset, production): - assert metrics["solver_failures"] == 0, diagnostics - assert not metrics["fcw"], diagnostics - assert metrics["min_gap_m"] > 29.0, diagnostics - assert metrics["min_ttc_s"] > 15.0, diagnostics - for measured_lag in metrics["lag_s"].values(): - self.assertAlmostEqual(measured_lag, lead_accel_lag, delta=DT_MDL / 2.0, msg=diagnostics) - - self.assertAlmostEqual(without_v_rel_widen["max_t_follow_pre_lag"], 1.0, places=6, msg=diagnostics) - self.assertGreater(production["max_t_follow_pre_lag"], 1.015, diagnostics) - self.assertLessEqual(production["onset_s"], without_v_rel_widen["onset_s"] + DT_MDL, diagnostics) - - # The gentler onset filter must improve braking jerk over the previous 0.15 value - # without delaying the response or materially reducing separation. - self.assertLessEqual(production["onset_s"], reference_onset["onset_s"] + DT_MDL, diagnostics) - self.assertGreater(production["worst_negative_realized_jerk_mps3"], - reference_onset["worst_negative_realized_jerk_mps3"], diagnostics) - self.assertGreater(production["worst_negative_target_jerk_mps3"], - reference_onset["worst_negative_target_jerk_mps3"], diagnostics) - self.assertLessEqual(production["worst_positive_realized_jerk_mps3"], - reference_onset["worst_positive_realized_jerk_mps3"] + 0.1, diagnostics) - self.assertGreaterEqual(production["min_ttc_s"], reference_onset["min_ttc_s"] - 0.1, diagnostics) - self.assertGreaterEqual(production["end_gap_m"], reference_onset["end_gap_m"] - 0.5, diagnostics) - self.assertGreaterEqual(production["peak_realized_decel_mps2"], - reference_onset["peak_realized_decel_mps2"], diagnostics) - self.assertGreaterEqual(production["peak_target_decel_mps2"], - reference_onset["peak_target_decel_mps2"] - 0.1, diagnostics) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index f8ec4efb76..b30d0d8735 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -656,7 +656,7 @@ "key": "AccelPersonalityEnabled", "widget": "toggle", "title": "Enable Accel Controller", - "description": "Sets your preferred acceleration ceiling by profile, and gives extra following distance when a lead is braking for an earlier, smoother response. Stock braking and stopping logic remain in control at all times.", + "description": "Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain independent of this setting.", "visibility": [ { "type": "capability", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index d7466d7fba..bd6d08d6a0 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -46,8 +46,8 @@ sections: - key: AccelPersonalityEnabled widget: toggle title: Enable Accel Controller - description: Sets your preferred acceleration ceiling by profile, and gives extra following distance when a lead - is braking for an earlier, smoother response. Stock braking and stopping logic remain in control at all times. + description: Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, + and stopping behavior remain independent of this setting. visibility: - $ref: '#/macros/longitudinal' enablement: