mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-19 22:23:44 +08:00
remove tfollow modifier
This commit is contained in:
rav4kumar
@@ -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;
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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."
|
||||
|
||||
@@ -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
|
||||
|
||||
+14
-147
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user