mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-03 16:53:44 +08:00
maybe better
This commit is contained in:
@@ -145,7 +145,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
|
||||
is_e2e = self.is_e2e(sm)
|
||||
|
||||
max_accel_override = self.get_max_accel_override(v_ego, v_cruise, is_e2e)
|
||||
max_accel_override = self.get_max_accel_override(v_ego)
|
||||
v_cruise = self.get_cruise_target_override(v_ego, v_cruise, force_decel)
|
||||
a_cruise_prev = self.a_cruise
|
||||
gated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset,
|
||||
self.CP, self.dt, accel_coast, self.allow_throttle, max_accel_override)
|
||||
@@ -165,6 +166,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
|
||||
self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
|
||||
self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
|
||||
self.accel_controller_active = bool(self.accel_controller.is_enabled() and not force_decel and
|
||||
self.mpc.source == LongitudinalPlanSource.cruise)
|
||||
|
||||
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
|
||||
|
||||
|
||||
@@ -14,11 +14,16 @@ from openpilot.sunnypilot import get_sanitize_int_param
|
||||
|
||||
AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
|
||||
MAX_ACCEL_BREAKPOINTS = [0., 3., 12, 24., 36.] # m/s
|
||||
MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 10., 20., 25., 40.] # m/s
|
||||
MAX_ACCEL_PROFILES = {
|
||||
AccelProfile.eco: [1.60, 1.48, 0.50, 0.30, 0.10],
|
||||
AccelProfile.normal: [1.90, 1.70, 0.80, 0.42, 0.30],
|
||||
AccelProfile.sport: [2.00, 2.00, 1.86, 1.30, 0.60],
|
||||
AccelProfile.eco: [1.60, 1.48, 1.22, 0.86, 0.66, 0.52, 0.40],
|
||||
AccelProfile.normal: [1.90, 1.70, 1.42, 0.99, 0.80, 0.66, 0.52],
|
||||
AccelProfile.sport: [2.00, 2.00, 1.86, 1.30, 1.02, 0.86, 0.72],
|
||||
}
|
||||
CRUISE_DECEL_RESPONSE_TIME = { # seconds
|
||||
AccelProfile.eco: 3.0,
|
||||
AccelProfile.normal: 2.5,
|
||||
AccelProfile.sport: 2.0,
|
||||
}
|
||||
|
||||
|
||||
@@ -44,3 +49,9 @@ class AccelController:
|
||||
|
||||
def get_max_accel(self, v_ego: float) -> float:
|
||||
return float(np.interp(max(0.0, v_ego), MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
|
||||
|
||||
def get_cruise_target(self, v_ego: float, v_target: float) -> float:
|
||||
if not np.isfinite(v_target) or v_target <= 0.0 or v_target >= v_ego:
|
||||
return v_target
|
||||
|
||||
return float(v_ego + (v_target - v_ego) / CRUISE_DECEL_RESPONSE_TIME[self._profile])
|
||||
|
||||
+60
-40
@@ -15,7 +15,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import (
|
||||
A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
AccelController, AccelProfile, CRUISE_DECEL_RESPONSE_TIME, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
)
|
||||
|
||||
|
||||
@@ -111,7 +111,25 @@ class TestAccelController(OpenpilotTestCase):
|
||||
controller = self.set_profile(AccelProfile.sport)
|
||||
assert controller.get_max_accel(-1.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][0]
|
||||
|
||||
def test_profile_change_has_no_controller_filter(self):
|
||||
def test_cruise_decel_response(self):
|
||||
v_ego = 25.0
|
||||
v_target = 22.5
|
||||
targets = {}
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
controller = self.set_profile(profile)
|
||||
targets[profile] = controller.get_cruise_target(v_ego, v_target)
|
||||
assert np.isclose(targets[profile] - v_ego, (v_target - v_ego) / CRUISE_DECEL_RESPONSE_TIME[profile])
|
||||
|
||||
assert v_ego > targets[AccelProfile.eco] > targets[AccelProfile.normal] > targets[AccelProfile.sport] > v_target
|
||||
|
||||
def test_cruise_target_bypasses_non_decel_requests(self):
|
||||
controller = self.set_profile(AccelProfile.eco)
|
||||
assert controller.get_cruise_target(20.0, 25.0) == 25.0
|
||||
assert controller.get_cruise_target(20.0, 20.0) == 20.0
|
||||
assert controller.get_cruise_target(20.0, 0.0) == 0.0
|
||||
assert np.isnan(controller.get_cruise_target(20.0, float("nan")))
|
||||
|
||||
def test_profile_change_refreshes_ceiling(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
controller.frame = int(1.0 / DT_MDL) - 1
|
||||
@@ -165,23 +183,21 @@ class TestPlannerIntegration(OpenpilotTestCase):
|
||||
|
||||
def test_disabled_leaves_stock_limit_active(self):
|
||||
planner = _bare_planner()
|
||||
for e2e in (False, True):
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=e2e) is None
|
||||
assert planner.accel_controller_active is False
|
||||
assert planner.get_max_accel_override(5.0) is None
|
||||
assert planner.accel_controller_active is False
|
||||
|
||||
def test_e2e_uses_enabled_profile(self):
|
||||
def test_enabled_profile_applies_to_cruise_candidate(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=True) == expected
|
||||
assert planner.accel_controller_active is True
|
||||
assert planner.get_max_accel_override(5.0) == expected
|
||||
|
||||
def test_enabled_acc_uses_python_native_telemetry_types(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
planner = _bare_planner()
|
||||
expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.sport])
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=False) == expected
|
||||
assert planner.get_max_accel_override(5.0) == expected
|
||||
assert type(planner.accel_controller_active) is bool
|
||||
assert type(planner.accel_controller.is_enabled()) is bool
|
||||
assert type(planner.accel_controller.profile) is int
|
||||
@@ -191,13 +207,9 @@ class TestPlannerIntegration(OpenpilotTestCase):
|
||||
self.params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
planner = _bare_planner()
|
||||
expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=False) == expected
|
||||
assert planner.accel_controller_active is True
|
||||
assert planner.get_max_accel_override(5.0) == expected
|
||||
|
||||
def test_ceiling_applies_to_every_target_source(self):
|
||||
# The ceiling is speed-scheduled only, so it is deliberately source-independent. This is what the old
|
||||
# COMFORT_SOURCES allow-list existed to qualify; with target shaping gone there is nothing to gate,
|
||||
# because an upper bound on acceleration cannot soften an SCC or speed-limit deceleration.
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
|
||||
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
@@ -208,40 +220,48 @@ class TestPlannerIntegration(OpenpilotTestCase):
|
||||
for source in (LongitudinalPlanSource.cruise, LongitudinalPlanSource.sccVision,
|
||||
LongitudinalPlanSource.sccMap, LongitudinalPlanSource.speedLimitAssist):
|
||||
planner.source = source
|
||||
assert np.isclose(planner.get_max_accel_override(speed, 33.0, e2e=False), expected), source
|
||||
assert np.isclose(planner.get_max_accel_override(speed), expected), source
|
||||
|
||||
def test_carried_accel_state_cannot_ratchet_above_the_ceiling(self):
|
||||
# get_cruise_accel clips to max_accel FIRST and applies its jerk limit SECOND, so when
|
||||
# a_cruise_prev - j*dt is above the ceiling, that second clip's lower bound pulls the command back over
|
||||
# it and can only walk down at j_cruise. a_cruise is force-set to the measured aEgo on reset_state, so
|
||||
# after the driver accelerates hard and lifts off, openpilot re-engages pinned above the profile.
|
||||
# Measured on route 000005dd: 87 frames commanding up to 1.70 m/s^2 where eco allows 0.87.
|
||||
def test_ceiling_remains_active_without_throttle_intent(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
planner = _bare_planner()
|
||||
v_ego = 9.84
|
||||
ceiling = planner.accel_controller.get_max_accel(v_ego)
|
||||
planner.allow_throttle = False
|
||||
assert planner.get_max_accel_override(12.0) == planner.accel_controller.get_max_accel(12.0)
|
||||
|
||||
planner.a_cruise = 1.90 # what a hard driver launch leaves behind
|
||||
override = planner.get_max_accel_override(v_ego, 30.0, e2e=False)
|
||||
def test_profile_switch_uses_stock_jerk_limit(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.sport, block=True)
|
||||
planner = _bare_planner()
|
||||
v_ego = 12.0
|
||||
planner.a_cruise = planner.accel_controller.get_max_accel(v_ego)
|
||||
|
||||
assert np.isclose(override, ceiling)
|
||||
assert planner.a_cruise <= ceiling + 1e-12
|
||||
accel = get_cruise_accel(False, 30.0, v_ego, planner.a_cruise, 0.0, _fake_cp(), DT_MDL, 0.0, True, override)
|
||||
assert accel <= ceiling + 1e-12
|
||||
self.params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
planner.accel_controller.frame = int(1.0 / DT_MDL) - 1
|
||||
planner.accel_controller.update()
|
||||
ceiling = planner.get_max_accel_override(v_ego)
|
||||
previous = planner.a_cruise
|
||||
accel = get_cruise_accel(False, 30.0, v_ego, previous, 0.0, _fake_cp(), DT_MDL, 0.0, True, ceiling)
|
||||
jerk_step = float(np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)) * DT_MDL
|
||||
|
||||
# Braking must be untouched: the clamp is upper-side only.
|
||||
for carried in (-3.5, -1.2, -0.4, 0.0):
|
||||
planner.a_cruise = carried
|
||||
planner.get_max_accel_override(v_ego, 30.0, e2e=False)
|
||||
assert planner.a_cruise == carried, carried
|
||||
assert previous > ceiling
|
||||
assert np.isclose(previous - accel, jerk_step)
|
||||
assert planner.a_cruise == previous
|
||||
|
||||
# Disabled must not touch the carried state at all.
|
||||
self.params.put_bool("AccelPersonalityEnabled", False, block=True)
|
||||
off = _bare_planner()
|
||||
off.a_cruise = 1.90
|
||||
assert off.get_max_accel_override(v_ego, 30.0, e2e=False) is None
|
||||
assert off.a_cruise == 1.90
|
||||
def test_cruise_target_shaping_is_source_and_force_decel_gated(self):
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
|
||||
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
shaped = planner.get_cruise_target_override(25.0, 22.5, force_decel=False)
|
||||
assert 22.5 < shaped < 25.0
|
||||
|
||||
for source in (LongitudinalPlanSource.sccVision, LongitudinalPlanSource.sccMap, LongitudinalPlanSource.speedLimitAssist):
|
||||
planner.source = source
|
||||
assert planner.get_cruise_target_override(25.0, 22.5, force_decel=False) == 22.5
|
||||
|
||||
planner.source = LongitudinalPlanSource.cruise
|
||||
assert planner.get_cruise_target_override(25.0, 0.0, force_decel=True) == 0.0
|
||||
|
||||
def test_e2e_candidate_is_held_through_a_brake_but_not_otherwise(self):
|
||||
# Route 000005dd: e2e -> lead1 stepped +2.25 m/s^2 in one frame (45 m/s^3) and back the next, while the
|
||||
|
||||
+67
-17
@@ -14,7 +14,7 @@ from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MAX_BP, J_CRUISE_VALS, get_cruise_accel
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MAX_BP, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource, T_IDXS as T_IDXS_MPC
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
@@ -53,22 +53,26 @@ def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cru
|
||||
target_speed = v_cruise if v_cruise_fn is None else v_cruise_fn(frame)
|
||||
measured = speed + (float(rng.normal(0.0, speed_noise)) if speed_noise else 0.0)
|
||||
max_accel_override = controller.get_max_accel(measured) if controller.is_enabled() else None
|
||||
if controller.is_enabled():
|
||||
target_speed = controller.get_cruise_target(measured, target_speed)
|
||||
accel = get_cruise_accel(e2e, target_speed, measured, accel, 0.0, CarParams(), DT_MDL, 2.0, True, max_accel_override)
|
||||
speed = max(0.0, speed + accel * DT_MDL)
|
||||
rows.append((speed, accel, should_stop(speed, accel)))
|
||||
return rows
|
||||
|
||||
|
||||
def run_vehicle_profile(profile: int, duration: float = 80.0, enabled: bool = True):
|
||||
def run_vehicle_profile(profile: int, duration: float = 80.0, enabled: bool = True, speed: float = 0.0,
|
||||
v_cruise_fn: Callable[[float], float] | None = None):
|
||||
params = Params()
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
params.put("AccelPersonality", profile, block=True)
|
||||
|
||||
plant = PlantSP(speed=0.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
|
||||
plant = PlantSP(speed=speed, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
|
||||
_set_mpc_acceleration(plant)
|
||||
rows = []
|
||||
while plant.current_time < duration:
|
||||
result = plant.step(v_cruise=25.0)
|
||||
v_cruise = 25.0 if v_cruise_fn is None else v_cruise_fn(plant.current_time)
|
||||
result = plant.step(v_cruise=v_cruise)
|
||||
rows.append((plant.current_time, result["speed"], result["a_target"], result["actuator_command"], result["acceleration"]))
|
||||
return np.asarray(rows)
|
||||
|
||||
@@ -131,7 +135,7 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
self.assertAlmostEqual(settled["a_target"], eco_limit, delta=0.01)
|
||||
self.assertLess(settled["a_target"], settled["model_action"]["desiredAcceleration"])
|
||||
|
||||
def test_blended_profile_does_not_change_model_braking(self):
|
||||
def test_lower_cruise_target_does_not_soften_model_braking(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
@@ -144,11 +148,10 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
plant = PlantSP(speed=20.0, e2e=True, model_action_fn=request_braking)
|
||||
_set_mpc_acceleration(plant)
|
||||
traces[enabled] = [plant.step(v_cruise=30.0) for _ in range(10)]
|
||||
traces[enabled] = [plant.step(v_cruise=19.5) for _ in range(10)]
|
||||
|
||||
self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.e2e for row in traces[True]))
|
||||
self.assertTrue(all(row["controller_active"] for row in traces[True]))
|
||||
self.assertTrue(all(not row["controller_active"] for row in traces[False]))
|
||||
self.assertTrue(all(not row["controller_active"] for trace in traces.values() for row in trace))
|
||||
for key in ("a_target", "should_stop", "mpc_source"):
|
||||
self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
|
||||
|
||||
@@ -170,7 +173,7 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
for key in ("a_target", "should_stop", "mpc_source"):
|
||||
self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]])
|
||||
|
||||
def test_profile_does_not_change_lead_braking(self):
|
||||
def test_lower_cruise_target_does_not_soften_lead_braking(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
@@ -180,7 +183,7 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
plant = PlantSP(speed=20.0)
|
||||
_set_mpc_acceleration(plant, -0.8)
|
||||
traces[enabled] = [plant.step(v_cruise=30.0) for _ in range(10)]
|
||||
traces[enabled] = [plant.step(v_cruise=19.5) for _ in range(10)]
|
||||
|
||||
self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.lead0 for row in traces[True]))
|
||||
for key in ("a_target", "should_stop", "mpc_source"):
|
||||
@@ -191,19 +194,66 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
normal = run_profile(AccelProfile.normal, speed=4.0, steps=120)
|
||||
self.assertGreater(normal[-1][0], eco[-1][0])
|
||||
|
||||
def test_profiles_do_not_change_far_braking(self):
|
||||
# The ceiling is an upper bound only, so a deceleration is bit-identical to stock for every profile.
|
||||
def test_zero_speed_stop_request_is_unchanged(self):
|
||||
# Zero-speed stop requests bypass the small cruise-setpoint pre-shape.
|
||||
for e2e in (False, True):
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
self.assertEqual(run_profile(profile, speed=20.0, v_cruise=0.0, e2e=e2e, steps=100), stock)
|
||||
|
||||
def test_cruise_decel_is_identical_to_stock_for_every_profile(self):
|
||||
# Deceleration is deliberately NOT profile-dependent: the controller sets an acceleration ceiling and
|
||||
# nothing else, and an upper bound cannot participate in a brake. Stock's clip to A_CRUISE_MIN owns it.
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, speed=25.0, v_cruise=20.0, steps=220)
|
||||
def test_large_cruise_decel_retains_stock_authority(self):
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
self.assertEqual(run_profile(profile, speed=25.0, v_cruise=20.0, steps=220), stock, profile)
|
||||
trace = np.asarray(run_profile(profile, speed=25.0, v_cruise=20.0, steps=220))
|
||||
self.assertAlmostEqual(float(np.min(trace[:, 1])), A_CRUISE_MIN, places=12)
|
||||
self.assertGreaterEqual(float(np.min(trace[:, 0])), 20.0 - 1e-9)
|
||||
|
||||
def test_small_cruise_decel_is_profiled_and_smooth(self):
|
||||
target = 25.0 - 5.0 * CV.MPH_TO_MS
|
||||
stock = np.asarray(run_profile(AccelProfile.normal, enabled=False, speed=25.0, v_cruise=target, steps=300))
|
||||
traces = {
|
||||
profile: np.asarray(run_profile(profile, speed=25.0, v_cruise=target, steps=300))
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
}
|
||||
peak_decel = {profile: float(np.min(trace[:, 1])) for profile, trace in traces.items()}
|
||||
|
||||
self.assertGreater(peak_decel[AccelProfile.eco], peak_decel[AccelProfile.normal])
|
||||
self.assertGreater(peak_decel[AccelProfile.normal], peak_decel[AccelProfile.sport])
|
||||
self.assertGreater(peak_decel[AccelProfile.sport], float(np.min(stock[:, 1])))
|
||||
|
||||
for trace in traces.values():
|
||||
peak_frame = int(np.argmin(trace[:, 1]))
|
||||
self.assertTrue(np.all(np.diff(trace[peak_frame:, 1]) >= -1e-12))
|
||||
self.assertGreaterEqual(float(np.min(trace[:, 0])), target - 1e-9)
|
||||
|
||||
speeds = np.concatenate(([25.0], trace[:-1, 0]))
|
||||
jerk_limit = np.interp(speeds, A_CRUISE_MAX_BP, J_CRUISE_VALS)
|
||||
jerk = np.abs(np.diff(np.concatenate(([0.0], trace[:, 1])))) / DT_MDL
|
||||
self.assertTrue(np.all(jerk <= jerk_limit + 1e-9))
|
||||
|
||||
def test_small_cruise_decel_stays_smooth_through_actuator(self):
|
||||
target = 25.0 - 5.0 * CV.MPH_TO_MS
|
||||
|
||||
def cruise_target(current_time: float) -> float:
|
||||
return 25.0 if current_time < 2.0 else target
|
||||
|
||||
stock = run_vehicle_profile(AccelProfile.normal, duration=12.0, enabled=False, speed=25.0, v_cruise_fn=cruise_target)
|
||||
traces = [
|
||||
run_vehicle_profile(profile, duration=12.0, speed=25.0, v_cruise_fn=cruise_target)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
]
|
||||
step_frame = int(np.flatnonzero(stock[:, 0] >= 2.0)[0])
|
||||
stock_decel = stock[step_frame:]
|
||||
|
||||
for trace in traces:
|
||||
decel = trace[step_frame:]
|
||||
self.assertGreater(float(np.min(decel[:, 2])), float(np.min(stock_decel[:, 2])))
|
||||
self.assertGreater(float(np.min(decel[:, 4])), float(np.min(stock_decel[:, 4])))
|
||||
self.assertGreaterEqual(float(np.min(decel[:, 1])), target - 1e-9)
|
||||
|
||||
for column in (2, 3, 4):
|
||||
jerk = np.max(np.abs(np.diff(trace[step_frame - 1:, column]))) / DT_MDL
|
||||
stock_jerk = np.max(np.abs(np.diff(stock[step_frame - 1:, column]))) / DT_MDL
|
||||
self.assertLessEqual(float(jerk), float(stock_jerk) + 1e-9)
|
||||
|
||||
def test_blended_launch_respects_profiles(self):
|
||||
traces = {
|
||||
|
||||
@@ -39,7 +39,6 @@ class LongitudinalPlannerSP:
|
||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||
self.source = LongitudinalPlanSource.cruise
|
||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||
self.a_cruise = 0. # re-assigned by the subclass; declared here because get_max_accel_override clamps it
|
||||
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
@@ -59,20 +58,17 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return False
|
||||
|
||||
def get_max_accel_override(self, v_ego: float, _v_target: float, e2e: bool) -> float | None:
|
||||
"""Pure speed-scheduled authority. The arrival taper is the comfort law's job, not the ceiling's."""
|
||||
self.accel_controller_active = bool(self.accel_controller.is_enabled() and (e2e or self.allow_throttle))
|
||||
if not self.accel_controller_active:
|
||||
def get_max_accel_override(self, v_ego: float) -> float | None:
|
||||
if not self.accel_controller.is_enabled():
|
||||
return None
|
||||
|
||||
ceiling = self.accel_controller.get_max_accel(v_ego)
|
||||
return self.accel_controller.get_max_accel(v_ego)
|
||||
|
||||
# get_cruise_accel jerk-limits AFTER clipping to max_accel, so a carried value above the ceiling ratchets
|
||||
# the command back over it. upper side only: never make braking less negative
|
||||
if math.isfinite(self.a_cruise):
|
||||
self.a_cruise = min(self.a_cruise, ceiling)
|
||||
def get_cruise_target_override(self, v_ego: float, v_target: float, force_decel: bool) -> float:
|
||||
if not self.accel_controller.is_enabled() or force_decel or self.source != LongitudinalPlanSource.cruise:
|
||||
return v_target
|
||||
|
||||
return ceiling
|
||||
return self.accel_controller.get_cruise_target(v_ego, v_target)
|
||||
|
||||
def _has_valid_selected_lead(self, sm: messaging.SubMaster, source: MpcPlanSource) -> bool:
|
||||
radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
|
||||
|
||||
Reference in New Issue
Block a user