diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 39cfda4e8a..bad37add63 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -4,6 +4,7 @@ import time import numpy as np from cereal import log from openpilot.common.numpy_fast import clip +from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function @@ -220,8 +221,9 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, mode='acc'): + def __init__(self, mode='acc', dt=DT_MDL): self.mode = mode + self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] @@ -440,7 +442,7 @@ class LongitudinalMpc: self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] - self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) + self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution) t = time.monotonic() if self.solution_status != 0: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6cc6e80d3d..9113bb5a96 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -47,7 +47,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP - self.mpc = LongitudinalMpc() + self.mpc = LongitudinalMpc(dt=dt) self.fcw = False self.dt = dt