From 828f36210ca00494e190e135d10512bbfe2f72c5 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Fri, 17 Jul 2026 14:33:17 -0700 Subject: [PATCH] Prevent solver faults during early mode transitions --- .../lib/longitudinal_mpc_lib/long_mpc.py | 9 ++++---- .../test_accel_controller_closed_loop.py | 23 +++++++++++++++++++ 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index d5dafe3f65..d5ea6a8968 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -292,17 +292,18 @@ class LongitudinalMpc: a_guess[0] = self.x0[2] v_guess = np.zeros(N + 1) x_guess = np.zeros(N + 1) + j_guess = np.zeros(N) v_guess[0] = max(self.x0[1], 0.0) x_guess[0] = self.x0[0] for i in range(1, N + 1): dt = T_IDXS[i] - T_IDXS[i - 1] - v_guess[i] = max(0.0, v_guess[i - 1] + 0.5 * (a_guess[i - 1] + a_guess[i]) * dt) - x_guess[i] = x_guess[i - 1] + 0.5 * (v_guess[i - 1] + v_guess[i]) * dt + j_guess[i - 1] = (a_guess[i] - a_guess[i - 1]) / dt + x_guess[i] = x_guess[i - 1] + v_guess[i - 1] * dt + 0.5 * a_guess[i - 1] * dt**2 + j_guess[i - 1] * dt**3 / 6.0 + v_guess[i] = max(0.0, v_guess[i - 1] + a_guess[i - 1] * dt + 0.5 * j_guess[i - 1] * dt**2) for i in range(N + 1): self.solver.set(i, "x", np.array([x_guess[i], v_guess[i], a_guess[i]])) for i in range(N): - dt = T_IDXS[i + 1] - T_IDXS[i] - self.solver.set(i, "u", np.array([(a_guess[i + 1] - a_guess[i]) / dt])) + self.solver.set(i, "u", np.array([j_guess[i]])) @staticmethod def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau): diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py index c7ef0fa75e..473158704c 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -717,3 +717,26 @@ def test_solver_fault_keeps_restrictive_lead_target_until_recovery(): plant.step(v_lead=15.0, v_cruise=30.0) assert plant.planner.accel_controller_result.active + + +@pytest.mark.parametrize("pre_frames", (1, 2)) +@pytest.mark.parametrize("mode", ("disabled", "e2e")) +def test_early_launch_transition_returns_to_stock_without_solver_fault(pre_frames, mode): + _set_params(enabled=True, profile=1) + plant = Plant(speed=0.0, actuator_delay=0.15, actuator_lag=0.20) + for _ in range(pre_frames): + plant.step(v_cruise=15.0) + + if mode == "disabled": + plant.planner.accel_personality_enabled = False + plant.planner._read_accel_controller_params = lambda: None + else: + plant.e2e = True + + for _ in range(4): + plant.step(v_cruise=15.0) + controller = plant.planner.accel_controller_result + assert not controller.active + assert controller.mpc_accel_max is None + assert plant.planner.mpc.last_solution_status == 0 + np.testing.assert_array_equal(plant.planner.mpc.params[:, 1], ACCEL_MAX)