diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index d767f14541..3b313fc1c8 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -35,12 +35,8 @@ def get_max_accel(v_ego): def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py -def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle, - max_accel_override=None): - if max_accel_override is not None: - max_accel = max_accel_override - else: - max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego) +def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle): + max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego) if not e2e: a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) @@ -55,23 +51,9 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS) target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt)) - # Keep a newly selected profile ceiling strict even when the carried target is above the ceiling. - if max_accel_override is not None: - target_accel = apply_accel_ceiling(target_accel, max_accel_override) - return target_accel -def select_accel_candidate(candidates): - """Select the lowest acceleration and keep its source and stop intent together.""" - return min(candidates, key=lambda candidate: candidate[0]) - - -def apply_accel_ceiling(accel: float, max_accel: float | None) -> float: - """Limit positive acceleration without reducing stock braking authority.""" - return min(accel, max_accel) if max_accel is not None and accel > 0.0 else accel - - class LongitudinalPlanner(LongitudinalPlannerSP): def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP @@ -159,12 +141,11 @@ class LongitudinalPlanner(LongitudinalPlannerSP): is_e2e = self.is_e2e(sm) - max_accel_override = self.get_max_accel_override(v_ego) 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) + self.CP, self.dt, accel_coast, self.allow_throttle) ungated_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, a_cruise_prev, steer_angle_without_offset, - self.CP, self.dt, accel_coast, True, max_accel_override) + self.CP, self.dt, accel_coast, True) self.a_cruise = self.arbitrate_cruise_candidate( sm, gated_cruise, ungated_cruise, output_a_target_mpc, self.mpc.source, allow_throttle=self.allow_throttle, e2e=is_e2e, force_decel=force_decel, @@ -176,12 +157,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP): if is_e2e: candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) - output_a_target, self.mpc.source, self.output_should_stop = select_accel_candidate(candidates) - - # Accel personality is a positive-acceleration ceiling, not a braking limit. Apply it after arbitration so - # lead/model/SCC candidates cannot bypass the selected profile, while all negative acceleration retains stock - # authority. - output_a_target = apply_accel_ceiling(output_a_target, max_accel_override) + output_a_target, self.mpc.source, self.output_should_stop = min(candidates, key=lambda candidate: candidate[0]) + output_a_target = self.accel_controller.limit_accel(output_a_target, v_ego) self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX) self.accel_controller_active = self.is_accel_controller_active(force_decel, self.output_a_target) @@ -209,7 +186,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP): longitudinalPlan.aTarget = float(self.output_a_target) longitudinalPlan.shouldStop = bool(self.output_should_stop) longitudinalPlan.allowBrake = True - # Raw model throttle intent used for path visualization; lead MPC can still request positive acceleration. longitudinalPlan.allowThrottle = bool(self.allow_throttle) pm.send('longitudinalPlan', plan_send) 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 4375f9ecea..6d5b7bd350 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -39,3 +39,8 @@ 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 limit_accel(self, accel: float, v_ego: float) -> float: + if not self.is_enabled() or accel <= 0.0: + return accel + return min(accel, self.get_max_accel(v_ego)) 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 e80352a908..4bc9425fd3 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 @@ -9,12 +9,7 @@ import numpy as np from opendbc.car.interfaces import ACCEL_MAX from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL from openpilot.common.test import OpenpilotTestCase -from openpilot.selfdrive.controls.lib.longitudinal_planner import ( - A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS, A_CRUISE_MIN, J_CRUISE_VALS, apply_accel_ceiling, - get_cruise_accel, select_accel_candidate, -) from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import ( AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, ) @@ -38,24 +33,36 @@ class TestAccelController(OpenpilotTestCase): def test_profile_ordering_and_bounds(self): controllers = { - AccelProfile.eco: self.set_profile(AccelProfile.eco), - AccelProfile.normal: self.set_profile(AccelProfile.normal), - AccelProfile.sport: self.set_profile(AccelProfile.sport), + profile: self.set_profile(profile) + for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport) } previous = {profile: float("inf") for profile in controllers} for speed in np.linspace(0.0, 55.0, 551): values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()} - assert 0.0 <= values[AccelProfile.eco] <= values[AccelProfile.normal] <= values[AccelProfile.sport] <= 2.0 + assert 0.0 <= values[AccelProfile.eco] <= values[AccelProfile.normal] <= values[AccelProfile.sport] <= ACCEL_MAX for profile, value in values.items(): assert value <= previous[profile] previous[profile] = value - def test_profiles_stay_within_openpilot_accel_max(self): - for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): - controller = self.set_profile(profile) - for speed in np.linspace(0.0, 55.0, 551): - assert controller.get_max_accel(speed) <= ACCEL_MAX + def test_profiles_keep_usable_road_speed_acceleration(self): + controllers = { + profile: self.set_profile(profile) + for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport) + } + for speed in np.linspace(8.0, 40.0, 321): + stock = float(np.interp(speed, [0.0, 10.0, 25.0, 40.0], [1.6, 1.2, 0.8, 0.6])) + values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()} + assert values[AccelProfile.eco] >= max(0.35, 0.60 * stock), speed + assert values[AccelProfile.normal] >= 0.80 * stock, speed + assert values[AccelProfile.sport] >= stock, speed + + def test_eco_never_exceeds_stock(self): + controller = self.set_profile(AccelProfile.eco) + stock_breakpoints = [0.0, 10.0, 25.0, 40.0] + stock_values = [1.6, 1.2, 0.8, 0.6] + for speed in np.linspace(0.0, 55.0, 551): + assert controller.get_max_accel(speed) <= np.interp(speed, stock_breakpoints, stock_values) + 1e-12 def test_profiles_have_material_separation(self): controllers = [self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)] @@ -63,61 +70,33 @@ class TestAccelController(OpenpilotTestCase): eco, normal, sport = (controller.get_max_accel(speed) for controller in controllers) assert normal - eco >= 0.1 - 1e-12 assert sport - normal >= 0.1 - 1e-12 - for speed in MAX_ACCEL_BREAKPOINTS[1:-1]: - assert controllers[2].get_max_accel(speed) - controllers[0].get_max_accel(speed) >= 0.3 - 1e-12 - - def test_profiles_keep_usable_road_speed_acceleration(self): - # A previous revision had eco at 0.20 m/s^2 at 40 m/s. 1% of road grade costs 0.098 m/s^2 of gravity, so - # that profile cannot hold speed on anything steeper than ~2% and can never recover once it bleeds off. - # This is a LOWER bound on purpose: the tapered upper bounds it replaces let highway accel go to zero. - controllers = {profile: self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)} - for speed in np.linspace(8.0, 40.0, 321): - stock = float(np.interp(speed, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)) - values = {profile: controller.get_max_accel(speed) for profile, controller in controllers.items()} - # 0.35 m/s^2 holds a 3% grade; the fractions keep merges and passes usable. - assert values[AccelProfile.eco] >= max(0.35, 0.60 * stock), speed - assert values[AccelProfile.normal] >= 0.80 * stock, speed - assert values[AccelProfile.sport] >= stock, speed - - def test_eco_never_exceeds_stock(self): - controller = self.set_profile(AccelProfile.eco) - for speed in np.linspace(0.0, 55.0, 551): - assert controller.get_max_accel(speed) <= float(np.interp(speed, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)) + 1e-12, speed - - def test_comfort_profile_caps_taper_after_launch(self): - for profile in (AccelProfile.eco, AccelProfile.normal): - values = MAX_ACCEL_PROFILES[profile] - assert values[3] <= 0.55 * values[0] def test_sport_uses_openpilot_accel_max_at_launch(self): controller = self.set_profile(AccelProfile.sport) assert controller.get_max_accel(0.0) == ACCEL_MAX - assert all(controller.get_max_accel(speed) <= ACCEL_MAX for speed in np.linspace(0.0, 55.0, 551)) - - def test_ceiling_is_continuous_in_speed(self): - # The ceiling is the only thing the controller sets, so a step in it is a step in the commanded - # acceleration. dt=10 makes the stock jerk limiter a no-op so nothing can hide a discontinuity. - for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): - controller = self.set_profile(profile) - speeds = np.linspace(0.0, 45.0, 451) - spacing = float(speeds[1] - speeds[0]) - commands = np.asarray([ - get_cruise_accel(False, 60.0, speed, 0.0, 0.0, _fake_cp(), 10.0, 0.0, True, controller.get_max_accel(speed)) - for speed in speeds - ]) - assert np.all(np.isfinite(commands)), profile - assert np.all(np.abs(np.diff(commands)) <= spacing * 1.05 + 1e-9), profile def test_negative_speed_uses_standstill_value(self): controller = self.set_profile(AccelProfile.sport) assert controller.get_max_accel(-1.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][0] + def test_limit_accel_only_limits_positive_values(self): + controller = self.set_profile(AccelProfile.eco) + assert controller.limit_accel(2.0, 0.0) == controller.get_max_accel(0.0) + assert controller.limit_accel(1.0, 0.0) == 1.0 + assert controller.limit_accel(-1.5, 0.0) == -1.5 + + def test_disabled_limit_is_passthrough(self): + controller = self.set_profile(AccelProfile.eco) + self.params.put_bool("AccelPersonalityEnabled", False, block=True) + controller.update() + assert controller.limit_accel(1.5, 0.0) == 1.5 + assert controller.limit_accel(-1.5, 0.0) == -1.5 + def test_profile_change_refreshes_ceiling(self): controller = self.set_profile(AccelProfile.normal) self.params.put("AccelPersonality", AccelProfile.sport, block=True) controller.update() - index = MAX_ACCEL_BREAKPOINTS.index(10.0) - assert controller.get_max_accel(10.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][index] + assert controller.get_max_accel(10.0) == MAX_ACCEL_PROFILES[AccelProfile.sport][3] def test_params_refresh_every_update(self): controller = self.set_profile(AccelProfile.normal) @@ -130,191 +109,3 @@ class TestAccelController(OpenpilotTestCase): self.params.put_bool("AccelPersonalityEnabled", False, block=True) controller.update() assert not controller.is_enabled() - - -class TestPlannerIntegration(OpenpilotTestCase): - def setUp(self): - self.params = Params() - self.params.put_bool("AccelPersonalityEnabled", False, block=True) - - def test_candidate_selection_keeps_stop_intent_with_acceleration_source(self): - candidates = [ - (-0.2, 1, True), - (0.3, 0, False), - ] - assert select_accel_candidate(candidates) == (-0.2, 1, True) - - # A losing stop request must not force LongControl into its stopping ramp. - candidates = [ - (0.3, 0, False), - (0.4, 1, True), - ] - assert select_accel_candidate(candidates) == (0.3, 0, False) - - def test_profile_ceiling_limits_positive_targets_without_limiting_braking(self): - assert apply_accel_ceiling(1.5, 0.8) == 0.8 - assert apply_accel_ceiling(-1.5, 0.8) == -1.5 - assert apply_accel_ceiling(1.5, None) == 1.5 - - def test_none_override_matches_stock(self): - for e2e in (False, True): - for allow_throttle in (False, True): - 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_far_braking(self): - # The ceiling is an upper bound only, so it can never participate in a deceleration. Braking authority - # stays with stock's clip to A_CRUISE_MIN for every profile. - 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 - for profile_values in MAX_ACCEL_PROFILES.values(): - assert get_cruise_accel(*args, max_accel_override=profile_values[0]) == stock - - def test_stock_jerk_limit_still_owns_smoothing(self): - speed = 8.0 - sport_limit = np.interp(speed, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.sport]) - target = get_cruise_accel(False, 30.0, speed, 0.0, 0.0, _fake_cp(), DT_MDL, 0.0, True, sport_limit) - jerk_limit = np.interp(speed, A_CRUISE_MAX_BP, J_CRUISE_VALS) * DT_MDL - assert np.isclose(target, jerk_limit) - - def test_disabled_leaves_stock_limit_active(self): - planner = _bare_planner() - assert planner.get_max_accel_override(5.0) is None - assert planner.accel_controller_active is False - - 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) == 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) == 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 - - 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() - expected = np.interp(5.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal]) - assert planner.get_max_accel_override(5.0) == expected - - def test_ceiling_applies_to_every_target_source(self): - from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource - - self.params.put_bool("AccelPersonalityEnabled", True, block=True) - planner = _bare_planner() - speed = 29.0 - expected = planner.accel_controller.get_max_accel(speed) - - for source in (LongitudinalPlanSource.cruise, LongitudinalPlanSource.sccVision, - LongitudinalPlanSource.sccMap, LongitudinalPlanSource.speedLimitAssist): - planner.source = source - assert np.isclose(planner.get_max_accel_override(speed), expected), source - - 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() - planner.allow_throttle = False - assert planner.get_max_accel_override(12.0) == planner.accel_controller.get_max_accel(12.0) - - 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) - - self.params.put("AccelPersonality", AccelProfile.eco, block=True) - 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) - assert previous > ceiling - assert np.isclose(accel, ceiling) - assert accel <= ceiling - assert planner.a_cruise == previous - - def test_model_source_selection_preserves_decel_policy(self): - # Route 000005dd: e2e -> lead1 stepped +2.25 m/s^2 in one frame (45 m/s^3) and back the next, while the - # model held desiredAcceleration at -1.63 and never moved more than 0.024. Dropping a candidate the model - # still owns is what produced the brake/gas/brake flip. - from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import E2E_BRAKE_HOLD_ACCEL, MpcPlanSource - - planner = _bare_planner() - - class _Mpc: - source = MpcPlanSource.cruise - - class _Dec: - def __init__(self): - self._active = True - self._mode = "acc" - - def active(self): - return self._active - - def mode(self): - return self._mode - - planner.mpc = _Mpc() - planner.dec = _Dec() - - def sm(experimental: bool, model_accel: float): - return { - 'selfdriveState': type("S", (), {"experimentalMode": experimental})(), - 'modelV2': type("M", (), {"action": type("A", (), {"desiredAcceleration": model_accel})()})(), - } - - braking = E2E_BRAKE_HOLD_ACCEL - 1.0 - - # Not experimental: never e2e, whatever the model wants. - assert planner.is_e2e(sm(False, braking)) is False - - # DEC in acc, and the model was NOT the selected source: acc stands. This is the case that must stay - # untouched, or a phantom model brake could be pulled into the arbitration that never won it. - planner.mpc.source = MpcPlanSource.lead0 - assert planner.is_e2e(sm(True, braking)) is False - - # DEC in acc, model WAS selected and is still braking: hold it rather than release the brake. - planner.mpc.source = MpcPlanSource.e2e - assert planner.is_e2e(sm(True, braking)) is True - - # Still selected but no longer braking: release, DEC's decision stands. - assert planner.is_e2e(sm(True, 0.0)) is False - assert planner.is_e2e(sm(True, E2E_BRAKE_HOLD_ACCEL + 0.01)) is False - - # DEC blended, or DEC inactive, is unconditionally e2e as before. - planner.dec._mode = "blended" - assert planner.is_e2e(sm(True, 1.0)) is True - planner.dec._mode = "acc" - planner.dec._active = False - assert planner.is_e2e(sm(True, 1.0)) is True - - -def _fake_cp(): - class CP: - steerRatio = 15.0 - wheelbase = 2.7 - - return CP() - - -def _bare_planner(): - 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.a_cruise = 0.0 - 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 c01258db06..a279d25075 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 @@ -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, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel +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, @@ -52,8 +52,8 @@ def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cru for frame in range(steps): 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 - accel = get_cruise_accel(e2e, target_speed, measured, accel, 0.0, CarParams(), DT_MDL, 2.0, True, max_accel_override) + accel = get_cruise_accel(e2e, target_speed, measured, accel, 0.0, CarParams(), DT_MDL, 2.0, True) + accel = controller.limit_accel(accel, measured) speed = max(0.0, speed + accel * DT_MDL) rows.append((speed, accel, should_stop(speed, accel))) return rows @@ -96,12 +96,6 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase): self.assertGreaterEqual(time_to_50[AccelProfile.eco] - time_to_50[AccelProfile.normal], 2.0) self.assertGreaterEqual(time_to_50[AccelProfile.normal] - time_to_50[AccelProfile.sport], 3.0) - # Asserted against stock rather than against the actuator's rate limit. The peak command jerk in this run - # is stock's stop-release ramp at launch (LongCtrlState.stopping -> pid), which on its own already exceeds - # PRIUS_TSS2_ROUTE_MODEL.command_rate_limit: measured 4.392 for stock and for all three profiles alike. - # An absolute bound here would only be testing that stock ramp, and would pass or fail on stock changes - # that have nothing to do with the profiles. What this test can honestly own is that the profiles add no - # command jerk of their own. stock_peak_jerk = float(np.max(np.abs(np.diff(stock[:, 3])) / DT_MDL)) for profile, trace in traces.items(): command_jerk = np.abs(np.diff(trace[:, 3])) / DT_MDL @@ -133,78 +127,17 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase): self.assertAlmostEqual(settled["a_target"], eco_limit, delta=0.01) self.assertLess(settled["a_target"], settled["model_action"]["desiredAcceleration"]) - 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) - - 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=19.5) for _ in range(10)] - - self.assertTrue(all(row["mpc_source"] == LongitudinalPlanSource.e2e for row in traces[True])) - 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]]) - - 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_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) - - 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=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"): - self.assertEqual([row[key] for row in traces[True]], [row[key] for row in traces[False]]) - def test_normal_launch_is_faster_than_eco(self): eco = run_profile(AccelProfile.eco, speed=4.0, steps=120) normal = run_profile(AccelProfile.normal, speed=4.0, steps=120) self.assertGreater(normal[-1][0], eco[-1][0]) 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_large_cruise_decel_retains_stock_authority(self): - for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport): - 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_cruise_decel_remains_stock_for_all_profiles(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)) @@ -253,8 +186,6 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase): } stock_first_motion = next(frame for frame, row in enumerate(stock) if row[0] > 0.01) - # No launch dead time: motion starts on the same frame as stock, because the ceiling only ever bounds the - # command from above and stock's own law owns the first frame. self.assertEqual(len(set(first_motion.values())), 1) self.assertTrue(all(frame == stock_first_motion for frame in first_motion.values())) self.assertGreaterEqual(time_to_five[AccelProfile.eco] - time_to_five[AccelProfile.normal], 0.1) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 9ebc2a0bb7..b652b0e86c 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -51,24 +51,13 @@ class LongitudinalPlannerSP: if not self.dec.active() or self.dec.mode() == "blended": return True - # hold a brake the model already owns rather than release it mid-brake; min() means this can only ever - # add deceleration, and never one that was not already the selected source if self.mpc.source == MpcPlanSource.e2e and sm['modelV2'].action.desiredAcceleration < E2E_BRAKE_HOLD_ACCEL: return True return False - def get_max_accel_override(self, v_ego: float) -> float | None: - if not self.accel_controller.is_enabled(): - return None - - return self.accel_controller.get_max_accel(v_ego) - - def is_accel_controller_active(self, force_decel: bool, accel_target: float | None = None) -> bool: - # The profile ceiling is applied after arbitration, so a lead/model/SCC winner can still be profile-controlled. - # Braking remains owned by the selected safety source and is not reported as profile activity. - return bool(self.accel_controller.is_enabled() and not force_decel and - (accel_target is None or accel_target >= 0.0)) + def is_accel_controller_active(self, force_decel: bool, accel_target: float) -> bool: + return bool(self.accel_controller.is_enabled() and not force_decel and accel_target >= 0.0) 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)