mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-24 04:23:47 +08:00
Make acceleration profiles smooth and predictable
This commit is contained in:
rav4kumar
@@ -147,7 +147,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
|
||||
is_e2e = self.is_e2e(sm)
|
||||
|
||||
max_accel_override = self.get_max_accel_override(v_ego, is_e2e)
|
||||
max_accel_override = self.get_max_accel_override(v_ego, v_cruise, is_e2e)
|
||||
v_cruise = self.get_cruise_target_override(v_ego, v_cruise, is_e2e)
|
||||
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)
|
||||
|
||||
@@ -28,10 +28,10 @@ DESCRIPTIONS = {
|
||||
"your steering wheel distance button."
|
||||
),
|
||||
"AccelPersonalityEnabled": tr_noop(
|
||||
"Lets you choose the acceleration response. Lead following, braking, and stopping are unchanged."
|
||||
"Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged."
|
||||
),
|
||||
"AccelPersonality": tr_noop(
|
||||
"Eco accelerates more gently, Normal matches the stock cruise response, and Sport adds stronger low-speed acceleration."
|
||||
"Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive."
|
||||
),
|
||||
"IsLdwEnabled": tr_noop(
|
||||
"Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
|
||||
|
||||
@@ -16,10 +16,17 @@ AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
|
||||
MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 8., 10., 25., 40.]
|
||||
MAX_ACCEL_PROFILES = {
|
||||
AccelProfile.eco: [1.45, 1.40, 1.20, 0.96, 0.90, 0.60, 0.45],
|
||||
AccelProfile.normal: [1.60, 1.48, 1.40, 1.28, 1.20, 0.80, 0.60],
|
||||
AccelProfile.sport: [2.00, 1.99, 1.95, 1.45, 1.30, 0.80, 0.60],
|
||||
AccelProfile.eco: [1.50, 1.42, 0.90, 0.52, 0.44, 0.30, 0.23],
|
||||
AccelProfile.normal: [1.60, 1.48, 1.00, 0.60, 0.50, 0.36, 0.28],
|
||||
AccelProfile.sport: [1.80, 1.70, 1.12, 0.70, 0.60, 0.44, 0.35],
|
||||
}
|
||||
TARGET_SPEED_DEADBAND = 0.2 # m/s
|
||||
TARGET_SPEED_APPROACH_WINDOW = 2.0 # m/s
|
||||
TARGET_SPEED_APPROACH_GAIN = 0.5
|
||||
TARGET_SPEED_APPROACH_MIN_SPEED = 3.0 # m/s
|
||||
TARGET_SPEED_APPROACH_FULL_SPEED = 5.0 # m/s
|
||||
CATCHUP_ERROR_BREAKPOINTS = [TARGET_SPEED_DEADBAND, 0.5, 1.0, 2.0, 3.0, 4.0]
|
||||
CATCHUP_ACCEL_SCALE = [0.0, 0.3, 0.5, 0.7, 0.85, 1.0]
|
||||
|
||||
|
||||
class AccelController:
|
||||
@@ -42,6 +49,30 @@ class AccelController:
|
||||
def is_enabled(self) -> bool:
|
||||
return self._enabled
|
||||
|
||||
def get_max_accel(self, v_ego: float) -> float:
|
||||
def get_max_accel(self, v_ego: float, v_target: float | None = None) -> float:
|
||||
v_ego = max(0.0, v_ego)
|
||||
return float(np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
|
||||
max_accel = float(np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile]))
|
||||
if v_target is None or v_ego <= TARGET_SPEED_APPROACH_MIN_SPEED or v_target <= v_ego:
|
||||
return max_accel
|
||||
|
||||
speed_error = v_target - v_ego
|
||||
speed_blend = float(np.interp(v_ego, [TARGET_SPEED_APPROACH_MIN_SPEED, TARGET_SPEED_APPROACH_FULL_SPEED], [0.0, 1.0]))
|
||||
catchup_scale = float(np.interp(speed_error, CATCHUP_ERROR_BREAKPOINTS, CATCHUP_ACCEL_SCALE))
|
||||
raw_accel = min(speed_error, max_accel)
|
||||
catchup_accel = min(speed_error, max_accel * catchup_scale)
|
||||
return float(raw_accel + speed_blend * (catchup_accel - raw_accel))
|
||||
|
||||
def get_cruise_target(self, v_ego: float, v_target: float) -> float:
|
||||
if v_ego <= TARGET_SPEED_APPROACH_MIN_SPEED:
|
||||
return v_target
|
||||
|
||||
speed_error = v_target - v_ego
|
||||
if speed_error >= 0.0:
|
||||
return v_target
|
||||
|
||||
speed_blend = float(np.interp(v_ego, [TARGET_SPEED_APPROACH_MIN_SPEED, TARGET_SPEED_APPROACH_FULL_SPEED], [0.0, 1.0]))
|
||||
target_blend = float(np.interp(abs(speed_error), [TARGET_SPEED_DEADBAND, TARGET_SPEED_APPROACH_WINDOW], [1.0, 0.0]))
|
||||
deadband = TARGET_SPEED_DEADBAND * speed_blend * target_blend
|
||||
adjusted_error = np.sign(speed_error) * max(0.0, abs(speed_error) - deadband)
|
||||
gain = 1.0 - (1.0 - TARGET_SPEED_APPROACH_GAIN) * speed_blend * target_blend
|
||||
return float(v_ego + gain * adjusted_error)
|
||||
|
||||
+166
-15
@@ -14,7 +14,9 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import (
|
||||
A_CRUISE_MAX_BP, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel, get_max_accel,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
|
||||
AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
AccelController, AccelProfile, CATCHUP_ACCEL_SCALE, CATCHUP_ERROR_BREAKPOINTS, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
|
||||
TARGET_SPEED_APPROACH_FULL_SPEED, TARGET_SPEED_APPROACH_GAIN, TARGET_SPEED_APPROACH_MIN_SPEED, TARGET_SPEED_APPROACH_WINDOW,
|
||||
TARGET_SPEED_DEADBAND,
|
||||
)
|
||||
|
||||
|
||||
@@ -49,15 +51,120 @@ class TestAccelController(OpenpilotTestCase):
|
||||
assert value <= previous[profile]
|
||||
previous[profile] = value
|
||||
|
||||
def test_normal_matches_stock(self):
|
||||
def test_normal_stays_below_stock(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
for speed in np.linspace(0.0, 55.0, 551):
|
||||
assert np.isclose(controller.get_max_accel(speed), get_max_accel(speed), rtol=0.0, atol=1e-12)
|
||||
assert controller.get_max_accel(speed) <= get_max_accel(speed) + 1e-12
|
||||
|
||||
def test_profiles_taper_below_stock_at_road_speed(self):
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
controller = self.set_profile(profile)
|
||||
for speed in np.linspace(8.0, 40.0, 321):
|
||||
assert controller.get_max_accel(speed) < get_max_accel(speed)
|
||||
|
||||
def test_launch_caps_stay_close_to_stock(self):
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
controller = self.set_profile(profile)
|
||||
for speed in np.linspace(0.0, 3.0, 61):
|
||||
assert controller.get_max_accel(speed) >= 0.9 * get_max_accel(speed)
|
||||
|
||||
def test_eco_keeps_useful_road_speed_acceleration(self):
|
||||
controller = self.set_profile(AccelProfile.eco)
|
||||
for speed in np.linspace(8.0, 40.0, 321):
|
||||
assert controller.get_max_accel(speed) >= 0.75 * get_max_accel(speed) - 1e-12
|
||||
assert controller.get_max_accel(speed) >= 0.35 * get_max_accel(speed) - 1e-12
|
||||
|
||||
def test_profile_caps_drop_quickly_after_launch(self):
|
||||
for values in MAX_ACCEL_PROFILES.values():
|
||||
assert values[2] <= 0.7 * values[1]
|
||||
assert values[3] <= 0.5 * values[1]
|
||||
|
||||
def test_sport_stays_below_reported_route_acceleration(self):
|
||||
controller = self.set_profile(AccelProfile.sport)
|
||||
route_samples = ((8.1, 0.877), (12.0, 0.858), (15.5, 0.802), (18.8, 0.750), (21.7, 0.654))
|
||||
for speed, recorded_accel in route_samples:
|
||||
assert controller.get_max_accel(speed) <= 0.85 * recorded_accel
|
||||
|
||||
def test_positive_catchup_limit_is_continuous_and_monotonic(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
v_ego = 20.0
|
||||
max_accel = controller.get_max_accel(v_ego)
|
||||
errors = np.linspace(1e-4, 8.0, 321)
|
||||
accel_limits = np.asarray([controller.get_max_accel(v_ego, v_ego + error) for error in errors])
|
||||
|
||||
assert np.all(np.isfinite(accel_limits))
|
||||
assert np.all(np.diff(accel_limits) >= -1e-12)
|
||||
assert np.all(accel_limits <= errors + 1e-12)
|
||||
assert np.all(accel_limits <= max_accel + 1e-12)
|
||||
assert controller.get_max_accel(v_ego, v_ego + TARGET_SPEED_DEADBAND) == 0.0
|
||||
assert np.isclose(controller.get_max_accel(v_ego, v_ego + CATCHUP_ERROR_BREAKPOINTS[-1]), max_accel)
|
||||
|
||||
for error, scale in zip(CATCHUP_ERROR_BREAKPOINTS, CATCHUP_ACCEL_SCALE, strict=True):
|
||||
expected_accel = min(error, max_accel * scale)
|
||||
assert np.isclose(controller.get_max_accel(v_ego, v_ego + error), expected_accel)
|
||||
below = controller.get_max_accel(v_ego, v_ego + error - 1e-6)
|
||||
above = controller.get_max_accel(v_ego, v_ego + error + 1e-6)
|
||||
assert above >= below
|
||||
assert above - below < 1e-4
|
||||
|
||||
def test_cruise_decel_settling_is_continuous(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
v_ego = 20.0
|
||||
near_error = 0.5
|
||||
target_blend = (TARGET_SPEED_APPROACH_WINDOW - near_error) / (TARGET_SPEED_APPROACH_WINDOW - TARGET_SPEED_DEADBAND)
|
||||
adjusted_error = near_error - TARGET_SPEED_DEADBAND * target_blend
|
||||
gain = 1.0 - (1.0 - TARGET_SPEED_APPROACH_GAIN) * target_blend
|
||||
expected_error = adjusted_error * gain
|
||||
|
||||
assert np.isclose(controller.get_cruise_target(v_ego, v_ego - near_error), v_ego - expected_error)
|
||||
assert controller.get_cruise_target(v_ego, v_ego - TARGET_SPEED_DEADBAND) == v_ego
|
||||
assert controller.get_cruise_target(v_ego, v_ego - TARGET_SPEED_APPROACH_WINDOW) == v_ego - TARGET_SPEED_APPROACH_WINDOW
|
||||
|
||||
errors = np.linspace(-5.0, 0.0, 201)
|
||||
shaped_errors = np.asarray([controller.get_cruise_target(v_ego, v_ego + error) - v_ego for error in errors])
|
||||
assert np.all(np.isfinite(shaped_errors))
|
||||
assert np.all(np.diff(shaped_errors) >= 0.0)
|
||||
assert np.all(np.abs(shaped_errors) <= np.abs(errors) + 1e-12)
|
||||
|
||||
def test_catchup_limit_does_not_touch_launch(self):
|
||||
controller = self.set_profile(AccelProfile.sport)
|
||||
|
||||
def command(speed: float, target: float, dynamic: bool = True) -> float:
|
||||
max_accel = controller.get_max_accel(speed, target) if dynamic else controller.get_max_accel(speed)
|
||||
return get_cruise_accel(False, target, speed, 0.0, 0.0, _fake_cp(), 10.0, 0.0, True, max_accel)
|
||||
|
||||
for speed in np.linspace(0.0, TARGET_SPEED_APPROACH_MIN_SPEED, 101):
|
||||
for target in (0.0, 8.0, 30.0):
|
||||
assert command(speed, target) == command(speed, target, dynamic=False)
|
||||
|
||||
below = command(TARGET_SPEED_APPROACH_MIN_SPEED - 1e-3, TARGET_SPEED_APPROACH_MIN_SPEED + 0.5 - 1e-3)
|
||||
at = command(TARGET_SPEED_APPROACH_MIN_SPEED, TARGET_SPEED_APPROACH_MIN_SPEED + 0.5)
|
||||
above = command(TARGET_SPEED_APPROACH_MIN_SPEED + 1e-3, TARGET_SPEED_APPROACH_MIN_SPEED + 0.5 + 1e-3)
|
||||
assert np.isclose(below, 0.5)
|
||||
assert np.isclose(at, 0.5)
|
||||
assert abs(above - at) < 1e-3
|
||||
|
||||
below_full = command(TARGET_SPEED_APPROACH_FULL_SPEED - 1e-3, TARGET_SPEED_APPROACH_FULL_SPEED + 0.5 - 1e-3)
|
||||
at_full = command(TARGET_SPEED_APPROACH_FULL_SPEED, TARGET_SPEED_APPROACH_FULL_SPEED + 0.5)
|
||||
above_full = command(TARGET_SPEED_APPROACH_FULL_SPEED + 1e-3, TARGET_SPEED_APPROACH_FULL_SPEED + 0.5 + 1e-3)
|
||||
assert abs(below_full - at_full) < 1e-3
|
||||
assert abs(above_full - at_full) < 1e-3
|
||||
|
||||
def test_cruise_target_deadband_removes_small_sign_flips(self):
|
||||
controller = self.set_profile(AccelProfile.normal)
|
||||
v_ego = 20.0
|
||||
errors = np.asarray([0.10, -0.10, 0.15, -0.15, 0.30, -0.30])
|
||||
commands = []
|
||||
for error in errors:
|
||||
raw_target = v_ego + error
|
||||
target = controller.get_cruise_target(v_ego, raw_target)
|
||||
max_accel = controller.get_max_accel(v_ego, raw_target)
|
||||
commands.append(get_cruise_accel(False, target, v_ego, 0.0, 0.0, _fake_cp(), 10.0, 0.0, True, max_accel))
|
||||
shaped = np.asarray(commands)
|
||||
|
||||
assert np.count_nonzero(shaped[:4]) == 0
|
||||
assert shaped[4] > 0.0
|
||||
assert shaped[5] < 0.0
|
||||
assert np.all(np.abs(shaped) <= np.abs(errors) + 1e-12)
|
||||
|
||||
def test_negative_speed_uses_standstill_value(self):
|
||||
controller = self.set_profile(AccelProfile.sport)
|
||||
@@ -98,7 +205,7 @@ class TestPlannerIntegration(OpenpilotTestCase):
|
||||
args = (e2e, 30.0, 12.0, 0.2, 4.0, _fake_cp(), DT_MDL, -0.3, allow_throttle)
|
||||
assert get_cruise_accel(*args) == get_cruise_accel(*args, max_accel_override=None)
|
||||
|
||||
def test_profiles_do_not_change_braking(self):
|
||||
def test_profiles_do_not_change_far_braking(self):
|
||||
args = (False, 0.0, 20.0, 0.0, 0.0, _fake_cp(), 10.0, -0.3, True)
|
||||
stock = get_cruise_accel(*args)
|
||||
assert stock == A_CRUISE_MIN
|
||||
@@ -112,31 +219,73 @@ class TestPlannerIntegration(OpenpilotTestCase):
|
||||
jerk_limit = np.interp(speed, A_CRUISE_MAX_BP, J_CRUISE_VALS) * DT_MDL
|
||||
assert np.isclose(target, jerk_limit)
|
||||
|
||||
def test_disabled_and_e2e_leave_stock_limit_active(self):
|
||||
planner = _bare_planner()
|
||||
assert planner.get_max_accel_override(5.0, e2e=False) is None
|
||||
assert planner.accel_controller_active is False
|
||||
def test_cruise_accel_tapers_before_target(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
controller = AccelController()
|
||||
speed = 20.0
|
||||
speed_errors = (4.0, 3.0, 2.0, 1.0, 0.5, TARGET_SPEED_DEADBAND)
|
||||
targets = [controller.get_cruise_target(speed, speed + error) for error in speed_errors]
|
||||
max_accels = [controller.get_max_accel(speed, speed + error) for error in speed_errors]
|
||||
accels = [get_cruise_accel(False, target, speed, 0.0, 0.0, _fake_cp(), 10.0, 0.0, True, max_accel)
|
||||
for target, max_accel in zip(targets, max_accels, strict=True)]
|
||||
|
||||
assert np.isclose(accels[0], controller.get_max_accel(speed))
|
||||
assert accels[1] < controller.get_max_accel(speed)
|
||||
assert all(current > following for current, following in zip(accels, accels[1:], strict=False))
|
||||
assert accels[-1] == 0.0
|
||||
|
||||
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.get_cruise_target_override(20.0, 20.5, e2e=e2e) == 20.5
|
||||
assert planner.accel_controller_active is False
|
||||
|
||||
def test_e2e_uses_enabled_profile(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
assert planner.get_max_accel_override(5.0, e2e=True) is None
|
||||
assert planner.accel_controller_active is False
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=True) == MAX_ACCEL_PROFILES[AccelProfile.normal][2]
|
||||
assert planner.accel_controller_active is True
|
||||
|
||||
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()
|
||||
assert planner.get_max_accel_override(5.0, e2e=False) == MAX_ACCEL_PROFILES[AccelProfile.sport][2]
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=False) == MAX_ACCEL_PROFILES[AccelProfile.sport][2]
|
||||
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
|
||||
|
||||
def test_normal_profile_uses_exact_stock_path(self):
|
||||
def test_normal_profile_uses_tuned_limit(self):
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
self.params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
planner = _bare_planner()
|
||||
assert planner.get_max_accel_override(5.0, e2e=False) is None
|
||||
assert planner.get_max_accel_override(5.0, 30.0, e2e=False) == MAX_ACCEL_PROFILES[AccelProfile.normal][2]
|
||||
assert planner.accel_controller_active is True
|
||||
|
||||
def test_planner_applies_cruise_settling_only_when_safe(self):
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
|
||||
|
||||
self.params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
planner = _bare_planner()
|
||||
expected_limit = planner.accel_controller.get_max_accel(20.0, 20.5)
|
||||
expected_decel_target = planner.accel_controller.get_cruise_target(20.5, 20.0)
|
||||
assert planner.get_cruise_target_override(20.0, 20.5, e2e=False) == 20.5
|
||||
assert np.isclose(planner.get_cruise_target_override(20.5, 20.0, e2e=False), expected_decel_target)
|
||||
assert np.isclose(planner.get_max_accel_override(20.0, 20.5, e2e=False), expected_limit)
|
||||
|
||||
planner.allow_throttle = False
|
||||
assert planner.get_cruise_target_override(20.0, 20.5, e2e=False) == 20.5
|
||||
assert planner.get_max_accel_override(20.0, 20.5, e2e=False) is None
|
||||
assert planner.accel_controller_active is False
|
||||
assert planner.get_cruise_target_override(20.0, 20.5, e2e=True) == 20.5
|
||||
assert np.isclose(planner.get_max_accel_override(20.0, 20.5, e2e=True), expected_limit)
|
||||
assert planner.accel_controller_active is True
|
||||
|
||||
planner.source = LongitudinalPlanSource.sccVision
|
||||
assert planner.get_cruise_target_override(20.0, 20.5, e2e=True) == 20.5
|
||||
assert planner.get_max_accel_override(20.0, 20.5, e2e=True) == planner.accel_controller.get_max_accel(20.0)
|
||||
|
||||
|
||||
def _fake_cp():
|
||||
@@ -148,9 +297,11 @@ def _fake_cp():
|
||||
|
||||
|
||||
def _bare_planner():
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
|
||||
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.accel_controller = AccelController()
|
||||
planner.accel_controller_active = False
|
||||
planner.allow_throttle = True
|
||||
planner.source = LongitudinalPlanSource.cruise
|
||||
return planner
|
||||
|
||||
+184
-12
@@ -7,12 +7,18 @@ See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
from collections.abc import Callable
|
||||
|
||||
import numpy as np
|
||||
|
||||
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 get_cruise_accel
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelProfile
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MAX_BP, 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, TARGET_SPEED_DEADBAND,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
|
||||
|
||||
|
||||
class CarParams:
|
||||
@@ -20,8 +26,19 @@ class CarParams:
|
||||
wheelbase = 2.7
|
||||
|
||||
|
||||
def _set_mpc_acceleration(plant: PlantSP, acceleration: float = 2.0) -> None:
|
||||
def update(_radar_state, **_kwargs):
|
||||
mpc = plant.planner.mpc
|
||||
mpc.source = LongitudinalPlanSource.lead0
|
||||
mpc.v_solution[:] = mpc.x0[1] + acceleration * T_IDXS_MPC
|
||||
mpc.a_solution.fill(acceleration)
|
||||
mpc.j_solution.fill(0.0)
|
||||
|
||||
plant.planner.mpc.update = update
|
||||
|
||||
|
||||
def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cruise: float = 30.0,
|
||||
v_cruise_fn: Callable[[int], float] | None = None, steps: int = 120):
|
||||
v_cruise_fn: Callable[[int], float] | None = None, e2e: bool = False, steps: int = 120):
|
||||
params = Params()
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
params.put("AccelPersonality", profile, block=True)
|
||||
@@ -31,26 +48,138 @@ def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cru
|
||||
rows = []
|
||||
for frame in range(steps):
|
||||
target_speed = v_cruise if v_cruise_fn is None else v_cruise_fn(frame)
|
||||
custom_profile = controller.is_enabled() and controller.profile != AccelProfile.normal
|
||||
max_accel_override = controller.get_max_accel(speed) if custom_profile else None
|
||||
accel = get_cruise_accel(False, target_speed, speed, accel, 0.0, CarParams(), DT_MDL, 2.0, True, max_accel_override)
|
||||
use_profile = controller.is_enabled()
|
||||
cruise_target = controller.get_cruise_target(speed, target_speed) if use_profile else target_speed
|
||||
max_accel_override = controller.get_max_accel(speed, target_speed) if use_profile else None
|
||||
accel = get_cruise_accel(e2e, cruise_target, speed, 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
|
||||
|
||||
|
||||
class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
def test_normal_matches_disabled_stock_path(self):
|
||||
def test_blended_positive_model_request_uses_profile_cruise_cap(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put_bool("AccelPersonalityEnabled", True, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
def request_acceleration(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
|
||||
return 2.0, False
|
||||
|
||||
plant = PlantSP(speed=15.0, e2e=True, model_action_fn=request_acceleration)
|
||||
_set_mpc_acceleration(plant)
|
||||
results = [plant.step(v_cruise=35.0) for _ in range(20)]
|
||||
settled = results[-1]
|
||||
eco_limit = float(np.interp(settled["published_v_ego"], MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.eco]))
|
||||
|
||||
self.assertTrue(settled["controller_active"])
|
||||
self.assertEqual(settled["mpc_source"], LongitudinalPlanSource.cruise)
|
||||
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):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
def request_braking(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
|
||||
return -0.8, False
|
||||
|
||||
traces = {}
|
||||
for enabled in (False, True):
|
||||
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)]
|
||||
|
||||
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]))
|
||||
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_model_stop_request(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
def request_stop(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
|
||||
return -0.8, True
|
||||
|
||||
traces = {}
|
||||
for enabled in (False, True):
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
plant = PlantSP(speed=1.0, e2e=True, model_action_fn=request_stop)
|
||||
_set_mpc_acceleration(plant)
|
||||
traces[enabled] = [plant.step(v_cruise=30.0) for _ in range(10)]
|
||||
|
||||
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):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.eco, block=True)
|
||||
|
||||
traces = {}
|
||||
for enabled in (False, True):
|
||||
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)]
|
||||
|
||||
self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.lead0 for row in traces[True]))
|
||||
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_blended_cruise_settling_reduces_small_corrections_both_directions(self):
|
||||
params = Params()
|
||||
params.put_bool("DynamicExperimentalControl", False, block=True)
|
||||
params.put("AccelPersonality", AccelProfile.normal, block=True)
|
||||
|
||||
def request_acceleration(_current_time: float, _speed: float, _acceleration: float) -> tuple[float, bool]:
|
||||
return 2.0, False
|
||||
|
||||
peak_corrections = {}
|
||||
for enabled in (False, True):
|
||||
params.put_bool("AccelPersonalityEnabled", enabled, block=True)
|
||||
for direction, speed, cruise in (("accelerate", 20.0, 20.5), ("decelerate", 20.5, 20.0)):
|
||||
plant = PlantSP(speed=speed, e2e=True, model_action_fn=request_acceleration)
|
||||
_set_mpc_acceleration(plant)
|
||||
trace = [plant.step(v_cruise=cruise) for _ in range(20)]
|
||||
peak_corrections[enabled, direction] = max(abs(row["a_target"]) for row in trace)
|
||||
|
||||
for direction in ("accelerate", "decelerate"):
|
||||
self.assertLess(peak_corrections[True, direction], peak_corrections[False, direction])
|
||||
|
||||
def test_normal_is_less_aggressive_than_disabled_stock_path(self):
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, speed=4.0, steps=120)
|
||||
normal = run_profile(AccelProfile.normal, speed=4.0, steps=120)
|
||||
self.assertEqual(normal, stock)
|
||||
self.assertTrue(all(tuned[1] <= original[1] + 1e-12 for tuned, original in zip(normal, stock, strict=True)))
|
||||
self.assertLess(normal[-1][0], stock[-1][0])
|
||||
|
||||
def test_profiles_do_not_change_braking(self):
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, speed=20.0, v_cruise=0.0, steps=100)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
self.assertEqual(run_profile(profile, speed=20.0, v_cruise=0.0, steps=100), stock)
|
||||
def test_profiles_do_not_change_far_braking(self):
|
||||
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_blended_launch_respects_profiles(self):
|
||||
traces = {
|
||||
profile: run_profile(profile, v_cruise=8.0, e2e=True, steps=180)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
}
|
||||
time_to_five = {
|
||||
profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
|
||||
for profile, rows in traces.items()
|
||||
}
|
||||
|
||||
self.assertLess(time_to_five[AccelProfile.sport], time_to_five[AccelProfile.normal])
|
||||
self.assertLess(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.eco])
|
||||
|
||||
def test_launch_ordering_without_departure_delay(self):
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, v_cruise=8.0, steps=160)
|
||||
traces = {
|
||||
profile: run_profile(profile, v_cruise=8.0, steps=160)
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
|
||||
@@ -63,8 +192,17 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
profile: next(frame for frame, row in enumerate(rows) if row[0] >= 5.0) * DT_MDL
|
||||
for profile, rows in traces.items()
|
||||
}
|
||||
stock_first_motion = next(frame for frame, row in enumerate(stock) if row[0] > 0.01)
|
||||
stock_time_to_two = next(frame for frame, row in enumerate(stock) if row[0] >= 2.0) * DT_MDL
|
||||
stock_time_to_three = next(frame for frame, row in enumerate(stock) if row[0] >= 3.0) * DT_MDL
|
||||
|
||||
self.assertEqual(len(set(first_motion.values())), 1)
|
||||
self.assertTrue(all(frame == stock_first_motion for frame in first_motion.values()))
|
||||
for rows in traces.values():
|
||||
time_to_two = next(frame for frame, row in enumerate(rows) if row[0] >= 2.0) * DT_MDL
|
||||
time_to_three = next(frame for frame, row in enumerate(rows) if row[0] >= 3.0) * DT_MDL
|
||||
self.assertLessEqual(time_to_two, stock_time_to_two + DT_MDL)
|
||||
self.assertLessEqual(time_to_three, stock_time_to_three + 0.1)
|
||||
self.assertLessEqual(time_to_five[AccelProfile.sport], time_to_five[AccelProfile.normal])
|
||||
self.assertLessEqual(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.eco])
|
||||
self.assertLessEqual(time_to_five[AccelProfile.eco], 1.25 * time_to_five[AccelProfile.normal])
|
||||
@@ -78,6 +216,37 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
self.assertGreaterEqual(gains[AccelProfile.sport], gains[AccelProfile.normal])
|
||||
self.assertGreaterEqual(gains[AccelProfile.eco], 0.72 * gains[AccelProfile.normal])
|
||||
|
||||
def test_catchup_settles_inside_deadband_without_oscillation(self):
|
||||
target_speed = 24.0
|
||||
rows = run_profile(AccelProfile.normal, speed=20.0, v_cruise=target_speed, steps=800)
|
||||
final_deficit = target_speed - rows[-1][0]
|
||||
|
||||
self.assertGreaterEqual(final_deficit, -1e-9)
|
||||
self.assertLessEqual(final_deficit, TARGET_SPEED_DEADBAND + 0.01)
|
||||
self.assertTrue(all(row[1] >= -1e-12 for row in rows))
|
||||
self.assertTrue(all(current[0] <= following[0] for current, following in zip(rows, rows[1:], strict=False)))
|
||||
|
||||
def test_smaller_target_gap_uses_stock_downward_jerk(self):
|
||||
def target_speed(frame: int) -> float:
|
||||
return 24.0 if frame < 40 else 21.5
|
||||
|
||||
rows = run_profile(AccelProfile.normal, speed=20.0, v_cruise_fn=target_speed, steps=80)
|
||||
command_drop = rows[39][1] - rows[40][1]
|
||||
|
||||
self.assertLessEqual(command_drop, max(J_CRUISE_VALS) * DT_MDL + 1e-12)
|
||||
self.assertTrue(all(row[1] >= -1e-12 for row in rows[40:]))
|
||||
|
||||
def test_full_catchup_trace_respects_stock_jerk(self):
|
||||
for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
|
||||
rows = run_profile(profile, v_cruise=30.0, steps=300)
|
||||
previous_speed = 0.0
|
||||
previous_accel = 0.0
|
||||
for speed, accel, _should_stop in rows:
|
||||
jerk_step = float(np.interp(previous_speed, A_CRUISE_MAX_BP, J_CRUISE_VALS)) * DT_MDL
|
||||
self.assertLessEqual(abs(accel - previous_accel), jerk_step + 1e-12)
|
||||
previous_speed = speed
|
||||
previous_accel = accel
|
||||
|
||||
def test_stop_release_frame_is_profile_independent(self):
|
||||
def target_speed(frame: int) -> float:
|
||||
return 0.0 if frame < 20 else 8.0
|
||||
@@ -90,4 +259,7 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
|
||||
profile: next(frame for frame, row in enumerate(rows) if frame >= 20 and not row[2])
|
||||
for profile, rows in traces.items()
|
||||
}
|
||||
stock = run_profile(AccelProfile.normal, enabled=False, v_cruise_fn=target_speed, steps=80)
|
||||
stock_release_frame = next(frame for frame, row in enumerate(stock) if frame >= 20 and not row[2])
|
||||
self.assertEqual(len(set(release_frames.values())), 1)
|
||||
self.assertTrue(all(frame == stock_release_frame for frame in release_frames.values()))
|
||||
|
||||
@@ -11,7 +11,7 @@ from openpilot.cereal import messaging, custom, log
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelProfile
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LeadDepartureController
|
||||
@@ -52,12 +52,17 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return experimental_mode and self.dec.mode() == "blended"
|
||||
|
||||
def get_max_accel_override(self, v_ego: float, e2e: bool) -> float | None:
|
||||
custom_profile = self.accel_controller.profile != AccelProfile.normal
|
||||
self.accel_controller_active = bool(self.accel_controller.is_enabled() and custom_profile and not e2e)
|
||||
def get_max_accel_override(self, v_ego: float, v_target: float, e2e: bool) -> float | None:
|
||||
self.accel_controller_active = bool(self.accel_controller.is_enabled() and (e2e or self.allow_throttle))
|
||||
if not self.accel_controller_active:
|
||||
return None
|
||||
return self.accel_controller.get_max_accel(v_ego)
|
||||
target = v_target if self.source == LongitudinalPlanSource.cruise else None
|
||||
return self.accel_controller.get_max_accel(v_ego, target)
|
||||
|
||||
def get_cruise_target_override(self, v_ego: float, v_target: float, e2e: bool) -> float:
|
||||
if not self.accel_controller.is_enabled() or self.source != LongitudinalPlanSource.cruise or not (e2e or self.allow_throttle):
|
||||
return v_target
|
||||
return self.accel_controller.get_cruise_target(v_ego, v_target)
|
||||
|
||||
def update_allow_throttle(self, throttle_prob: float, low_speed_override: bool, threshold: float) -> bool:
|
||||
return self.throttle_intent_controller.update(throttle_prob, low_speed_override=low_speed_override, threshold=threshold)
|
||||
|
||||
@@ -656,7 +656,7 @@
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Enable Accel Controller",
|
||||
"description": "Lets you choose the acceleration response. Lead following, braking, and stopping are unchanged.",
|
||||
"description": "Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are unchanged.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
@@ -676,7 +676,7 @@
|
||||
"key": "AccelPersonality",
|
||||
"widget": "multiple_button",
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Eco accelerates more gently, Normal matches the stock cruise response, and Sport adds stronger low-speed acceleration.",
|
||||
"description": "Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
|
||||
@@ -46,7 +46,8 @@ sections:
|
||||
- key: AccelPersonalityEnabled
|
||||
widget: toggle
|
||||
title: Enable Accel Controller
|
||||
description: Lets you choose the acceleration response. Lead following, braking, and stopping are unchanged.
|
||||
description: Lets you choose how sunnypilot starts, catches up, and settles at the cruise speed. Emergency braking and stopping are
|
||||
unchanged.
|
||||
visibility:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
enablement:
|
||||
@@ -54,8 +55,7 @@ sections:
|
||||
- key: AccelPersonality
|
||||
widget: multiple_button
|
||||
title: Acceleration Profile
|
||||
description: Eco accelerates more gently, Normal matches the stock cruise response, and Sport adds stronger low-speed
|
||||
acceleration.
|
||||
description: Eco is gentlest, Normal balances a prompt start with smooth catch-up, and Sport is more responsive.
|
||||
options:
|
||||
- value: 0
|
||||
label: Eco
|
||||
|
||||
Reference in New Issue
Block a user