Compile FrogPilot

selfdrive/controls/controlsd.py

@@ -986,7 +986,7 @@ class Controls:
             self.params_memory.put_int("CurrentRandomEvent", 1)
             self.random_event_triggered = True
           else:
-            lac_log.active and self.events.add(EventName.frogSteerSaturated if self.goat_scream else EventName.steerSaturated)
+            lac_log.active and self.events.add(EventName.frogSteerSaturated if self.goat_scream and self.params_memory.get_int("CurrentHolidayTheme") == 0 else EventName.steerSaturated)
       elif lac_log.saturated:
         # TODO probably should not use dpath_points but curvature
         dpath_points = model_v2.position.y

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -1,89 +0,0 @@
-Import('env', 'envCython', 'arch')
-
-gen = "c_generated_code"
-
-casadi_model = [
-  f'{gen}/lat_model/lat_expl_ode_fun.c',
-  f'{gen}/lat_model/lat_expl_vde_forw.c',
-]
-
-casadi_cost_y = [
-  f'{gen}/lat_cost/lat_cost_y_fun.c',
-  f'{gen}/lat_cost/lat_cost_y_fun_jac_ut_xt.c',
-  f'{gen}/lat_cost/lat_cost_y_hess.c',
-]
-
-casadi_cost_e = [
-  f'{gen}/lat_cost/lat_cost_y_e_fun.c',
-  f'{gen}/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',
-  f'{gen}/lat_cost/lat_cost_y_e_hess.c',
-]
-
-casadi_cost_0 = [
-  f'{gen}/lat_cost/lat_cost_y_0_fun.c',
-  f'{gen}/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',
-  f'{gen}/lat_cost/lat_cost_y_0_hess.c',
-]
-
-build_files = [f'{gen}/acados_solver_lat.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
-
-# extra generated files used to trigger a rebuild
-generated_files = [
-  f'{gen}/Makefile',
-
-  f'{gen}/main_lat.c',
-  f'{gen}/main_sim_lat.c',
-  f'{gen}/acados_solver_lat.h',
-  f'{gen}/acados_sim_solver_lat.h',
-  f'{gen}/acados_sim_solver_lat.c',
-  f'{gen}/acados_solver.pxd',
-
-  f'{gen}/lat_model/lat_expl_vde_adj.c',
-
-  f'{gen}/lat_model/lat_model.h',
-  f'{gen}/lat_constraints/lat_constraints.h',
-  f'{gen}/lat_cost/lat_cost.h',
-] + build_files
-
-acados_dir = '#third_party/acados'
-acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
-
-source_list = ['lat_mpc.py',
-  '#selfdrive/modeld/constants.py',
-  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
-  f'{acados_templates_dir}/acados_solver.in.c',
-]
-
-lenv = env.Clone()
-lenv.Clean(generated_files, Dir(gen))
-
-generated_lat = lenv.Command(generated_files,
-                             source_list,
-                             f"cd {Dir('.').abspath} && python3 lat_mpc.py")
-
-lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
-lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
-lenv["CCFLAGS"].append("-Wno-unused")
-if arch != "Darwin":
-  lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
-lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
-                                build_files,
-                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
-
-# generate cython stuff
-acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
-acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd")
-libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
-libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
-
-lenv2 = envCython.Clone()
-lenv2["LINKFLAGS"] += [lib_solver[0].get_labspath()]
-lenv2.Command(libacados_ocp_solver_c,
-  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
-  f'cython' + \
-  f' -o {libacados_ocp_solver_c.get_labspath()}' + \
-  f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
-  f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
-  f' {acados_ocp_solver_pyx.get_labspath()}')
-lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c])
-lenv2.Depends(lib_cython, lib_solver)

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/Makefile

@@ -0,0 +1,211 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+
+
+
+# define sources and use make's implicit rules to generate object files (*.o)
+
+# model
+MODEL_SRC=
+MODEL_SRC+= lat_model/lat_expl_ode_fun.c
+MODEL_SRC+= lat_model/lat_expl_vde_forw.c
+MODEL_SRC+= lat_model/lat_expl_vde_adj.c
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
+
+# optimal control problem - mostly CasADi exports
+OCP_SRC=
+OCP_SRC+= lat_cost/lat_cost_y_0_fun.c
+OCP_SRC+= lat_cost/lat_cost_y_0_fun_jac_ut_xt.c
+OCP_SRC+= lat_cost/lat_cost_y_0_hess.c
+OCP_SRC+= lat_cost/lat_cost_y_fun.c
+OCP_SRC+= lat_cost/lat_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= lat_cost/lat_cost_y_hess.c
+OCP_SRC+= lat_cost/lat_cost_y_e_fun.c
+OCP_SRC+= lat_cost/lat_cost_y_e_fun_jac_ut_xt.c
+OCP_SRC+= lat_cost/lat_cost_y_e_hess.c
+
+OCP_SRC+= acados_solver_lat.c
+OCP_OBJ := $(OCP_SRC:.c=.o)
+
+# for sim solver
+SIM_SRC= acados_sim_solver_lat.c
+SIM_OBJ := $(SIM_SRC:.c=.o)
+
+# for target example
+EX_SRC= main_lat.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
+
+# for target example_sim
+EX_SIM_SRC= main_sim_lat.c
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
+
+# combine model, sim and ocp object files
+OBJ=
+OBJ+= $(MODEL_OBJ)
+OBJ+= $(SIM_OBJ)
+OBJ+= $(OCP_OBJ)
+
+EXTERNAL_DIR=
+EXTERNAL_LIB=
+
+INCLUDE_PATH = /data/openpilot/third_party/acados/include
+LIB_PATH = /data/openpilot/third_party/acados/lib
+
+# preprocessor flags for make's implicit rules
+CPPFLAGS+= -I$(INCLUDE_PATH)
+CPPFLAGS+= -I$(INCLUDE_PATH)/acados
+CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
+CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
+
+
+# define the c-compiler flags for make's implicit rules
+CFLAGS = -fPIC -std=c99   -O2#-fno-diagnostics-show-line-numbers -g
+# # Debugging
+# CFLAGS += -g3
+
+# linker flags
+LDFLAGS+= -L$(LIB_PATH)
+
+# link to libraries
+LDLIBS+= -lacados
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= 
+
+# libraries
+LIBACADOS_SOLVER=libacados_solver_lat.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_lat.so
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
+
+# virtual targets
+.PHONY : all clean
+
+#all: clean example_sim example shared_lib
+
+all: clean example_sim example
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
+
+# some linker targets
+example: $(EX_OBJ) $(OBJ)
+	$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
+
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
+
+bundled_shared_lib: $(OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
+
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+
+# Cython targets
+ocp_cython_c: ocp_shared_lib
+	cython \
+	-o acados_ocp_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code \
+
+ocp_cython_o: ocp_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_ocp_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
+	-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
+	acados_ocp_solver_pyx.c \
+
+ocp_cython: ocp_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_ocp_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_ocp_solver_pyx.o \
+	$(abspath .)/libacados_ocp_solver_lat.so \
+	$(LDFLAGS) $(LDLIBS)
+
+# Sim Cython targets
+sim_cython_c: sim_shared_lib
+	cython \
+	-o acados_sim_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code \
+
+sim_cython_o: sim_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_sim_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
+	-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
+	acados_sim_solver_pyx.c \
+
+sim_cython: sim_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_sim_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_sim_solver_pyx.o \
+	$(abspath .)/libacados_sim_solver_lat.so \
+	$(LDFLAGS) $(LDLIBS)
+
+clean:
+	$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
+
+clean_ocp_shared_lib:
+	$(RM) $(LIBACADOS_OCP_SOLVER)
+	$(RM) $(OCP_OBJ)
+
+clean_ocp_cython:
+	$(RM) libacados_ocp_solver_lat.so
+	$(RM) acados_solver_lat.o
+	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx.o
+
+clean_sim_cython:
+	$(RM) libacados_sim_solver_lat.so
+	$(RM) acados_sim_solver_lat.o
+	$(RM) acados_sim_solver_pyx.so
+	$(RM) acados_sim_solver_pyx.o

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_sim_solver_lat.c

@@ -0,0 +1,290 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "lat_model/lat_model.h"
+#include "acados_sim_solver_lat.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * lat_acados_sim_solver_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+    sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int lat_acados_sim_create(sim_solver_capsule * capsule)
+{
+    // initialize
+    const int nx = LAT_NX;
+    const int nu = LAT_NU;
+    const int nz = LAT_NZ;
+    const int np = LAT_NP;
+    bool tmp_bool;
+
+    
+    double Tsim = 0.009765625;
+
+    
+    // explicit ode
+    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
+    capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
+    capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
+    capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
+    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+    capsule->sim_vde_adj_casadi->casadi_fun = &lat_expl_vde_adj;
+    capsule->sim_vde_adj_casadi->casadi_n_in = &lat_expl_vde_adj_n_in;
+    capsule->sim_vde_adj_casadi->casadi_n_out = &lat_expl_vde_adj_n_out;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_in = &lat_expl_vde_adj_sparsity_in;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_out = &lat_expl_vde_adj_sparsity_out;
+    capsule->sim_vde_adj_casadi->casadi_work = &lat_expl_vde_adj_work;
+    external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
+
+    capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
+    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+    
+
+    // sim plan & config
+    sim_solver_plan_t plan;
+    plan.sim_solver = ERK;
+
+    // create correct config based on plan
+    sim_config * lat_sim_config = sim_config_create(plan);
+    capsule->acados_sim_config = lat_sim_config;
+
+    // sim dims
+    void *lat_sim_dims = sim_dims_create(lat_sim_config);
+    capsule->acados_sim_dims = lat_sim_dims;
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
+    sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
+
+
+    // sim opts
+    sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_opts = lat_sim_opts;
+    int tmp_int = 3;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
+    double tmp_double = 0.0;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "newton_tol", &tmp_double);
+    sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
+
+ 
+    tmp_int = 4;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
+    tmp_int = 1;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
+    tmp_bool = 0;
+    sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
+
+
+    // sim in / out
+    sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_in = lat_sim_in;
+    sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
+    capsule->acados_sim_out = lat_sim_out;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "T", &Tsim);
+
+    // model functions
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_vde_forw", capsule->sim_forw_vde_casadi);
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_vde_adj", capsule->sim_vde_adj_casadi);
+    lat_sim_config->model_set(lat_sim_in->model,
+                 "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+    // sim solver
+    sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
+                                               lat_sim_dims, lat_sim_opts);
+    capsule->acados_sim_solver = lat_sim_solver;
+
+
+    /* initialize parameter values */
+    double* p = calloc(np, sizeof(double));
+    
+
+    lat_acados_sim_update_params(capsule, p, np);
+    free(p);
+
+
+    /* initialize input */
+    // x
+    double x0[4];
+    for (int ii = 0; ii < 4; ii++)
+        x0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "x", x0);
+
+
+    // u
+    double u0[1];
+    for (int ii = 0; ii < 1; ii++)
+        u0[ii] = 0.0;
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "u", u0);
+
+    // S_forw
+    double S_forw[20];
+    for (int ii = 0; ii < 20; ii++)
+        S_forw[ii] = 0.0;
+    for (int ii = 0; ii < 4; ii++)
+        S_forw[ii + ii * 4 ] = 1.0;
+
+
+    sim_in_set(lat_sim_config, lat_sim_dims,
+               lat_sim_in, "S_forw", S_forw);
+
+    int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
+
+    return status;
+}
+
+
+int lat_acados_sim_solve(sim_solver_capsule *capsule)
+{
+    // integrate dynamics using acados sim_solver
+    int status = sim_solve(capsule->acados_sim_solver,
+                           capsule->acados_sim_in, capsule->acados_sim_out);
+    if (status != 0)
+        printf("error in lat_acados_sim_solve()! Exiting.\n");
+
+    return status;
+}
+
+
+int lat_acados_sim_free(sim_solver_capsule *capsule)
+{
+    // free memory
+    sim_solver_destroy(capsule->acados_sim_solver);
+    sim_in_destroy(capsule->acados_sim_in);
+    sim_out_destroy(capsule->acados_sim_out);
+    sim_opts_destroy(capsule->acados_sim_opts);
+    sim_dims_destroy(capsule->acados_sim_dims);
+    sim_config_destroy(capsule->acados_sim_config);
+
+    // free external function
+    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+    external_function_param_casadi_free(capsule->sim_vde_adj_casadi);
+    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+    return 0;
+}
+
+
+int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+    int status = 0;
+    int casadi_np = LAT_NP;
+
+    if (casadi_np != np) {
+        printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+    return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_config;
+};
+
+sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_in;
+};
+
+sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_out;
+};
+
+void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_dims;
+};
+
+sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_opts;
+};
+
+sim_solver  * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_solver;
+};
+

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver.pxd

@@ -0,0 +1,62 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+cimport acados_solver_common
+
+cdef extern from "acados_solver_lat.h":
+    ctypedef struct nlp_solver_capsule "lat_solver_capsule":
+        pass
+
+    nlp_solver_capsule * acados_create_capsule "lat_acados_create_capsule"()
+    int acados_free_capsule "lat_acados_free_capsule"(nlp_solver_capsule *capsule)
+
+    int acados_create "lat_acados_create"(nlp_solver_capsule * capsule)
+
+    int acados_create_with_discretization "lat_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
+    int acados_update_time_steps "lat_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
+    int acados_update_qp_solver_cond_N "lat_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
+
+    int acados_update_params "lat_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
+    int acados_update_params_sparse "lat_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
+    int acados_solve "lat_acados_solve"(nlp_solver_capsule * capsule)
+    int acados_reset "lat_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
+    int acados_free "lat_acados_free"(nlp_solver_capsule * capsule)
+    void acados_print_stats "lat_acados_print_stats"(nlp_solver_capsule * capsule)
+
+    int acados_custom_update "lat_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
+
+    acados_solver_common.ocp_nlp_in *acados_get_nlp_in "lat_acados_get_nlp_in"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_nlp_out "lat_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_sens_out "lat_acados_get_sens_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "lat_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_config *acados_get_nlp_config "lat_acados_get_nlp_config"(nlp_solver_capsule * capsule)
+    void *acados_get_nlp_opts "lat_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "lat_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "lat_acados_get_nlp_plan"(nlp_solver_capsule * capsule)

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun.c

@@ -0,0 +1,167 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_0_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+
+/* lat_cost_y_0_fun:(i0[4],i1,i2[],i3[2])->(o0[5]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=10.;
+  a1=(a0+a1);
+  a2=arg[0]? arg[0][2] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[0]? arg[0][3] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  a1=(a1*a2);
+  if (res[0]!=0) res[0][3]=a1;
+  a1=1.0000000000000001e-01;
+  a0=(a0+a1);
+  a2=(a2/a0);
+  if (res[0]!=0) res[0][4]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c

@@ -0,0 +1,182 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_0_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_s6 CASADI_PREFIX(s6)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s5[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
+static const casadi_int casadi_s6[3] = {5, 0, 0};
+
+/* lat_cost_y_0_fun_jac_ut_xt:(i0[4],i1,i2[],i3[2])->(o0[5],o1[5x5,5nz],o2[5x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=10.;
+  a1=(a0+a1);
+  a2=arg[0]? arg[0][2] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[0]? arg[0][3] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  a3=(a1*a2);
+  if (res[0]!=0) res[0][3]=a3;
+  a3=1.0000000000000001e-01;
+  a0=(a0+a3);
+  a2=(a2/a0);
+  if (res[0]!=0) res[0][4]=a2;
+  a2=1.;
+  if (res[1]!=0) res[1][0]=a2;
+  if (res[1]!=0) res[1][1]=a1;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  a0=(1./a0);
+  if (res[1]!=0) res[1][4]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    case 1: return casadi_s5;
+    case 2: return casadi_s6;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_0_hess.c

@@ -0,0 +1,152 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_0_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s4[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s5[8] = {5, 5, 0, 0, 0, 0, 0, 0};
+
+/* lat_cost_y_0_hess:(i0[4],i1,i2[],i3[5],i4[2])->(o0[5x5,0nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    case 4: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun.c

@@ -0,0 +1,158 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_e_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
+
+/* lat_cost_y_e_fun:(i0[4],i1[],i2[],i3[2])->(o0[3]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=10.;
+  a0=(a0+a1);
+  a1=arg[0]? arg[0][2] : 0;
+  a1=(a0*a1);
+  if (res[0]!=0) res[0][1]=a1;
+  a1=arg[0]? arg[0][3] : 0;
+  a0=(a0*a1);
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c

@@ -0,0 +1,170 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_e_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s4[9] = {4, 3, 0, 1, 2, 3, 1, 2, 3};
+static const casadi_int casadi_s5[3] = {3, 0, 0};
+
+/* lat_cost_y_e_fun_jac_ut_xt:(i0[4],i1[],i2[],i3[2])->(o0[3],o1[4x3,3nz],o2[3x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=10.;
+  a0=(a0+a1);
+  a1=arg[0]? arg[0][2] : 0;
+  a1=(a0*a1);
+  if (res[0]!=0) res[0][1]=a1;
+  a1=arg[0]? arg[0][3] : 0;
+  a1=(a0*a1);
+  if (res[0]!=0) res[0][2]=a1;
+  a1=1.;
+  if (res[1]!=0) res[1][0]=a1;
+  if (res[1]!=0) res[1][1]=a0;
+  if (res[1]!=0) res[1][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    case 1: return casadi_s4;
+    case 2: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_e_hess.c

@@ -0,0 +1,150 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_e_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s4[7] = {4, 4, 0, 0, 0, 0, 0};
+
+/* lat_cost_y_e_hess:(i0[4],i1[],i2[],i3[3],i4[2])->(o0[4x4,0nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun.c

@@ -0,0 +1,167 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+
+/* lat_cost_y_fun:(i0[4],i1,i2[],i3[2])->(o0[5]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=10.;
+  a1=(a0+a1);
+  a2=arg[0]? arg[0][2] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[0]? arg[0][3] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  a1=(a1*a2);
+  if (res[0]!=0) res[0][3]=a1;
+  a1=1.0000000000000001e-01;
+  a0=(a0+a1);
+  a2=(a2/a0);
+  if (res[0]!=0) res[0][4]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_fun_jac_ut_xt.c

@@ -0,0 +1,182 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_s6 CASADI_PREFIX(s6)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s4[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s5[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
+static const casadi_int casadi_s6[3] = {5, 0, 0};
+
+/* lat_cost_y_fun_jac_ut_xt:(i0[4],i1,i2[],i3[2])->(o0[5],o1[5x5,5nz],o2[5x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=10.;
+  a1=(a0+a1);
+  a2=arg[0]? arg[0][2] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[0]? arg[0][3] : 0;
+  a2=(a1*a2);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  a3=(a1*a2);
+  if (res[0]!=0) res[0][3]=a3;
+  a3=1.0000000000000001e-01;
+  a0=(a0+a3);
+  a2=(a2/a0);
+  if (res[0]!=0) res[0][4]=a2;
+  a2=1.;
+  if (res[1]!=0) res[1][0]=a2;
+  if (res[1]!=0) res[1][1]=a1;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  a0=(1./a0);
+  if (res[1]!=0) res[1][4]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    case 1: return casadi_s5;
+    case 2: return casadi_s6;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost_y_hess.c

@@ -0,0 +1,152 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_cost_y_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s4[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s5[8] = {5, 5, 0, 0, 0, 0, 0, 0};
+
+/* lat_cost_y_hess:(i0[4],i1,i2[],i3[5],i4[2])->(o0[5x5,0nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_cost_y_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    case 4: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_expl_ode_fun.c

@@ -0,0 +1,164 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_expl_ode_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
+
+/* lat_expl_ode_fun:(i0[4],i1,i2[2])->(o0[4]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[2]? arg[2][0] : 0;
+  a1=arg[0]? arg[0][2] : 0;
+  a2=cos(a1);
+  a2=(a0*a2);
+  a3=arg[2]? arg[2][1] : 0;
+  a4=sin(a1);
+  a4=(a3*a4);
+  a5=arg[0]? arg[0][3] : 0;
+  a4=(a4*a5);
+  a2=(a2-a4);
+  if (res[0]!=0) res[0][0]=a2;
+  a2=sin(a1);
+  a0=(a0*a2);
+  a1=cos(a1);
+  a3=(a3*a1);
+  a3=(a3*a5);
+  a0=(a0+a3);
+  if (res[0]!=0) res[0][1]=a0;
+  if (res[0]!=0) res[0][2]=a5;
+  a5=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][3]=a5;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_expl_ode_fun_n_in(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_expl_ode_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_expl_ode_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_expl_ode_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_expl_ode_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_ode_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_ode_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 3;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_expl_vde_adj.c

@@ -0,0 +1,186 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_expl_vde_adj_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+
+/* lat_expl_vde_adj:(i0[4],i1[4],i2,i3[2])->(o0[5]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5, a6, a7;
+  a0=0.;
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a0;
+  a0=arg[0]? arg[0][2] : 0;
+  a1=cos(a0);
+  a2=arg[3]? arg[3][0] : 0;
+  a3=arg[1]? arg[1][1] : 0;
+  a4=(a2*a3);
+  a1=(a1*a4);
+  a4=sin(a0);
+  a5=arg[3]? arg[3][1] : 0;
+  a6=arg[0]? arg[0][3] : 0;
+  a7=(a6*a3);
+  a7=(a5*a7);
+  a4=(a4*a7);
+  a1=(a1-a4);
+  a4=cos(a0);
+  a7=arg[1]? arg[1][0] : 0;
+  a6=(a6*a7);
+  a6=(a5*a6);
+  a4=(a4*a6);
+  a1=(a1-a4);
+  a4=sin(a0);
+  a2=(a2*a7);
+  a4=(a4*a2);
+  a1=(a1-a4);
+  if (res[0]!=0) res[0][2]=a1;
+  a1=arg[1]? arg[1][2] : 0;
+  a4=cos(a0);
+  a4=(a5*a4);
+  a4=(a4*a3);
+  a1=(a1+a4);
+  a0=sin(a0);
+  a5=(a5*a0);
+  a5=(a5*a7);
+  a1=(a1-a5);
+  if (res[0]!=0) res[0][3]=a1;
+  a1=arg[1]? arg[1][3] : 0;
+  if (res[0]!=0) res[0][4]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_adj_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_adj_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_expl_vde_adj_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_expl_vde_adj_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_expl_vde_adj_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_adj_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s0;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_adj_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_expl_vde_forw.c

@@ -0,0 +1,299 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) lat_expl_vde_forw_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s1[23] = {4, 4, 0, 4, 8, 12, 16, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3};
+static const casadi_int casadi_s2[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
+
+/* lat_expl_vde_forw:(i0[4],i1[4x4],i2[4],i3,i4[2])->(o0[4],o1[4x4],o2[4]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a2, a3, a4, a5, a6, a7, a8, a9;
+  a0=arg[4]? arg[4][0] : 0;
+  a1=arg[0]? arg[0][2] : 0;
+  a2=cos(a1);
+  a2=(a0*a2);
+  a3=arg[4]? arg[4][1] : 0;
+  a4=sin(a1);
+  a4=(a3*a4);
+  a5=arg[0]? arg[0][3] : 0;
+  a6=(a4*a5);
+  a2=(a2-a6);
+  if (res[0]!=0) res[0][0]=a2;
+  a2=sin(a1);
+  a2=(a0*a2);
+  a6=cos(a1);
+  a6=(a3*a6);
+  a7=(a6*a5);
+  a2=(a2+a7);
+  if (res[0]!=0) res[0][1]=a2;
+  if (res[0]!=0) res[0][2]=a5;
+  a2=arg[3]? arg[3][0] : 0;
+  if (res[0]!=0) res[0][3]=a2;
+  a2=sin(a1);
+  a7=arg[1]? arg[1][2] : 0;
+  a8=(a2*a7);
+  a8=(a0*a8);
+  a9=cos(a1);
+  a10=(a9*a7);
+  a10=(a3*a10);
+  a10=(a5*a10);
+  a11=arg[1]? arg[1][3] : 0;
+  a12=(a4*a11);
+  a10=(a10+a12);
+  a8=(a8+a10);
+  a8=(-a8);
+  if (res[1]!=0) res[1][0]=a8;
+  a8=cos(a1);
+  a10=(a8*a7);
+  a10=(a0*a10);
+  a12=(a6*a11);
+  a13=sin(a1);
+  a7=(a13*a7);
+  a7=(a3*a7);
+  a7=(a5*a7);
+  a12=(a12-a7);
+  a10=(a10+a12);
+  if (res[1]!=0) res[1][1]=a10;
+  if (res[1]!=0) res[1][2]=a11;
+  a11=0.;
+  if (res[1]!=0) res[1][3]=a11;
+  a10=arg[1]? arg[1][6] : 0;
+  a12=(a2*a10);
+  a12=(a0*a12);
+  a7=(a9*a10);
+  a7=(a3*a7);
+  a7=(a5*a7);
+  a14=arg[1]? arg[1][7] : 0;
+  a15=(a4*a14);
+  a7=(a7+a15);
+  a12=(a12+a7);
+  a12=(-a12);
+  if (res[1]!=0) res[1][4]=a12;
+  a12=(a8*a10);
+  a12=(a0*a12);
+  a7=(a6*a14);
+  a10=(a13*a10);
+  a10=(a3*a10);
+  a10=(a5*a10);
+  a7=(a7-a10);
+  a12=(a12+a7);
+  if (res[1]!=0) res[1][5]=a12;
+  if (res[1]!=0) res[1][6]=a14;
+  if (res[1]!=0) res[1][7]=a11;
+  a14=arg[1]? arg[1][10] : 0;
+  a12=(a2*a14);
+  a12=(a0*a12);
+  a7=(a9*a14);
+  a7=(a3*a7);
+  a7=(a5*a7);
+  a10=arg[1]? arg[1][11] : 0;
+  a15=(a4*a10);
+  a7=(a7+a15);
+  a12=(a12+a7);
+  a12=(-a12);
+  if (res[1]!=0) res[1][8]=a12;
+  a12=(a8*a14);
+  a12=(a0*a12);
+  a7=(a6*a10);
+  a14=(a13*a14);
+  a14=(a3*a14);
+  a14=(a5*a14);
+  a7=(a7-a14);
+  a12=(a12+a7);
+  if (res[1]!=0) res[1][9]=a12;
+  if (res[1]!=0) res[1][10]=a10;
+  if (res[1]!=0) res[1][11]=a11;
+  a10=arg[1]? arg[1][14] : 0;
+  a2=(a2*a10);
+  a2=(a0*a2);
+  a9=(a9*a10);
+  a9=(a3*a9);
+  a9=(a5*a9);
+  a12=arg[1]? arg[1][15] : 0;
+  a7=(a4*a12);
+  a9=(a9+a7);
+  a2=(a2+a9);
+  a2=(-a2);
+  if (res[1]!=0) res[1][12]=a2;
+  a8=(a8*a10);
+  a8=(a0*a8);
+  a2=(a6*a12);
+  a13=(a13*a10);
+  a13=(a3*a13);
+  a13=(a5*a13);
+  a2=(a2-a13);
+  a8=(a8+a2);
+  if (res[1]!=0) res[1][13]=a8;
+  if (res[1]!=0) res[1][14]=a12;
+  if (res[1]!=0) res[1][15]=a11;
+  a11=sin(a1);
+  a12=arg[2]? arg[2][2] : 0;
+  a11=(a11*a12);
+  a11=(a0*a11);
+  a8=cos(a1);
+  a8=(a8*a12);
+  a8=(a3*a8);
+  a8=(a5*a8);
+  a2=arg[2]? arg[2][3] : 0;
+  a4=(a4*a2);
+  a8=(a8+a4);
+  a11=(a11+a8);
+  a11=(-a11);
+  if (res[2]!=0) res[2][0]=a11;
+  a11=cos(a1);
+  a11=(a11*a12);
+  a0=(a0*a11);
+  a6=(a6*a2);
+  a1=sin(a1);
+  a1=(a1*a12);
+  a3=(a3*a1);
+  a5=(a5*a3);
+  a6=(a6-a5);
+  a0=(a0+a6);
+  if (res[2]!=0) res[2][1]=a0;
+  if (res[2]!=0) res[2][2]=a2;
+  a2=1.;
+  if (res[2]!=0) res[2][3]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_forw_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_forw_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real lat_expl_vde_forw_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_expl_vde_forw_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* lat_expl_vde_forw_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_forw_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s0;
+    case 3: return casadi_s2;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_forw_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s0;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_lat.c

@@ -0,0 +1,216 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+#include "acados_solver_lat.h"
+
+// blasfeo
+#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+
+#define NX     LAT_NX
+#define NZ     LAT_NZ
+#define NU     LAT_NU
+#define NP     LAT_NP
+#define NBX    LAT_NBX
+#define NBX0   LAT_NBX0
+#define NBU    LAT_NBU
+#define NSBX   LAT_NSBX
+#define NSBU   LAT_NSBU
+#define NSH    LAT_NSH
+#define NSG    LAT_NSG
+#define NSPHI  LAT_NSPHI
+#define NSHN   LAT_NSHN
+#define NSGN   LAT_NSGN
+#define NSPHIN LAT_NSPHIN
+#define NSBXN  LAT_NSBXN
+#define NS     LAT_NS
+#define NSN    LAT_NSN
+#define NG     LAT_NG
+#define NBXN   LAT_NBXN
+#define NGN    LAT_NGN
+#define NY0    LAT_NY0
+#define NY     LAT_NY
+#define NYN    LAT_NYN
+#define NH     LAT_NH
+#define NPHI   LAT_NPHI
+#define NHN    LAT_NHN
+#define NPHIN  LAT_NPHIN
+#define NR     LAT_NR
+
+
+int main()
+{
+
+    lat_solver_capsule *acados_ocp_capsule = lat_acados_create_capsule();
+    // there is an opportunity to change the number of shooting intervals in C without new code generation
+    int N = LAT_N;
+    // allocate the array and fill it accordingly
+    double* new_time_steps = NULL;
+    int status = lat_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
+
+    if (status)
+    {
+        printf("lat_acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(acados_ocp_capsule);
+    ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(acados_ocp_capsule);
+    ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(acados_ocp_capsule);
+    ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(acados_ocp_capsule);
+    ocp_nlp_solver *nlp_solver = lat_acados_get_nlp_solver(acados_ocp_capsule);
+    void *nlp_opts = lat_acados_get_nlp_opts(acados_ocp_capsule);
+
+    // initial condition
+    int idxbx0[NBX0];
+    idxbx0[0] = 0;
+    idxbx0[1] = 1;
+    idxbx0[2] = 2;
+    idxbx0[3] = 3;
+
+    double lbx0[NBX0];
+    double ubx0[NBX0];
+    lbx0[0] = 0.0;
+    ubx0[0] = 0.0;
+    lbx0[1] = 0.0;
+    ubx0[1] = 0.0;
+    lbx0[2] = 0.0;
+    ubx0[2] = 0.0;
+    lbx0[3] = 0.0;
+    ubx0[3] = 0.0;
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
+
+    // initialization for state values
+    double x_init[NX];
+    x_init[0] = 0.0;
+    x_init[1] = 0.0;
+    x_init[2] = 0.0;
+    x_init[3] = 0.0;
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = 0.0;
+    p[1] = 0.0;
+
+    for (int ii = 0; ii <= N; ii++)
+    {
+        lat_acados_update_params(acados_ocp_capsule, ii, p, NP);
+    }
+  
+
+    // prepare evaluation
+    int NTIMINGS = 1;
+    double min_time = 1e12;
+    double kkt_norm_inf;
+    double elapsed_time;
+    int sqp_iter;
+
+    double xtraj[NX * (N+1)];
+    double utraj[NU * N];
+
+
+    // solve ocp in loop
+    int rti_phase = 0;
+
+    for (int ii = 0; ii < NTIMINGS; ii++)
+    {
+        // initialize solution
+        for (int i = 0; i < N; i++)
+        {
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
+        }
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
+        ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
+        status = lat_acados_solve(acados_ocp_capsule);
+        ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
+        min_time = MIN(elapsed_time, min_time);
+    }
+
+    /* print solution and statistics */
+    for (int ii = 0; ii <= nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
+    for (int ii = 0; ii < nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
+
+    printf("\n--- xtraj ---\n");
+    d_print_exp_tran_mat( NX, N+1, xtraj, NX);
+    printf("\n--- utraj ---\n");
+    d_print_exp_tran_mat( NU, N, utraj, NU );
+    // ocp_nlp_out_print(nlp_solver->dims, nlp_out);
+
+    printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
+
+    if (status == ACADOS_SUCCESS)
+    {
+        printf("lat_acados_solve(): SUCCESS!\n");
+    }
+    else
+    {
+        printf("lat_acados_solve() failed with status %d.\n", status);
+    }
+
+    // get solution
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
+    ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
+
+    lat_acados_print_stats(acados_ocp_capsule);
+
+    printf("\nSolver info:\n");
+    printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
+           sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
+
+    // free solver
+    status = lat_acados_free(acados_ocp_capsule);
+    if (status) {
+        printf("lat_acados_free() returned status %d. \n", status);
+    }
+    // free solver capsule
+    status = lat_acados_free_capsule(acados_ocp_capsule);
+    if (status) {
+        printf("lat_acados_free_capsule() returned status %d. \n", status);
+    }
+
+    return status;
+}

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/main_sim_lat.c

@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_lat.h"
+
+#define NX     LAT_NX
+#define NZ     LAT_NZ
+#define NU     LAT_NU
+#define NP     LAT_NP
+
+
+int main()
+{
+    int status = 0;
+    sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
+    status = lat_acados_sim_create(capsule);
+
+    if (status)
+    {
+        printf("acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
+    void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
+
+    // initial condition
+    double x_current[NX];
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+    x_current[3] = 0.0;
+
+  
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+    x_current[3] = 0.0;
+    
+  
+
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = 0.0;
+    p[1] = 0.0;
+
+    lat_acados_sim_update_params(capsule, p, NP);
+  
+
+    int n_sim_steps = 3;
+    // solve ocp in loop
+    for (int ii = 0; ii < n_sim_steps; ii++)
+    {
+        sim_in_set(acados_sim_config, acados_sim_dims,
+            acados_sim_in, "x", x_current);
+        status = lat_acados_sim_solve(capsule);
+
+        if (status != ACADOS_SUCCESS)
+        {
+            printf("acados_solve() failed with status %d.\n", status);
+        }
+
+        sim_out_get(acados_sim_config, acados_sim_dims,
+               acados_sim_out, "x", x_current);
+        
+        printf("\nx_current, %d\n", ii);
+        for (int jj = 0; jj < NX; jj++)
+        {
+            printf("%e\n", x_current[jj]);
+        }
+    }
+
+    printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+    // free solver
+    status = lat_acados_sim_free(capsule);
+    if (status) {
+        printf("lat_acados_sim_free() returned status %d. \n", status);
+    }
+
+    lat_acados_sim_solver_free_capsule(capsule);
+
+    return status;
+}

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -1,96 +0,0 @@
-Import('env', 'envCython', 'arch', 'messaging_python', 'common_python')
-
-gen = "c_generated_code"
-
-casadi_model = [
-  f'{gen}/long_model/long_expl_ode_fun.c',
-  f'{gen}/long_model/long_expl_vde_forw.c',
-]
-
-casadi_cost_y = [
-  f'{gen}/long_cost/long_cost_y_fun.c',
-  f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c',
-  f'{gen}/long_cost/long_cost_y_hess.c',
-]
-
-casadi_cost_e = [
-  f'{gen}/long_cost/long_cost_y_e_fun.c',
-  f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c',
-  f'{gen}/long_cost/long_cost_y_e_hess.c',
-]
-
-casadi_cost_0 = [
-  f'{gen}/long_cost/long_cost_y_0_fun.c',
-  f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c',
-  f'{gen}/long_cost/long_cost_y_0_hess.c',
-]
-
-casadi_constraints = [
-  f'{gen}/long_constraints/long_constr_h_fun.c',
-  f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
-]
-
-build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e +  \
-              casadi_cost_0 + casadi_constraints
-
-# extra generated files used to trigger a rebuild
-generated_files = [
-  f'{gen}/Makefile',
-
-  f'{gen}/main_long.c',
-  f'{gen}/main_sim_long.c',
-  f'{gen}/acados_solver_long.h',
-  f'{gen}/acados_sim_solver_long.h',
-  f'{gen}/acados_sim_solver_long.c',
-  f'{gen}/acados_solver.pxd',
-
-  f'{gen}/long_model/long_expl_vde_adj.c',
-
-  f'{gen}/long_model/long_model.h',
-  f'{gen}/long_constraints/long_constraints.h',
-  f'{gen}/long_cost/long_cost.h',
-] + build_files
-
-acados_dir = '#third_party/acados'
-acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
-
-source_list = ['long_mpc.py',
-  '#selfdrive/modeld/constants.py',
-  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
-  f'{acados_templates_dir}/acados_solver.in.c',
-]
-
-lenv = env.Clone()
-lenv.Clean(generated_files, Dir(gen))
-
-generated_long = lenv.Command(generated_files,
-                              source_list,
-                              f"cd {Dir('.').abspath} && python3 long_mpc.py")
-lenv.Depends(generated_long, [messaging_python, common_python])
-
-lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
-lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
-lenv["CCFLAGS"].append("-Wno-unused")
-if arch != "Darwin":
-  lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
-lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
-                                build_files,
-                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
-
-# generate cython stuff
-acados_ocp_solver_pyx = File("#third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
-acados_ocp_solver_common = File("#third_party/acados/acados_template/acados_solver_common.pxd")
-libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
-libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
-
-lenv2 = envCython.Clone()
-lenv2["LINKFLAGS"] += [lib_solver[0].get_labspath()]
-lenv2.Command(libacados_ocp_solver_c,
-  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
-  f'cython' + \
-  f' -o {libacados_ocp_solver_c.get_labspath()}' + \
-  f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
-  f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
-  f' {acados_ocp_solver_pyx.get_labspath()}')
-lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c])
-lenv2.Depends(lib_cython, lib_solver)

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile

@@ -0,0 +1,213 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+
+
+
+# define sources and use make's implicit rules to generate object files (*.o)
+
+# model
+MODEL_SRC=
+MODEL_SRC+= long_model/long_expl_ode_fun.c
+MODEL_SRC+= long_model/long_expl_vde_forw.c
+MODEL_SRC+= long_model/long_expl_vde_adj.c
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
+
+# optimal control problem - mostly CasADi exports
+OCP_SRC=
+OCP_SRC+= long_constraints/long_constr_h_fun_jac_uxt_zt.c
+OCP_SRC+= long_constraints/long_constr_h_fun.c
+OCP_SRC+= long_cost/long_cost_y_0_fun.c
+OCP_SRC+= long_cost/long_cost_y_0_fun_jac_ut_xt.c
+OCP_SRC+= long_cost/long_cost_y_0_hess.c
+OCP_SRC+= long_cost/long_cost_y_fun.c
+OCP_SRC+= long_cost/long_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= long_cost/long_cost_y_hess.c
+OCP_SRC+= long_cost/long_cost_y_e_fun.c
+OCP_SRC+= long_cost/long_cost_y_e_fun_jac_ut_xt.c
+OCP_SRC+= long_cost/long_cost_y_e_hess.c
+
+OCP_SRC+= acados_solver_long.c
+OCP_OBJ := $(OCP_SRC:.c=.o)
+
+# for sim solver
+SIM_SRC= acados_sim_solver_long.c
+SIM_OBJ := $(SIM_SRC:.c=.o)
+
+# for target example
+EX_SRC= main_long.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
+
+# for target example_sim
+EX_SIM_SRC= main_sim_long.c
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
+
+# combine model, sim and ocp object files
+OBJ=
+OBJ+= $(MODEL_OBJ)
+OBJ+= $(SIM_OBJ)
+OBJ+= $(OCP_OBJ)
+
+EXTERNAL_DIR=
+EXTERNAL_LIB=
+
+INCLUDE_PATH = /data/openpilot/third_party/acados/include
+LIB_PATH = /data/openpilot/third_party/acados/lib
+
+# preprocessor flags for make's implicit rules
+CPPFLAGS+= -I$(INCLUDE_PATH)
+CPPFLAGS+= -I$(INCLUDE_PATH)/acados
+CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
+CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
+
+
+# define the c-compiler flags for make's implicit rules
+CFLAGS = -fPIC -std=c99   -O2#-fno-diagnostics-show-line-numbers -g
+# # Debugging
+# CFLAGS += -g3
+
+# linker flags
+LDFLAGS+= -L$(LIB_PATH)
+
+# link to libraries
+LDLIBS+= -lacados
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= 
+
+# libraries
+LIBACADOS_SOLVER=libacados_solver_long.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_long.so
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
+
+# virtual targets
+.PHONY : all clean
+
+#all: clean example_sim example shared_lib
+
+all: clean example_sim example
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
+
+# some linker targets
+example: $(EX_OBJ) $(OBJ)
+	$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
+
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
+
+bundled_shared_lib: $(OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
+
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+
+# Cython targets
+ocp_cython_c: ocp_shared_lib
+	cython \
+	-o acados_ocp_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
+
+ocp_cython_o: ocp_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_ocp_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
+	-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
+	acados_ocp_solver_pyx.c \
+
+ocp_cython: ocp_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_ocp_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_ocp_solver_pyx.o \
+	$(abspath .)/libacados_ocp_solver_long.so \
+	$(LDFLAGS) $(LDLIBS)
+
+# Sim Cython targets
+sim_cython_c: sim_shared_lib
+	cython \
+	-o acados_sim_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
+
+sim_cython_o: sim_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_sim_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
+	-I /usr/local/pyenv/versions/3.11.4/include/python3.11 \
+	acados_sim_solver_pyx.c \
+
+sim_cython: sim_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_sim_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_sim_solver_pyx.o \
+	$(abspath .)/libacados_sim_solver_long.so \
+	$(LDFLAGS) $(LDLIBS)
+
+clean:
+	$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
+
+clean_ocp_shared_lib:
+	$(RM) $(LIBACADOS_OCP_SOLVER)
+	$(RM) $(OCP_OBJ)
+
+clean_ocp_cython:
+	$(RM) libacados_ocp_solver_long.so
+	$(RM) acados_solver_long.o
+	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx.o
+
+clean_sim_cython:
+	$(RM) libacados_sim_solver_long.so
+	$(RM) acados_sim_solver_long.o
+	$(RM) acados_sim_solver_pyx.so
+	$(RM) acados_sim_solver_pyx.o

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.c

@@ -0,0 +1,294 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "long_model/long_model.h"
+#include "acados_sim_solver_long.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * long_acados_sim_solver_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+    sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int long_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int long_acados_sim_create(sim_solver_capsule * capsule)
+{
+    // initialize
+    const int nx = LONG_NX;
+    const int nu = LONG_NU;
+    const int nz = LONG_NZ;
+    const int np = LONG_NP;
+    bool tmp_bool;
+
+    
+    double Tsim = 0.06944444444444445;
+
+    
+    // explicit ode
+    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_forw_vde_casadi->casadi_fun = &long_expl_vde_forw;
+    capsule->sim_forw_vde_casadi->casadi_n_in = &long_expl_vde_forw_n_in;
+    capsule->sim_forw_vde_casadi->casadi_n_out = &long_expl_vde_forw_n_out;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_in = &long_expl_vde_forw_sparsity_in;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_out = &long_expl_vde_forw_sparsity_out;
+    capsule->sim_forw_vde_casadi->casadi_work = &long_expl_vde_forw_work;
+    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+    capsule->sim_vde_adj_casadi->casadi_fun = &long_expl_vde_adj;
+    capsule->sim_vde_adj_casadi->casadi_n_in = &long_expl_vde_adj_n_in;
+    capsule->sim_vde_adj_casadi->casadi_n_out = &long_expl_vde_adj_n_out;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_in = &long_expl_vde_adj_sparsity_in;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_out = &long_expl_vde_adj_sparsity_out;
+    capsule->sim_vde_adj_casadi->casadi_work = &long_expl_vde_adj_work;
+    external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
+
+    capsule->sim_expl_ode_fun_casadi->casadi_fun = &long_expl_ode_fun;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_in = &long_expl_ode_fun_n_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_out = &long_expl_ode_fun_n_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &long_expl_ode_fun_sparsity_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &long_expl_ode_fun_sparsity_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_work = &long_expl_ode_fun_work;
+    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+    
+
+    // sim plan & config
+    sim_solver_plan_t plan;
+    plan.sim_solver = ERK;
+
+    // create correct config based on plan
+    sim_config * long_sim_config = sim_config_create(plan);
+    capsule->acados_sim_config = long_sim_config;
+
+    // sim dims
+    void *long_sim_dims = sim_dims_create(long_sim_config);
+    capsule->acados_sim_dims = long_sim_dims;
+    sim_dims_set(long_sim_config, long_sim_dims, "nx", &nx);
+    sim_dims_set(long_sim_config, long_sim_dims, "nu", &nu);
+    sim_dims_set(long_sim_config, long_sim_dims, "nz", &nz);
+
+
+    // sim opts
+    sim_opts *long_sim_opts = sim_opts_create(long_sim_config, long_sim_dims);
+    capsule->acados_sim_opts = long_sim_opts;
+    int tmp_int = 3;
+    sim_opts_set(long_sim_config, long_sim_opts, "newton_iter", &tmp_int);
+    double tmp_double = 0.0;
+    sim_opts_set(long_sim_config, long_sim_opts, "newton_tol", &tmp_double);
+    sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+    sim_opts_set(long_sim_config, long_sim_opts, "collocation_type", &collocation_type);
+
+ 
+    tmp_int = 4;
+    sim_opts_set(long_sim_config, long_sim_opts, "num_stages", &tmp_int);
+    tmp_int = 1;
+    sim_opts_set(long_sim_config, long_sim_opts, "num_steps", &tmp_int);
+    tmp_bool = 0;
+    sim_opts_set(long_sim_config, long_sim_opts, "jac_reuse", &tmp_bool);
+
+
+    // sim in / out
+    sim_in *long_sim_in = sim_in_create(long_sim_config, long_sim_dims);
+    capsule->acados_sim_in = long_sim_in;
+    sim_out *long_sim_out = sim_out_create(long_sim_config, long_sim_dims);
+    capsule->acados_sim_out = long_sim_out;
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "T", &Tsim);
+
+    // model functions
+    long_sim_config->model_set(long_sim_in->model,
+                 "expl_vde_forw", capsule->sim_forw_vde_casadi);
+    long_sim_config->model_set(long_sim_in->model,
+                 "expl_vde_adj", capsule->sim_vde_adj_casadi);
+    long_sim_config->model_set(long_sim_in->model,
+                 "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+    // sim solver
+    sim_solver *long_sim_solver = sim_solver_create(long_sim_config,
+                                               long_sim_dims, long_sim_opts);
+    capsule->acados_sim_solver = long_sim_solver;
+
+
+    /* initialize parameter values */
+    double* p = calloc(np, sizeof(double));
+    
+    p[0] = -1.2;
+    p[1] = 1.2;
+    p[4] = 1.45;
+    p[5] = 0.75;
+
+    long_acados_sim_update_params(capsule, p, np);
+    free(p);
+
+
+    /* initialize input */
+    // x
+    double x0[3];
+    for (int ii = 0; ii < 3; ii++)
+        x0[ii] = 0.0;
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "x", x0);
+
+
+    // u
+    double u0[1];
+    for (int ii = 0; ii < 1; ii++)
+        u0[ii] = 0.0;
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "u", u0);
+
+    // S_forw
+    double S_forw[12];
+    for (int ii = 0; ii < 12; ii++)
+        S_forw[ii] = 0.0;
+    for (int ii = 0; ii < 3; ii++)
+        S_forw[ii + ii * 3 ] = 1.0;
+
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "S_forw", S_forw);
+
+    int status = sim_precompute(long_sim_solver, long_sim_in, long_sim_out);
+
+    return status;
+}
+
+
+int long_acados_sim_solve(sim_solver_capsule *capsule)
+{
+    // integrate dynamics using acados sim_solver
+    int status = sim_solve(capsule->acados_sim_solver,
+                           capsule->acados_sim_in, capsule->acados_sim_out);
+    if (status != 0)
+        printf("error in long_acados_sim_solve()! Exiting.\n");
+
+    return status;
+}
+
+
+int long_acados_sim_free(sim_solver_capsule *capsule)
+{
+    // free memory
+    sim_solver_destroy(capsule->acados_sim_solver);
+    sim_in_destroy(capsule->acados_sim_in);
+    sim_out_destroy(capsule->acados_sim_out);
+    sim_opts_destroy(capsule->acados_sim_opts);
+    sim_dims_destroy(capsule->acados_sim_dims);
+    sim_config_destroy(capsule->acados_sim_config);
+
+    // free external function
+    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+    external_function_param_casadi_free(capsule->sim_vde_adj_casadi);
+    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+    return 0;
+}
+
+
+int long_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+    int status = 0;
+    int casadi_np = LONG_NP;
+
+    if (casadi_np != np) {
+        printf("long_acados_sim_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+    return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_config;
+};
+
+sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_in;
+};
+
+sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_out;
+};
+
+void * long_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_dims;
+};
+
+sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_opts;
+};
+
+sim_solver  * long_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_solver;
+};
+

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver.pxd

@@ -0,0 +1,62 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+cimport acados_solver_common
+
+cdef extern from "acados_solver_long.h":
+    ctypedef struct nlp_solver_capsule "long_solver_capsule":
+        pass
+
+    nlp_solver_capsule * acados_create_capsule "long_acados_create_capsule"()
+    int acados_free_capsule "long_acados_free_capsule"(nlp_solver_capsule *capsule)
+
+    int acados_create "long_acados_create"(nlp_solver_capsule * capsule)
+
+    int acados_create_with_discretization "long_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
+    int acados_update_time_steps "long_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
+    int acados_update_qp_solver_cond_N "long_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
+
+    int acados_update_params "long_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
+    int acados_update_params_sparse "long_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
+    int acados_solve "long_acados_solve"(nlp_solver_capsule * capsule)
+    int acados_reset "long_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
+    int acados_free "long_acados_free"(nlp_solver_capsule * capsule)
+    void acados_print_stats "long_acados_print_stats"(nlp_solver_capsule * capsule)
+
+    int acados_custom_update "long_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
+
+    acados_solver_common.ocp_nlp_in *acados_get_nlp_in "long_acados_get_nlp_in"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_nlp_out "long_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_sens_out "long_acados_get_sens_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "long_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_config *acados_get_nlp_config "long_acados_get_nlp_config"(nlp_solver_capsule * capsule)
+    void *acados_get_nlp_opts "long_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "long_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "long_acados_get_nlp_plan"(nlp_solver_capsule * capsule)

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_fun.c

@@ -0,0 +1,179 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_constr_h_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+
+/* long_constr_h_fun:(i0[3],i1,i2[],i3[6])->(o0[4]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a1=arg[0]? arg[0][2] : 0;
+  a2=arg[3]? arg[3][0] : 0;
+  a2=(a1-a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[3]? arg[3][1] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a2=(a2-a1);
+  a1=arg[3]? arg[3][5] : 0;
+  a3=casadi_sq(a0);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a0);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a1=(a1*a3);
+  a2=(a2-a1);
+  a1=10.;
+  a0=(a0+a1);
+  a2=(a2/a0);
+  if (res[0]!=0) res[0][3]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_constr_h_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_fun_jac_uxt_zt.c

@@ -0,0 +1,205 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_constr_h_fun_jac_uxt_zt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_s6 CASADI_PREFIX(s6)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s5[12] = {4, 4, 0, 1, 2, 3, 5, 2, 3, 3, 1, 2};
+static const casadi_int casadi_s6[3] = {4, 0, 0};
+
+/* long_constr_h_fun_jac_uxt_zt:(i0[3],i1,i2[],i3[6])->(o0[4],o1[4x4,5nz],o2[4x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a1=arg[0]? arg[0][2] : 0;
+  a2=arg[3]? arg[3][0] : 0;
+  a2=(a1-a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[3]? arg[3][1] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a2=(a2-a1);
+  a1=arg[3]? arg[3][5] : 0;
+  a3=casadi_sq(a0);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a0);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a3=(a1*a3);
+  a2=(a2-a3);
+  a3=10.;
+  a3=(a0+a3);
+  a2=(a2/a3);
+  if (res[0]!=0) res[0][3]=a2;
+  a5=1.;
+  if (res[1]!=0) res[1][0]=a5;
+  if (res[1]!=0) res[1][1]=a5;
+  a5=-1.;
+  if (res[1]!=0) res[1][2]=a5;
+  a5=(1./a3);
+  a5=(-a5);
+  if (res[1]!=0) res[1][3]=a5;
+  a5=2.0000000000000001e-01;
+  a0=(a0+a0);
+  a5=(a5*a0);
+  a5=(a5+a4);
+  a1=(a1*a5);
+  a1=(a1/a3);
+  a2=(a2/a3);
+  a1=(a1+a2);
+  a1=(-a1);
+  if (res[1]!=0) res[1][4]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_jac_uxt_zt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_jac_uxt_zt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_constr_h_fun_jac_uxt_zt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_jac_uxt_zt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_jac_uxt_zt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_jac_uxt_zt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_jac_uxt_zt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    case 1: return casadi_s5;
+    case 2: return casadi_s6;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun.c

@@ -0,0 +1,176 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_0_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_cost_y_0_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a2);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a2;
+  a1=arg[3]? arg[3][3] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][4]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun_jac_ut_xt.c

@@ -0,0 +1,202 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_0_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
+static const casadi_int casadi_s5[3] = {6, 0, 0};
+
+/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a2);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a1=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a1;
+  a5=arg[3]? arg[3][3] : 0;
+  a1=(a1-a5);
+  if (res[0]!=0) res[0][4]=a1;
+  a1=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a1;
+  a1=(1./a3);
+  a1=(-a1);
+  if (res[1]!=0) res[1][0]=a1;
+  a1=2.0000000000000001e-01;
+  a2=(a2+a2);
+  a1=(a1*a2);
+  a1=(a1+a4);
+  a1=(a1/a3);
+  a0=(a0/a3);
+  a1=(a1+a0);
+  a1=(-a1);
+  if (res[1]!=0) res[1][1]=a1;
+  a1=1.;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a1;
+  if (res[1]!=0) res[1][6]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    case 1: return casadi_s4;
+    case 2: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_hess.c

@@ -0,0 +1,201 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_0_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
+
+/* long_cost_y_0_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=arg[0]? arg[0][1] : 0;
+  a2=10.;
+  a2=(a1+a2);
+  a3=(1./a2);
+  a3=(a3/a2);
+  a3=(a0*a3);
+  if (res[0]!=0) res[0][0]=a3;
+  a3=(a0/a2);
+  a4=(a3/a2);
+  if (res[0]!=0) res[0][1]=a4;
+  a5=2.0000000000000001e-01;
+  a6=(a1+a1);
+  a6=(a5*a6);
+  a7=arg[4]? arg[4][4] : 0;
+  a6=(a6+a7);
+  a6=(a6/a2);
+  a8=arg[4]? arg[4][2] : 0;
+  a9=arg[0]? arg[0][0] : 0;
+  a8=(a8-a9);
+  a9=casadi_sq(a1);
+  a10=5.;
+  a9=(a9/a10);
+  a10=(a7*a1);
+  a9=(a9+a10);
+  a10=6.;
+  a9=(a9+a10);
+  a8=(a8-a9);
+  a8=(a8/a2);
+  a9=(a8/a2);
+  a6=(a6+a9);
+  a6=(a6/a2);
+  a8=(a8/a2);
+  a8=(a8/a2);
+  a6=(a6+a8);
+  a0=(a0*a6);
+  a7=(a7*a4);
+  a0=(a0+a7);
+  a7=2.;
+  a3=(a5*a3);
+  a7=(a7*a3);
+  a1=(a1+a1);
+  a5=(a5*a4);
+  a1=(a1*a5);
+  a7=(a7-a1);
+  a0=(a0-a7);
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun.c

@@ -0,0 +1,174 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_e_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+
+/* long_cost_y_e_fun:(i0[3],i1[],i2[],i3[6])->(o0[5]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a2);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a2;
+  a1=arg[3]? arg[3][3] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][4]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun_jac_ut_xt.c

@@ -0,0 +1,199 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_e_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s4[14] = {3, 5, 0, 2, 3, 4, 5, 6, 0, 1, 0, 1, 2, 2};
+static const casadi_int casadi_s5[3] = {5, 0, 0};
+
+/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[],i3[6])->(o0[5],o1[3x5,6nz],o2[5x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a2);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a1=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a1;
+  a5=arg[3]? arg[3][3] : 0;
+  a1=(a1-a5);
+  if (res[0]!=0) res[0][4]=a1;
+  a1=(1./a3);
+  a1=(-a1);
+  if (res[1]!=0) res[1][0]=a1;
+  a1=2.0000000000000001e-01;
+  a2=(a2+a2);
+  a1=(a1*a2);
+  a1=(a1+a4);
+  a1=(a1/a3);
+  a0=(a0/a3);
+  a1=(a1+a0);
+  a1=(-a1);
+  if (res[1]!=0) res[1][1]=a1;
+  a1=1.;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    case 1: return casadi_s4;
+    case 2: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_hess.c

@@ -0,0 +1,201 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_e_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[9] = {3, 3, 0, 1, 3, 3, 1, 0, 1};
+
+/* long_cost_y_e_hess:(i0[3],i1[],i2[],i3[5],i4[6])->(o0[3x3,3nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=arg[0]? arg[0][1] : 0;
+  a2=10.;
+  a2=(a1+a2);
+  a3=(1./a2);
+  a3=(a3/a2);
+  a3=(a0*a3);
+  if (res[0]!=0) res[0][0]=a3;
+  a3=(a0/a2);
+  a4=(a3/a2);
+  if (res[0]!=0) res[0][1]=a4;
+  a5=2.0000000000000001e-01;
+  a6=(a1+a1);
+  a6=(a5*a6);
+  a7=arg[4]? arg[4][4] : 0;
+  a6=(a6+a7);
+  a6=(a6/a2);
+  a8=arg[4]? arg[4][2] : 0;
+  a9=arg[0]? arg[0][0] : 0;
+  a8=(a8-a9);
+  a9=casadi_sq(a1);
+  a10=5.;
+  a9=(a9/a10);
+  a10=(a7*a1);
+  a9=(a9+a10);
+  a10=6.;
+  a9=(a9+a10);
+  a8=(a8-a9);
+  a8=(a8/a2);
+  a9=(a8/a2);
+  a6=(a6+a9);
+  a6=(a6/a2);
+  a8=(a8/a2);
+  a8=(a8/a2);
+  a6=(a6+a8);
+  a0=(a0*a6);
+  a7=(a7*a4);
+  a0=(a0+a7);
+  a7=2.;
+  a3=(a5*a3);
+  a7=(a7*a3);
+  a1=(a1+a1);
+  a5=(a5*a4);
+  a1=(a1*a5);
+  a7=(a7-a1);
+  a0=(a0-a7);
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun.c

@@ -0,0 +1,176 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_cost_y_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a2);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a2;
+  a1=arg[3]? arg[3][3] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][4]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun_jac_ut_xt.c

@@ -0,0 +1,202 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
+static const casadi_int casadi_s5[3] = {6, 0, 0};
+
+/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a2);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a1=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a1;
+  a5=arg[3]? arg[3][3] : 0;
+  a1=(a1-a5);
+  if (res[0]!=0) res[0][4]=a1;
+  a1=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a1;
+  a1=(1./a3);
+  a1=(-a1);
+  if (res[1]!=0) res[1][0]=a1;
+  a1=2.0000000000000001e-01;
+  a2=(a2+a2);
+  a1=(a1*a2);
+  a1=(a1+a4);
+  a1=(a1/a3);
+  a0=(a0/a3);
+  a1=(a1+a0);
+  a1=(-a1);
+  if (res[1]!=0) res[1][1]=a1;
+  a1=1.;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a1;
+  if (res[1]!=0) res[1][6]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    case 1: return casadi_s4;
+    case 2: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_hess.c

@@ -0,0 +1,201 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
+
+/* long_cost_y_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=arg[0]? arg[0][1] : 0;
+  a2=10.;
+  a2=(a1+a2);
+  a3=(1./a2);
+  a3=(a3/a2);
+  a3=(a0*a3);
+  if (res[0]!=0) res[0][0]=a3;
+  a3=(a0/a2);
+  a4=(a3/a2);
+  if (res[0]!=0) res[0][1]=a4;
+  a5=2.0000000000000001e-01;
+  a6=(a1+a1);
+  a6=(a5*a6);
+  a7=arg[4]? arg[4][4] : 0;
+  a6=(a6+a7);
+  a6=(a6/a2);
+  a8=arg[4]? arg[4][2] : 0;
+  a9=arg[0]? arg[0][0] : 0;
+  a8=(a8-a9);
+  a9=casadi_sq(a1);
+  a10=5.;
+  a9=(a9/a10);
+  a10=(a7*a1);
+  a9=(a9+a10);
+  a10=6.;
+  a9=(a9+a10);
+  a8=(a8-a9);
+  a8=(a8/a2);
+  a9=(a8/a2);
+  a6=(a6+a9);
+  a6=(a6/a2);
+  a8=(a8/a2);
+  a8=(a8/a2);
+  a6=(a6+a8);
+  a0=(a0*a6);
+  a7=(a7*a4);
+  a0=(a0+a7);
+  a7=2.;
+  a3=(a5*a3);
+  a7=(a7*a3);
+  a1=(a1+a1);
+  a5=(a5*a4);
+  a1=(a1*a5);
+  a7=(a7-a1);
+  a0=(a0-a7);
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_expl_ode_fun.c

@@ -0,0 +1,149 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_expl_ode_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_expl_ode_fun:(i0[3],i1,i2[6])->(o0[3]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][1]=a0;
+  a0=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_ode_fun_n_in(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_ode_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_expl_ode_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_ode_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_ode_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_ode_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_ode_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 3;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_expl_vde_adj.c

@@ -0,0 +1,155 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_expl_vde_adj_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s3[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+
+/* long_expl_vde_adj:(i0[3],i1[3],i2,i3[6])->(o0[4]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0;
+  a0=0.;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][1]=a0;
+  a0=arg[1]? arg[1][1] : 0;
+  if (res[0]!=0) res[0][2]=a0;
+  a0=arg[1]? arg[1][2] : 0;
+  if (res[0]!=0) res[0][3]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_adj_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_adj_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_expl_vde_adj_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_adj_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_adj_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_adj_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s0;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_adj_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_expl_vde_forw.c

@@ -0,0 +1,181 @@
+/* This file was automatically generated by CasADi 3.6.4.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_expl_vde_forw_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[15] = {3, 3, 0, 3, 6, 9, 0, 1, 2, 0, 1, 2, 0, 1, 2};
+static const casadi_int casadi_s2[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_expl_vde_forw:(i0[3],i1[3x3],i2[3],i3,i4[6])->(o0[3],o1[3x3],o2[3]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][1]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  if (res[0]!=0) res[0][2]=a0;
+  a0=arg[1]? arg[1][1] : 0;
+  if (res[1]!=0) res[1][0]=a0;
+  a0=arg[1]? arg[1][2] : 0;
+  if (res[1]!=0) res[1][1]=a0;
+  a0=0.;
+  if (res[1]!=0) res[1][2]=a0;
+  a1=arg[1]? arg[1][4] : 0;
+  if (res[1]!=0) res[1][3]=a1;
+  a1=arg[1]? arg[1][5] : 0;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a0;
+  a1=arg[1]? arg[1][7] : 0;
+  if (res[1]!=0) res[1][6]=a1;
+  a1=arg[1]? arg[1][8] : 0;
+  if (res[1]!=0) res[1][7]=a1;
+  if (res[1]!=0) res[1][8]=a0;
+  a0=arg[2]? arg[2][1] : 0;
+  if (res[2]!=0) res[2][0]=a0;
+  a0=arg[2]? arg[2][2] : 0;
+  if (res[2]!=0) res[2][1]=a0;
+  a0=1.;
+  if (res[2]!=0) res[2][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_forw_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_forw_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_expl_vde_forw_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_forw_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_forw_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_forw_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s0;
+    case 3: return casadi_s2;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_forw_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s0;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_long.c

@@ -0,0 +1,216 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+#include "acados_solver_long.h"
+
+// blasfeo
+#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+
+#define NX     LONG_NX
+#define NZ     LONG_NZ
+#define NU     LONG_NU
+#define NP     LONG_NP
+#define NBX    LONG_NBX
+#define NBX0   LONG_NBX0
+#define NBU    LONG_NBU
+#define NSBX   LONG_NSBX
+#define NSBU   LONG_NSBU
+#define NSH    LONG_NSH
+#define NSG    LONG_NSG
+#define NSPHI  LONG_NSPHI
+#define NSHN   LONG_NSHN
+#define NSGN   LONG_NSGN
+#define NSPHIN LONG_NSPHIN
+#define NSBXN  LONG_NSBXN
+#define NS     LONG_NS
+#define NSN    LONG_NSN
+#define NG     LONG_NG
+#define NBXN   LONG_NBXN
+#define NGN    LONG_NGN
+#define NY0    LONG_NY0
+#define NY     LONG_NY
+#define NYN    LONG_NYN
+#define NH     LONG_NH
+#define NPHI   LONG_NPHI
+#define NHN    LONG_NHN
+#define NPHIN  LONG_NPHIN
+#define NR     LONG_NR
+
+
+int main()
+{
+
+    long_solver_capsule *acados_ocp_capsule = long_acados_create_capsule();
+    // there is an opportunity to change the number of shooting intervals in C without new code generation
+    int N = LONG_N;
+    // allocate the array and fill it accordingly
+    double* new_time_steps = NULL;
+    int status = long_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
+
+    if (status)
+    {
+        printf("long_acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    ocp_nlp_config *nlp_config = long_acados_get_nlp_config(acados_ocp_capsule);
+    ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(acados_ocp_capsule);
+    ocp_nlp_in *nlp_in = long_acados_get_nlp_in(acados_ocp_capsule);
+    ocp_nlp_out *nlp_out = long_acados_get_nlp_out(acados_ocp_capsule);
+    ocp_nlp_solver *nlp_solver = long_acados_get_nlp_solver(acados_ocp_capsule);
+    void *nlp_opts = long_acados_get_nlp_opts(acados_ocp_capsule);
+
+    // initial condition
+    int idxbx0[NBX0];
+    idxbx0[0] = 0;
+    idxbx0[1] = 1;
+    idxbx0[2] = 2;
+
+    double lbx0[NBX0];
+    double ubx0[NBX0];
+    lbx0[0] = 0.0;
+    ubx0[0] = 0.0;
+    lbx0[1] = 0.0;
+    ubx0[1] = 0.0;
+    lbx0[2] = 0.0;
+    ubx0[2] = 0.0;
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
+
+    // initialization for state values
+    double x_init[NX];
+    x_init[0] = 0.0;
+    x_init[1] = 0.0;
+    x_init[2] = 0.0;
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = -1.2;
+    p[1] = 1.2;
+    p[2] = 0.0;
+    p[3] = 0.0;
+    p[4] = 1.45;
+    p[5] = 0.75;
+
+    for (int ii = 0; ii <= N; ii++)
+    {
+        long_acados_update_params(acados_ocp_capsule, ii, p, NP);
+    }
+  
+
+    // prepare evaluation
+    int NTIMINGS = 1;
+    double min_time = 1e12;
+    double kkt_norm_inf;
+    double elapsed_time;
+    int sqp_iter;
+
+    double xtraj[NX * (N+1)];
+    double utraj[NU * N];
+
+
+    // solve ocp in loop
+    int rti_phase = 0;
+
+    for (int ii = 0; ii < NTIMINGS; ii++)
+    {
+        // initialize solution
+        for (int i = 0; i < N; i++)
+        {
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
+        }
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
+        ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
+        status = long_acados_solve(acados_ocp_capsule);
+        ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
+        min_time = MIN(elapsed_time, min_time);
+    }
+
+    /* print solution and statistics */
+    for (int ii = 0; ii <= nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
+    for (int ii = 0; ii < nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
+
+    printf("\n--- xtraj ---\n");
+    d_print_exp_tran_mat( NX, N+1, xtraj, NX);
+    printf("\n--- utraj ---\n");
+    d_print_exp_tran_mat( NU, N, utraj, NU );
+    // ocp_nlp_out_print(nlp_solver->dims, nlp_out);
+
+    printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
+
+    if (status == ACADOS_SUCCESS)
+    {
+        printf("long_acados_solve(): SUCCESS!\n");
+    }
+    else
+    {
+        printf("long_acados_solve() failed with status %d.\n", status);
+    }
+
+    // get solution
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
+    ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
+
+    long_acados_print_stats(acados_ocp_capsule);
+
+    printf("\nSolver info:\n");
+    printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
+           sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
+
+    // free solver
+    status = long_acados_free(acados_ocp_capsule);
+    if (status) {
+        printf("long_acados_free() returned status %d. \n", status);
+    }
+    // free solver capsule
+    status = long_acados_free_capsule(acados_ocp_capsule);
+    if (status) {
+        printf("long_acados_free_capsule() returned status %d. \n", status);
+    }
+
+    return status;
+}

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_sim_long.c

@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_long.h"
+
+#define NX     LONG_NX
+#define NZ     LONG_NZ
+#define NU     LONG_NU
+#define NP     LONG_NP
+
+
+int main()
+{
+    int status = 0;
+    sim_solver_capsule *capsule = long_acados_sim_solver_create_capsule();
+    status = long_acados_sim_create(capsule);
+
+    if (status)
+    {
+        printf("acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    sim_config *acados_sim_config = long_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = long_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = long_acados_get_sim_out(capsule);
+    void *acados_sim_dims = long_acados_get_sim_dims(capsule);
+
+    // initial condition
+    double x_current[NX];
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+
+  
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+    
+  
+
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = -1.2;
+    p[1] = 1.2;
+    p[2] = 0.0;
+    p[3] = 0.0;
+    p[4] = 1.45;
+    p[5] = 0.75;
+
+    long_acados_sim_update_params(capsule, p, NP);
+  
+
+    int n_sim_steps = 3;
+    // solve ocp in loop
+    for (int ii = 0; ii < n_sim_steps; ii++)
+    {
+        sim_in_set(acados_sim_config, acados_sim_dims,
+            acados_sim_in, "x", x_current);
+        status = long_acados_sim_solve(capsule);
+
+        if (status != ACADOS_SUCCESS)
+        {
+            printf("acados_solve() failed with status %d.\n", status);
+        }
+
+        sim_out_get(acados_sim_config, acados_sim_dims,
+               acados_sim_out, "x", x_current);
+        
+        printf("\nx_current, %d\n", ii);
+        for (int jj = 0; jj < NX; jj++)
+        {
+            printf("%e\n", x_current[jj]);
+        }
+    }
+
+    printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+    // free solver
+    status = long_acados_sim_free(capsule);
+    if (status) {
+        printf("long_acados_sim_free() returned status %d. \n", status);
+    }
+
+    long_acados_sim_solver_free_capsule(capsule);
+
+    return status;
+}

selfdrive/controls/tests/test_alerts.py

@@ -1,135 +0,0 @@
-#!/usr/bin/env python3
-import copy
-import json
-import os
-import unittest
-import random
-from PIL import Image, ImageDraw, ImageFont
-
-from cereal import log, car
-from cereal.messaging import SubMaster
-from openpilot.common.basedir import BASEDIR
-from openpilot.common.params import Params
-from openpilot.selfdrive.controls.lib.events import Alert, EVENTS, ET
-from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
-from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS
-
-AlertSize = log.ControlsState.AlertSize
-
-OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")
-
-# TODO: add callback alerts
-ALERTS = []
-for event_types in EVENTS.values():
-  for alert in event_types.values():
-    ALERTS.append(alert)
-
-
-class TestAlerts(unittest.TestCase):
-
-  @classmethod
-  def setUpClass(cls):
-    with open(OFFROAD_ALERTS_PATH) as f:
-      cls.offroad_alerts = json.loads(f.read())
-
-      # Create fake objects for callback
-      cls.CS = car.CarState.new_message()
-      cls.CP = car.CarParams.new_message()
-      cfg = [c for c in CONFIGS if c.proc_name == 'controlsd'][0]
-      cls.sm = SubMaster(cfg.pubs)
-
-  def test_events_defined(self):
-    # Ensure all events in capnp schema are defined in events.py
-    events = car.CarEvent.EventName.schema.enumerants
-
-    for name, e in events.items():
-      if not name.endswith("DEPRECATED"):
-        fail_msg = "%s @%d not in EVENTS" % (name, e)
-        self.assertTrue(e in EVENTS.keys(), msg=fail_msg)
-
-  # ensure alert text doesn't exceed allowed width
-  def test_alert_text_length(self):
-    font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts")
-    regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
-    bold_font_path = os.path.join(font_path, "Inter-Bold.ttf")
-    semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
-
-    max_text_width = 2160 - 300  # full screen width is usable, minus sidebar
-    draw = ImageDraw.Draw(Image.new('RGB', (0, 0)))
-
-    fonts = {
-      AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)],
-      AlertSize.mid: [ImageFont.truetype(bold_font_path, 88),
-                      ImageFont.truetype(regular_font_path, 66)],
-    }
-
-    for alert in ALERTS:
-      if not isinstance(alert, Alert):
-        alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100)
-
-      # for full size alerts, both text fields wrap the text,
-      # so it's unlikely that they  would go past the max width
-      if alert.alert_size in (AlertSize.none, AlertSize.full):
-        continue
-
-      for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]):
-        if i >= len(fonts[alert.alert_size]):
-          break
-
-        font = fonts[alert.alert_size][i]
-        left, _, right, _ = draw.textbbox((0, 0), txt, font)
-        width = right - left
-        msg = f"type: {alert.alert_type} msg: {txt}"
-        self.assertLessEqual(width, max_text_width, msg=msg)
-
-  def test_alert_sanity_check(self):
-    for event_types in EVENTS.values():
-      for event_type, a in event_types.items():
-        # TODO: add callback alerts
-        if not isinstance(a, Alert):
-          continue
-
-        if a.alert_size == AlertSize.none:
-          self.assertEqual(len(a.alert_text_1), 0)
-          self.assertEqual(len(a.alert_text_2), 0)
-        elif a.alert_size == AlertSize.small:
-          self.assertGreater(len(a.alert_text_1), 0)
-          self.assertEqual(len(a.alert_text_2), 0)
-        elif a.alert_size == AlertSize.mid:
-          self.assertGreater(len(a.alert_text_1), 0)
-          self.assertGreater(len(a.alert_text_2), 0)
-        else:
-          self.assertGreater(len(a.alert_text_1), 0)
-
-        self.assertGreaterEqual(a.duration, 0.)
-
-        if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE):
-          self.assertEqual(a.creation_delay, 0.)
-
-  def test_offroad_alerts(self):
-    params = Params()
-    for a in self.offroad_alerts:
-      # set the alert
-      alert = copy.copy(self.offroad_alerts[a])
-      set_offroad_alert(a, True)
-      alert['extra'] = ''
-      self.assertTrue(json.dumps(alert) == params.get(a, encoding='utf8'))
-
-      # then delete it
-      set_offroad_alert(a, False)
-      self.assertTrue(params.get(a) is None)
-
-  def test_offroad_alerts_extra_text(self):
-    params = Params()
-    for i in range(50):
-      # set the alert
-      a = random.choice(list(self.offroad_alerts))
-      alert = self.offroad_alerts[a]
-      set_offroad_alert(a, True, extra_text="a"*i)
-
-      written_alert = json.loads(params.get(a, encoding='utf8'))
-      self.assertTrue("a"*i == written_alert['extra'])
-      self.assertTrue(alert["text"] == written_alert['text'])
-
-if __name__ == "__main__":
-  unittest.main()

selfdrive/controls/tests/test_cruise_speed.py

@@ -1,158 +0,0 @@
-#!/usr/bin/env python3
-import itertools
-import numpy as np
-import unittest
-
-from parameterized import parameterized_class
-from cereal import log
-from openpilot.common.params import Params
-from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT
-from cereal import car
-from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
-
-ButtonEvent = car.CarState.ButtonEvent
-ButtonType = car.CarState.ButtonEvent.Type
-
-
-def run_cruise_simulation(cruise, e2e, t_end=20.):
-  man = Maneuver(
-    '',
-    duration=t_end,
-    initial_speed=max(cruise - 1., 0.0),
-    lead_relevancy=True,
-    initial_distance_lead=100,
-    cruise_values=[cruise],
-    prob_lead_values=[0.0],
-    breakpoints=[0.],
-    e2e=e2e,
-  )
-  valid, output = man.evaluate()
-  assert valid
-  return output[-1, 3]
-
-
-@parameterized_class(("e2e", "personality", "speed"), itertools.product(
-                      [True, False], # e2e
-                      log.LongitudinalPersonality.schema.enumerants, # personality
-                      [5,35])) # speed
-class TestCruiseSpeed(unittest.TestCase):
-  def test_cruise_speed(self):
-    params = Params()
-    params.put("LongitudinalPersonality", str(self.personality))
-    print(f'Testing {self.speed} m/s')
-    cruise_speed = float(self.speed)
-
-    simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e)
-    self.assertAlmostEqual(simulation_steady_state, cruise_speed, delta=.01, msg=f'Did not reach {self.speed} m/s')
-
-
-# TODO: test pcmCruise
-@parameterized_class(('pcm_cruise',), [(False,)])
-class TestVCruiseHelper(unittest.TestCase):
-  def setUp(self):
-    self.CP = car.CarParams(pcmCruise=self.pcm_cruise)
-    self.v_cruise_helper = VCruiseHelper(self.CP)
-    self.reset_cruise_speed_state()
-
-  def reset_cruise_speed_state(self):
-    # Two resets previous cruise speed
-    for _ in range(2):
-      self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False)
-
-  def enable(self, v_ego, experimental_mode):
-    # Simulates user pressing set with a current speed
-    self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode)
-
-  def test_adjust_speed(self):
-    """
-    Asserts speed changes on falling edges of buttons.
-    """
-
-    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
-
-    for btn in (ButtonType.accelCruise, ButtonType.decelCruise):
-      for pressed in (True, False):
-        CS = car.CarState(cruiseState={"available": True})
-        CS.buttonEvents = [ButtonEvent(type=btn, pressed=pressed)]
-
-        self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
-        self.assertEqual(pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
-
-  def test_rising_edge_enable(self):
-    """
-    Some car interfaces may enable on rising edge of a button,
-    ensure we don't adjust speed if enabled changes mid-press.
-    """
-
-    # NOTE: enabled is always one frame behind the result from button press in controlsd
-    for enabled, pressed in ((False, False),
-                             (False, True),
-                             (True, False)):
-      CS = car.CarState(cruiseState={"available": True})
-      CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)]
-      self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False)
-      if pressed:
-        self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
-
-      # Expected diff on enabling. Speed should not change on falling edge of pressed
-      self.assertEqual(not pressed, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
-
-  def test_resume_in_standstill(self):
-    """
-    Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill.
-    """
-
-    self.enable(0, False)
-
-    for standstill in (True, False):
-      for pressed in (True, False):
-        CS = car.CarState(cruiseState={"available": True, "standstill": standstill})
-        CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)]
-        self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
-
-        # speed should only update if not at standstill and button falling edge
-        should_equal = standstill or pressed
-        self.assertEqual(should_equal, self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
-
-  def test_set_gas_pressed(self):
-    """
-    Asserts pressing set while enabled with gas pressed sets
-    the speed to the maximum of vEgo and current cruise speed.
-    """
-
-    for v_ego in np.linspace(0, 100, 101):
-      self.reset_cruise_speed_state()
-      self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False)
-
-      # first decrement speed, then perform gas pressed logic
-      expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT
-      expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH)  # clip to min of vEgo
-      expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX))
-
-      CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True})
-      CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)]
-      self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
-
-      # TODO: fix skipping first run due to enabled on rising edge exception
-      if v_ego == 0.0:
-        continue
-      self.assertEqual(expected_v_cruise_kph, self.v_cruise_helper.v_cruise_kph)
-
-  def test_initialize_v_cruise(self):
-    """
-    Asserts allowed cruise speeds on enabling with SET.
-    """
-
-    for experimental_mode in (True, False):
-      for v_ego in np.linspace(0, 100, 101):
-        self.reset_cruise_speed_state()
-        self.assertFalse(self.v_cruise_helper.v_cruise_initialized)
-
-        self.enable(float(v_ego), experimental_mode)
-        self.assertTrue(V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX)
-        self.assertTrue(self.v_cruise_helper.v_cruise_initialized)
-
-
-if __name__ == "__main__":
-  unittest.main()

selfdrive/controls/tests/test_following_distance.py

@@ -1,47 +0,0 @@
-#!/usr/bin/env python3
-import unittest
-import itertools
-from parameterized import parameterized_class
-
-from openpilot.common.params import Params
-from cereal import log
-
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW
-from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
-
-
-def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False):
-  man = Maneuver(
-    '',
-    duration=t_end,
-    initial_speed=float(v_lead),
-    lead_relevancy=True,
-    initial_distance_lead=100,
-    speed_lead_values=[v_lead],
-    breakpoints=[0.],
-    e2e=e2e,
-  )
-  valid, output = man.evaluate()
-  assert valid
-  return output[-1,2] - output[-1,1]
-
-
-@parameterized_class(("e2e", "personality", "speed"), itertools.product(
-                      [True, False], # e2e
-                      [log.LongitudinalPersonality.relaxed, # personality
-                       log.LongitudinalPersonality.standard,
-                       log.LongitudinalPersonality.aggressive],
-                      [0,10,35])) # speed
-class TestFollowingDistance(unittest.TestCase):
-  def test_following_distance(self):
-    params = Params()
-    params.put("LongitudinalPersonality", str(self.personality))
-    v_lead = float(self.speed)
-    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e)
-    correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
-    err_ratio = 0.2 if self.e2e else 0.1
-    self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(err_ratio * correct_steady_state + .5))
-
-
-if __name__ == "__main__":
-  unittest.main()

selfdrive/controls/tests/test_lateral_mpc.py

@@ -1,89 +0,0 @@
-import unittest
-import numpy as np
-from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
-from openpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
-from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
-
-
-def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
-            lane_width=3.6, poly_shift=0.):
-
-  if lat_mpc is None:
-    lat_mpc = LateralMpc()
-  lat_mpc.set_weights(1., .1, 0.0, .05, 800)
-
-  y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
-  heading_pts = np.zeros(LAT_MPC_N + 1)
-  curv_rate_pts = np.zeros(LAT_MPC_N + 1)
-
-  x0 = np.array([x_init, y_init, psi_init, curvature_init])
-  p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1),
-                      CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)])
-
-  # converge in no more than 10 iterations
-  for _ in range(10):
-    lat_mpc.run(x0, p,
-                y_pts, heading_pts, curv_rate_pts)
-  return lat_mpc.x_sol
-
-
-class TestLateralMpc(unittest.TestCase):
-
-  def _assert_null(self, sol, curvature=1e-6):
-    for i in range(len(sol)):
-      self.assertAlmostEqual(sol[0,i,1], 0., delta=curvature)
-      self.assertAlmostEqual(sol[0,i,2], 0., delta=curvature)
-      self.assertAlmostEqual(sol[0,i,3], 0., delta=curvature)
-
-  def _assert_simmetry(self, sol, curvature=1e-6):
-    for i in range(len(sol)):
-      self.assertAlmostEqual(sol[0,i,1], -sol[1,i,1], delta=curvature)
-      self.assertAlmostEqual(sol[0,i,2], -sol[1,i,2], delta=curvature)
-      self.assertAlmostEqual(sol[0,i,3], -sol[1,i,3], delta=curvature)
-      self.assertAlmostEqual(sol[0,i,0], sol[1,i,0], delta=curvature)
-
-  def test_straight(self):
-    sol = run_mpc()
-    self._assert_null(np.array([sol]))
-
-  def test_y_symmetry(self):
-    sol = []
-    for y_init in [-0.5, 0.5]:
-      sol.append(run_mpc(y_init=y_init))
-    self._assert_simmetry(np.array(sol))
-
-  def test_poly_symmetry(self):
-    sol = []
-    for poly_shift in [-1., 1.]:
-      sol.append(run_mpc(poly_shift=poly_shift))
-    self._assert_simmetry(np.array(sol))
-
-  def test_curvature_symmetry(self):
-    sol = []
-    for curvature_init in [-0.1, 0.1]:
-      sol.append(run_mpc(curvature_init=curvature_init))
-    self._assert_simmetry(np.array(sol))
-
-  def test_psi_symmetry(self):
-    sol = []
-    for psi_init in [-0.1, 0.1]:
-      sol.append(run_mpc(psi_init=psi_init))
-    self._assert_simmetry(np.array(sol))
-
-  def test_no_overshoot(self):
-    y_init = 1.
-    sol = run_mpc(y_init=y_init)
-    for y in list(sol[:,1]):
-      self.assertGreaterEqual(y_init, abs(y))
-
-  def test_switch_convergence(self):
-    lat_mpc = LateralMpc()
-    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0)
-    right_psi_deg = np.degrees(sol[:,2])
-    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0)
-    left_psi_deg = np.degrees(sol[:,2])
-    np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)
-
-
-if __name__ == "__main__":
-  unittest.main()

selfdrive/controls/tests/test_leads.py

@@ -1,36 +0,0 @@
-#!/usr/bin/env python3
-import unittest
-
-import cereal.messaging as messaging
-
-from openpilot.selfdrive.test.process_replay import replay_process_with_name
-from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA
-
-
-class TestLeads(unittest.TestCase):
-  def test_radar_fault(self):
-    # if there's no radar-related can traffic, radard should either not respond or respond with an error
-    # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
-    def single_iter_pkg():
-      # single iter package, with meaningless cans and empty carState/modelV2
-      msgs = []
-      for _ in range(5):
-        can = messaging.new_message("can", 1)
-        cs = messaging.new_message("carState")
-        msgs.append(can.as_reader())
-        msgs.append(cs.as_reader())
-      model = messaging.new_message("modelV2")
-      msgs.append(model.as_reader())
-
-      return msgs
-
-    msgs = [m for _ in range(3) for m in single_iter_pkg()]
-    out = replay_process_with_name("radard", msgs, fingerprint=TOYOTA.COROLLA_TSS2)
-    states = [m for m in out if m.which() == "radarState"]
-    failures = [not state.valid and len(state.radarState.radarErrors) for state in states]
-
-    self.assertTrue(len(states) == 0 or all(failures))
-
-
-if __name__ == "__main__":
-  unittest.main()

selfdrive/controls/tests/test_startup.py

@@ -1,120 +0,0 @@
-import os
-from parameterized import parameterized
-
-from cereal import log, car
-import cereal.messaging as messaging
-from openpilot.common.params import Params
-from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
-from openpilot.selfdrive.car.fingerprints import _FINGERPRINTS
-from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA
-from openpilot.selfdrive.car.mazda.values import CAR as MAZDA
-from openpilot.selfdrive.controls.lib.events import EVENT_NAME
-from openpilot.selfdrive.manager.process_config import managed_processes
-
-EventName = car.CarEvent.EventName
-Ecu = car.CarParams.Ecu
-
-COROLLA_FW_VERSIONS = [
-  (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'),
-  (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'),
-  (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'),
-  (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'),
-  (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'),
-]
-COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')]
-COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1]
-
-CX5_FW_VERSIONS = [
-  (Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-  (Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'),
-]
-
-
-@parameterized.expand([
-  # TODO: test EventName.startup for release branches
-
-  # officially supported car
-  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"),
-  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS, "toyota"),
-
-  # dashcamOnly car
-  (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"),
-  (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS, "mazda"),
-
-  # unrecognized car with no fw
-  (EventName.startupNoFw, None, None, ""),
-  (EventName.startupNoFw, None, None, ""),
-
-  # unrecognized car
-  (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
-  (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"),
-
-  # fuzzy match
-  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
-  (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"),
-])
-def test_startup_alert(expected_event, car_model, fw_versions, brand):
-  controls_sock = messaging.sub_sock("controlsState")
-  pm = messaging.PubMaster(['can', 'pandaStates'])
-
-  params = Params()
-  params.put_bool("OpenpilotEnabledToggle", True)
-
-  # Build capnn version of FW array
-  if fw_versions is not None:
-    car_fw = []
-    cp = car.CarParams.new_message()
-    for ecu, addr, subaddress, version in fw_versions:
-      f = car.CarParams.CarFw.new_message()
-      f.ecu = ecu
-      f.address = addr
-      f.fwVersion = version
-      f.brand = brand
-
-      if subaddress is not None:
-        f.subAddress = subaddress
-
-      car_fw.append(f)
-    cp.carVin = "1" * 17
-    cp.carFw = car_fw
-    params.put("CarParamsCache", cp.to_bytes())
-  else:
-    os.environ['SKIP_FW_QUERY'] = '1'
-
-  managed_processes['controlsd'].start()
-
-  assert pm.wait_for_readers_to_update('can', 5)
-  pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]]))
-
-  assert pm.wait_for_readers_to_update('pandaStates', 5)
-  msg = messaging.new_message('pandaStates', 1)
-  msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno
-  pm.send('pandaStates', msg)
-
-  # fingerprint
-  if (car_model is None) or (fw_versions is not None):
-    finger = {addr: 1 for addr in range(1, 100)}
-  else:
-    finger = _FINGERPRINTS[car_model][0]
-
-  msgs = [[addr, 0, b'\x00'*length, 0] for addr, length in finger.items()]
-  for _ in range(1000):
-    # controlsd waits for boardd to echo back that it has changed the multiplexing mode
-    if not params.get_bool("ObdMultiplexingChanged"):
-      params.put_bool("ObdMultiplexingChanged", True)
-
-    pm.send('can', can_list_to_can_capnp(msgs))
-    assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}"
-
-    ctrls = messaging.drain_sock(controls_sock)
-    if len(ctrls):
-      event_name = ctrls[0].controlsState.alertType.split("/")[0]
-      assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}"
-      break
-  else:
-    raise Exception(f"failed to fingerprint {car_model}")

selfdrive/controls/tests/test_state_machine.py

@@ -1,109 +0,0 @@
-#!/usr/bin/env python3
-import unittest
-
-from cereal import car, log
-from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.car.car_helpers import interfaces
-from openpilot.selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME
-from openpilot.selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \
-                                          AudibleAlert, EVENTS
-from openpilot.selfdrive.car.mock.values import CAR as MOCK
-
-State = log.ControlsState.OpenpilotState
-
-# The event types that maintain the current state
-MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,),
-                   State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)}
-ALL_STATES = tuple(State.schema.enumerants.values())
-# The event types checked in DISABLED section of state machine
-ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)
-
-
-def make_event(event_types):
-  event = {}
-  for ev in event_types:
-    event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW,
-                      VisualAlert.none, AudibleAlert.none, 1.)
-  EVENTS[0] = event
-  return 0
-
-
-class TestStateMachine(unittest.TestCase):
-
-  def setUp(self):
-    CarInterface, CarController, CarState = interfaces[MOCK.MOCK]
-    CP = CarInterface.get_non_essential_params(MOCK.MOCK)
-    CI = CarInterface(CP, CarController, CarState)
-
-    self.controlsd = Controls(CI=CI)
-    self.controlsd.events = Events()
-    self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
-    self.CS = car.CarState()
-
-  def test_immediate_disable(self):
-    for state in ALL_STATES:
-      for et in MAINTAIN_STATES[state]:
-        self.controlsd.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
-        self.controlsd.state = state
-        self.controlsd.state_transition(self.CS)
-        self.assertEqual(State.disabled, self.controlsd.state)
-        self.controlsd.events.clear()
-
-  def test_user_disable(self):
-    for state in ALL_STATES:
-      for et in MAINTAIN_STATES[state]:
-        self.controlsd.events.add(make_event([et, ET.USER_DISABLE]))
-        self.controlsd.state = state
-        self.controlsd.state_transition(self.CS)
-        self.assertEqual(State.disabled, self.controlsd.state)
-        self.controlsd.events.clear()
-
-  def test_soft_disable(self):
-    for state in ALL_STATES:
-      if state == State.preEnabled:  # preEnabled considers NO_ENTRY instead
-        continue
-      for et in MAINTAIN_STATES[state]:
-        self.controlsd.events.add(make_event([et, ET.SOFT_DISABLE]))
-        self.controlsd.state = state
-        self.controlsd.state_transition(self.CS)
-        self.assertEqual(self.controlsd.state, State.disabled if state == State.disabled else State.softDisabling)
-        self.controlsd.events.clear()
-
-  def test_soft_disable_timer(self):
-    self.controlsd.state = State.enabled
-    self.controlsd.events.add(make_event([ET.SOFT_DISABLE]))
-    self.controlsd.state_transition(self.CS)
-    for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
-      self.assertEqual(self.controlsd.state, State.softDisabling)
-      self.controlsd.state_transition(self.CS)
-
-    self.assertEqual(self.controlsd.state, State.disabled)
-
-  def test_no_entry(self):
-    # Make sure noEntry keeps us disabled
-    for et in ENABLE_EVENT_TYPES:
-      self.controlsd.events.add(make_event([ET.NO_ENTRY, et]))
-      self.controlsd.state_transition(self.CS)
-      self.assertEqual(self.controlsd.state, State.disabled)
-      self.controlsd.events.clear()
-
-  def test_no_entry_pre_enable(self):
-    # preEnabled with noEntry event
-    self.controlsd.state = State.preEnabled
-    self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
-    self.controlsd.state_transition(self.CS)
-    self.assertEqual(self.controlsd.state, State.preEnabled)
-
-  def test_maintain_states(self):
-    # Given current state's event type, we should maintain state
-    for state in ALL_STATES:
-      for et in MAINTAIN_STATES[state]:
-        self.controlsd.state = state
-        self.controlsd.events.add(make_event([et]))
-        self.controlsd.state_transition(self.CS)
-        self.assertEqual(self.controlsd.state, state)
-        self.controlsd.events.clear()
-
-
-if __name__ == "__main__":
-  unittest.main()