diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 29e4c21f251d85..ac2ff25b5095a8 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -21,14 +21,14 @@ def __init__(self, mpc_id): def reset_mpc(self): - self.libmpc = libmpc_py.libmpc + ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id) if self.mpc_id == 0: self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0) else: self.libmpc.init(1.0, 1.0, 0.0, 5.0, 10000.0) - self.mpc_solution = libmpc_py.ffi.new("log_t *") - self.cur_state = libmpc_py.ffi.new("state_t *") + self.mpc_solution = ffi.new("log_t *") + self.cur_state = ffi.new("state_t *") self.cur_state[0].x_ego = 0 self.cur_state[0].v_ego = 0 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 19387d4ae13aca..649cce706577ad 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -44,4 +44,5 @@ if GetOption('mpc_generate'): mpc_files = ["longitudinal_mpc.c"] + generated_c -env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py index 3b15b08feef404..b3c5bbf7a1ae94 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py @@ -4,31 +4,38 @@ from common.ffi_wrapper import suffix mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) -libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix()) -ffi = FFI() -ffi.cdef(""" -const int MPC_N = 32; +def _get_libmpc(mpc_id): + libmpc_fn = os.path.join(mpc_dir, "libmpc%d%s" % (mpc_id, suffix())) -typedef struct { -double x_ego, v_ego, a_ego; -} state_t; + ffi = FFI() + ffi.cdef(""" + const int MPC_N = 32; + typedef struct { + double x_ego, v_ego, a_ego; + } state_t; -typedef struct { -double x_ego[MPC_N+1]; -double v_ego[MPC_N+1]; -double a_ego[MPC_N+1]; -double t[MPC_N+1]; -double j_ego[MPC_N]; -double cost; -} log_t; + typedef struct { + double x_ego[MPC_N+1]; + double v_ego[MPC_N+1]; + double a_ego[MPC_N+1]; + double t[MPC_N+1]; + double j_ego[MPC_N]; + double cost; + } log_t; -void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost); -int run_mpc(state_t * x0, log_t * solution, - double target_x[MPC_N+1], double target_v[MPC_N+1], double target_a[MPC_N+1], - double min_a, double max_a); -""") -libmpc = ffi.dlopen(libmpc_fn) + void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost); + int run_mpc(state_t * x0, log_t * solution, + double target_x[MPC_N+1], double target_v[MPC_N+1], double target_a[MPC_N+1], + double min_a, double max_a); + """) + + return (ffi, ffi.dlopen(libmpc_fn)) + +mpcs = [_get_libmpc(0), _get_libmpc(1)] + +def get_libmpc(mpc_id): + return mpcs[mpc_id]