Prevent solver faults during early mode transitions

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):

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)