From 39ef918cbb596b21010fc2b9e823f14e3e58b241 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 11 Nov 2026 15:29:50 -0600 Subject: [PATCH] Give Faster Takeoffs its full low-speed acceleration budget --- .../controls/lib/longitudinal_planner.py | 9 +-- .../tests/test_fast_takeoff_acceleration.py | 66 +++++++++++++++++++ prebuilt.json | 2 +- 3 files changed, 72 insertions(+), 5 deletions(-) create mode 100644 openpilot/starpilot/longitudinal/tests/test_fast_takeoff_acceleration.py diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 7ec6aa4fc1..6f12d9a36f 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -59,8 +59,9 @@ _A_TOTAL_MAX_BP = [20., 40.] _TAKEOFF_TOTAL_BP = [0., 2., 5., 20., 40.] _TAKEOFF_TOTAL_MAX = [3.5, 3.5, 1.7, 1.7, 3.2] -def get_max_accel(v_ego): - return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) +def get_max_accel(v_ego, faster_takeoffs=False): + max_accel = np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) + return np.interp(v_ego, [2., 5.], [ACCEL_MAX, max_accel]) if faster_takeoffs else max_accel 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 @@ -69,7 +70,7 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, profile: AppliedProfile | None = None, base_brake_floor: float = A_CRUISE_MIN, selected_acceleration_max: float | None = None, faster_takeoffs: bool = False, *, a_target_prev: float | None = None): - max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego) + max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego, faster_takeoffs) if selected_acceleration_max is not None and not e2e: max_accel = selected_acceleration_max if profile is not None: @@ -442,7 +443,7 @@ class LongitudinalPlanner: force_stop=self.force_stop_plan.forcing, traffic_mode=traffic_mode, accel_min=ACCEL_MIN) if faster_lead_takeoff: # Dom takeoff bypasses the standstill cruise slew, never its final bounds. - takeoff_max = ACCEL_MAX if sm['selfdriveState'].experimentalMode else get_max_accel(v_ego) + takeoff_max = ACCEL_MAX if sm['selfdriveState'].experimentalMode else get_max_accel(v_ego, faster_takeoffs) if selected_acceleration is not None and not sm['selfdriveState'].experimentalMode: takeoff_max = selected_acceleration if cruise_profile is not None: diff --git a/openpilot/starpilot/longitudinal/tests/test_fast_takeoff_acceleration.py b/openpilot/starpilot/longitudinal/tests/test_fast_takeoff_acceleration.py new file mode 100644 index 0000000000..f6d3d2afdc --- /dev/null +++ b/openpilot/starpilot/longitudinal/tests/test_fast_takeoff_acceleration.py @@ -0,0 +1,66 @@ +import numpy as np +import pytest + +from opendbc.car.interfaces import ACCEL_MAX +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_cruise_accel, get_max_accel +from openpilot.starpilot.longitudinal.tests.test_conditional_handoff import Frame, NOW, DRIVE +from openpilot.starpilot.longitudinal.tests.test_ioniq6_start import candidate + + +@pytest.mark.parametrize('speed', [0., .5, 1., 2., 3., 4.]) +def test_fast_takeoffs_raise_low_speed_default_within_existing_global_limit(speed): + assert get_max_accel(speed, True) > get_max_accel(speed) + assert get_max_accel(speed, True) <= ACCEL_MAX + + +@pytest.mark.parametrize('speed', [5., 6., 10., 15., 25., 40., 50.]) +def test_fast_takeoffs_rejoin_unchanged_normal_acceleration_by_5ms(speed): + assert get_max_accel(speed, True) == get_max_accel(speed) + + +@pytest.mark.parametrize('cap', [.2, .8, 1.2]) +def test_saved_lower_cap_and_cruise_ceiling_still_bound_fast_takeoff(cap): + cp = candidate()[1] + assert get_cruise_accel(False, 30., 0., cap, 0., cp, .05, 0., True, + selected_acceleration_max=cap, faster_takeoffs=True) == cap + assert get_cruise_accel(False, .1, 0., .1, 0., cp, .05, 0., True, + faster_takeoffs=True) == .1 + + +def test_toggle_does_not_change_braking_or_experimental_acceleration_ceiling(): + cp = candidate()[1] + for e2e in (False, True): + for set_speed in (0., 1.): + args = (e2e, set_speed, 3., -1., 0., cp, .05, 0., True) + assert get_cruise_accel(*args, faster_takeoffs=True) == get_cruise_accel(*args) + args = (True, 30., 0., 2., 0., cp, .05, 0., True) + assert get_cruise_accel(*args, faster_takeoffs=True) == get_cruise_accel(*args) + + +def test_real_ioniq_planner_accelerates_faster_after_release_but_keeps_model_stop(): + cp = candidate()[1] + normal, faster = (LongitudinalPlanner(cp) for _ in range(2)) + outputs = [] + for tick in range(65): + now = NOW + tick * 50_000_000 + sm = Frame() + sm.logMonoTime = dict.fromkeys(sm, now - 5_000_000) + sm.recv_time = dict.fromkeys(sm, (now - 5_000_000) / 1e9) + sm['carState'].vEgo = 0. + sm['carState'].standstill = True + sm['carState'].vCruise = 72. + sm['selfdriveState'].experimentalMode = tick < 8 + sm['modelV2'].action.shouldStop = tick < 8 + sm['modelV2'].action.desiredAcceleration = -.04 if tick < 8 else 2.1 + for planner, enabled in ((normal, False), (faster, True)): + planner.update(sm, faster_lead_takeoff=enabled, takeoff_drive_id=DRIVE, now_ns=now, drive_id=DRIVE) + assert planner.mpc.solution_status == 0 + assert planner.output_a_target <= ACCEL_MAX + if tick < 8: + assert planner.output_should_stop + assert planner.output_a_target <= 0. + outputs.append((float(normal.output_a_target), float(faster.output_a_target))) + assert outputs[-1][1] > outputs[-1][0] + .35 + assert outputs[-1] == pytest.approx((1.6, 2.0)) + assert not faster.output_should_stop + assert np.isfinite(outputs).all() diff --git a/prebuilt.json b/prebuilt.json index 9543d10b01..f6fa51b928 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "ca8a4ef82a15b4bc15bee5cf3c965227c2fd635c519325b202d8e218552d36ec", "schema_version": 1} +{"protected_inventory_sha256": "565be1e4c8d41d8a8c2b1d8d51e94c60fc784d0607da7c6e02132780043fea44", "schema_version": 1}