diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 3a8356cf00..2f41c877f4 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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) diff --git a/openpilot/selfdrive/ui/layouts/settings/toggles.py b/openpilot/selfdrive/ui/layouts/settings/toggles.py index 30d4271c6b..c816803bc0 100644 --- a/openpilot/selfdrive/ui/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/layouts/settings/toggles.py @@ -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 " + 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 cadccaf1b1..d8160db9ee 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -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) 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 eb7de2d831..6a0f34b247 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 @@ -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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py index 5477eeb195..671445963b 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_closed_loop.py @@ -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())) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index e6e12420a4..8c44a459d7 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index d49fe39e1a..cbfe5e1098 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": "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, diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 5aa6caf8b5..5d8de62f4a 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -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