fix jerky cruise control (#456)

Author: sshane

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

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)

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]