From e43afeab322b3637614ffc031a19ffce218edb35 Mon Sep 17 00:00:00 2001 From: David Walsh Date: Mon, 4 Mar 2019 21:00:06 -0700 Subject: [PATCH] Update longitudinal mpc with follow distance param Update generator.cpp to parameterize the follow time. Then, using the process outlined in the ACADO docs (https://acado.github.io/cgt_overview.html), rebuild the generator binary and run it to regenerate the source files in lib_mpc_export, then manually copy them into the openpilot repo. Update longitudinal_mpc.c, libmpc_py.py, and planner.py to take in the additional follow_time parameter. Incorporate arne182's PR to update the Makefile (https://github.com/commaai/openpilot/pull/248/). Note that I did not use the generate recipie inside the repo; I created the files outside of the repo as stated above. --- .../controls/lib/longitudinal_mpc/Makefile | 10 +-- .../lib/longitudinal_mpc/generator.cpp | 18 +++-- .../lib_mpc_export/acado_common.h | 18 ++--- .../lib_mpc_export/acado_integrator.c | 1 + .../lib_mpc_export/acado_solver.c | 81 ++++++++++--------- .../lib/longitudinal_mpc/libmpc_py.py | 3 +- .../lib/longitudinal_mpc/longitudinal_mpc.c | 3 +- selfdrive/controls/lib/planner.py | 4 +- 8 files changed, 77 insertions(+), 61 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc/Makefile b/selfdrive/controls/lib/longitudinal_mpc/Makefile index 17f78eada990df..4e5bfd3807d39c 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/Makefile +++ b/selfdrive/controls/lib/longitudinal_mpc/Makefile @@ -13,12 +13,10 @@ QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONEL ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado -ifeq ($(UNAME_S),Darwin) - ACADO_LIBS := -lacado_toolkit_s -else ifeq ($(UNAME_M),aarch64) - ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a +ifeq ($(UNAME_M),aarch64) + ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit_s.so -l:libacado_casadi.a -l:libacado_csparse.a else - ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a + ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit_s.so -l:libacado_casadi.a -l:libacado_csparse.a endif OBJS = \ @@ -80,7 +78,7 @@ generator: generator.cpp .PHONY: generate generate: generator - ./generator + LD_LIBRARY_PATH=../../../../phonelibs/acado/x64/lib ./generator .PHONY: clean clean: diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp index f5c95394d8d3f1..33206891556ede 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp +++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp @@ -5,10 +5,9 @@ const int controlHorizon = 50; using namespace std; #define G 9.81 -#define TR 1.8 -#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) -#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1)) +#define RW(v_ego, v_l, follow_time) (v_ego * follow_time - (v_l - v_ego) * follow_time + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) +#define NORM_RW_ERROR(v_ego, v_l, p, follow_time) ((RW(v_ego, v_l, follow_time) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1)) int main( ) { @@ -24,7 +23,12 @@ int main( ) Control j_ego; - auto desired = 4.0 + RW(v_ego, v_l); + // follow distance expressed as a stopping distance in seconds + // see https://github.com/rhinodavid/CommaButtons + // see https://github.com/acado/acado/issues/54 for a discussion of `OnlineData` + OnlineData follow_time; + + auto desired = 4.0 + RW(v_ego, v_l, follow_time); auto d_l = x_l - x_ego; // Directly calculate a_l to prevent instabilites due to discretization @@ -41,7 +45,7 @@ int main( ) // Running cost Function h; - h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); + h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l, follow_time)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired, follow_time)); h << (d_l - desired) / (0.05 * v_ego + 0.5); h << a_ego * (0.1 * v_ego + 1.0); h << j_ego * (0.1 * v_ego + 1.0); @@ -51,7 +55,7 @@ int main( ) // Terminal cost Function hN; - hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired)); + hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l, follow_time)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired, follow_time)); hN << (d_l - desired) / (0.05 * v_ego + 0.5); hN << a_ego * (0.1 * v_ego + 1.0); @@ -79,7 +83,7 @@ int main( ) ocp.minimizeLSQEndTerm(QN, hN); ocp.subjectTo( 0.0 <= v_ego); - ocp.setNOD(2); + ocp.setNOD(3); OCPexport mpc(ocp); mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON ); diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h index cecba6e7408cba..db1be5e0c71be8 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h @@ -64,7 +64,7 @@ extern "C" /** Number of control/estimation intervals. */ #define ACADO_N 20 /** Number of online data values. */ -#define ACADO_NOD 2 +#define ACADO_NOD 3 /** Number of path constraints. */ #define ACADO_NPAC 0 /** Number of control variables. */ @@ -114,11 +114,11 @@ real_t x[ 126 ]; */ real_t u[ 20 ]; -/** Matrix of size: 21 x 2 (row major format) +/** Matrix of size: 21 x 3 (row major format) * * Matrix containing 21 online data vectors. */ -real_t od[ 42 ]; +real_t od[ 63 ]; /** Column vector of size: 80 * @@ -160,14 +160,14 @@ real_t rhs_aux[ 10 ]; real_t rk_ttt; -/** Row vector of size: 51 */ -real_t rk_xxx[ 51 ]; +/** Row vector of size: 52 */ +real_t rk_xxx[ 52 ]; /** Matrix of size: 4 x 48 (row major format) */ real_t rk_kkk[ 192 ]; -/** Row vector of size: 51 */ -real_t state[ 51 ]; +/** Row vector of size: 52 */ +real_t state[ 52 ]; /** Column vector of size: 120 */ real_t d[ 120 ]; @@ -187,8 +187,8 @@ real_t evGu[ 120 ]; /** Column vector of size: 30 */ real_t objAuxVar[ 30 ]; -/** Row vector of size: 9 */ -real_t objValueIn[ 9 ]; +/** Row vector of size: 10 */ +real_t objValueIn[ 10 ]; /** Row vector of size: 32 */ real_t objValueOut[ 32 ]; diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c index 2e77d54147dbee..d348a5ecbfb161 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c @@ -145,6 +145,7 @@ rk_eta[47] = 0.0000000000000000e+00; acadoWorkspace.rk_xxx[48] = rk_eta[48]; acadoWorkspace.rk_xxx[49] = rk_eta[49]; acadoWorkspace.rk_xxx[50] = rk_eta[50]; +acadoWorkspace.rk_xxx[51] = rk_eta[51]; for (run1 = 0; run1 < 1; ++run1) { diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c index 676787bbfcbc7b..3b88710e5fd164 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -45,8 +45,9 @@ acadoWorkspace.state[4] = acadoVariables.x[lRun1 * 6 + 4]; acadoWorkspace.state[5] = acadoVariables.x[lRun1 * 6 + 5]; acadoWorkspace.state[48] = acadoVariables.u[lRun1]; -acadoWorkspace.state[49] = acadoVariables.od[lRun1 * 2]; -acadoWorkspace.state[50] = acadoVariables.od[lRun1 * 2 + 1]; +acadoWorkspace.state[49] = acadoVariables.od[lRun1 * 3]; +acadoWorkspace.state[50] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.state[51] = acadoVariables.od[lRun1 * 3 + 2]; ret = acado_integrate(acadoWorkspace.state, 1, lRun1); @@ -108,36 +109,37 @@ void acado_evaluateLSQ(const real_t* in, real_t* out) { const real_t* xd = in; const real_t* u = in + 6; +const real_t* od = in + 7; /* Vector of auxiliary variables; number of elements: 30. */ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]); a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[9] = (a[8]*(real_t)(5.0000000000000000e-01)); a[10] = (a[4]*a[4]); -a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); +a[11] = (((real_t)(2.9999999999999999e-01)*((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01))); a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[16] = (a[15]*(real_t)(5.0000000000000000e-01)); a[17] = (a[14]*a[14]); -a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); -a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); +a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[19] = (((real_t)(2.9999999999999999e-01)*(((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[12]))-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]); a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); +a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); +a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[28] = (a[26]*a[26]); @@ -145,7 +147,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); /* Compute outputs: */ out[0] = (a[1]-a[3]); -out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[4] = a[6]; @@ -155,10 +157,10 @@ out[7] = a[20]; out[8] = (a[22]-a[25]); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]); -out[11] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); +out[11] = ((((real_t)(0.0000000000000000e+00)-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); out[12] = (real_t)(0.0000000000000000e+00); out[13] = a[26]; -out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]); +out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[29])))*a[26]); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (real_t)(0.0000000000000000e+00); out[17] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -181,36 +183,37 @@ out[31] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) { const real_t* xd = in; +const real_t* od = in + 6; /* Vector of auxiliary variables; number of elements: 30. */ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]); a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[9] = (a[8]*(real_t)(5.0000000000000000e-01)); a[10] = (a[4]*a[4]); -a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); +a[11] = (((real_t)(2.9999999999999999e-01)*((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01))); a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[16] = (a[15]*(real_t)(5.0000000000000000e-01)); a[17] = (a[14]*a[14]); -a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); -a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); +a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[19] = (((real_t)(2.9999999999999999e-01)*(((((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[12]))-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]); a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); +a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); +a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[28] = (a[26]*a[26]); @@ -218,7 +221,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); /* Compute outputs: */ out[0] = (a[1]-a[3]); -out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = a[6]; out[4] = (a[11]-a[19]); @@ -227,10 +230,10 @@ out[6] = a[20]; out[7] = (a[22]-a[25]); out[8] = (real_t)(0.0000000000000000e+00); out[9] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]); -out[10] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); +out[10] = ((((real_t)(0.0000000000000000e+00)-((od[2]-(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*od[2]))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*od[2])-((xd[4]-xd[1])*od[2]))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); out[11] = (real_t)(0.0000000000000000e+00); out[12] = a[26]; -out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]); +out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-od[2])-((xd[4]+xd[4])*a[29])))*a[26]); out[14] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -383,8 +386,9 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 6 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.x[runObj * 6 + 4]; acadoWorkspace.objValueIn[5] = acadoVariables.x[runObj * 6 + 5]; acadoWorkspace.objValueIn[6] = acadoVariables.u[runObj]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 2 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 3]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 3 + 1]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 3 + 2]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0]; @@ -403,8 +407,9 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122]; acadoWorkspace.objValueIn[3] = acadoVariables.x[123]; acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[40]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[41]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[60]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[61]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[62]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; @@ -5205,8 +5210,9 @@ acadoWorkspace.state[3] = acadoVariables.x[index * 6 + 3]; acadoWorkspace.state[4] = acadoVariables.x[index * 6 + 4]; acadoWorkspace.state[5] = acadoVariables.x[index * 6 + 5]; acadoWorkspace.state[48] = acadoVariables.u[index]; -acadoWorkspace.state[49] = acadoVariables.od[index * 2]; -acadoWorkspace.state[50] = acadoVariables.od[index * 2 + 1]; +acadoWorkspace.state[49] = acadoVariables.od[index * 3]; +acadoWorkspace.state[50] = acadoVariables.od[index * 3 + 1]; +acadoWorkspace.state[51] = acadoVariables.od[index * 3 + 2]; acado_integrate(acadoWorkspace.state, index == 0, index); @@ -5257,8 +5263,9 @@ else { acadoWorkspace.state[48] = acadoVariables.u[19]; } -acadoWorkspace.state[49] = acadoVariables.od[40]; -acadoWorkspace.state[50] = acadoVariables.od[41]; +acadoWorkspace.state[49] = acadoVariables.od[60]; +acadoWorkspace.state[50] = acadoVariables.od[61]; +acadoWorkspace.state[51] = acadoVariables.od[62]; acado_integrate(acadoWorkspace.state, 1, 19); @@ -5333,8 +5340,9 @@ acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 6 + 3]; acadoWorkspace.objValueIn[4] = acadoVariables.x[lRun1 * 6 + 4]; acadoWorkspace.objValueIn[5] = acadoVariables.x[lRun1 * 6 + 5]; acadoWorkspace.objValueIn[6] = acadoVariables.u[lRun1]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2]; -acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 2 + 1]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 3]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 3 + 1]; +acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 3 + 2]; acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4]; @@ -5348,8 +5356,9 @@ acadoWorkspace.objValueIn[2] = acadoVariables.x[122]; acadoWorkspace.objValueIn[3] = acadoVariables.x[123]; acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; -acadoWorkspace.objValueIn[6] = acadoVariables.od[40]; -acadoWorkspace.objValueIn[7] = acadoVariables.od[41]; +acadoWorkspace.objValueIn[6] = acadoVariables.od[60]; +acadoWorkspace.objValueIn[7] = acadoVariables.od[61]; +acadoWorkspace.objValueIn[8] = acadoVariables.od[62]; acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index 36b6f8b7e0b8d3..765969d4433dea 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -39,7 +39,7 @@ def _get_libmpc(mpc_id): void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); int run_mpc(state_t * x0, log_t * solution, - double l, double a_l_0); + double l, double a_l_0, double follow_time); """) return (ffi, ffi.dlopen(libmpc_fn)) @@ -48,3 +48,4 @@ def _get_libmpc(mpc_id): def get_libmpc(mpc_id): return mpcs[mpc_id - 1] + \ No newline at end of file diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c index c0b43e53c93771..0aee482f012747 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -118,12 +118,13 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0 for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; } -int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ +int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double follow_time){ int i; for (i = 0; i <= NOD * N; i+= NOD){ acadoVariables.od[i] = l; acadoVariables.od[i+1] = a_l_0; + acadoVariables.od[i+2] = follow_time; } acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego; diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index e0a53e99736d95..c1a007470517cf 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -211,7 +211,9 @@ def update(self, CS, lead, v_cruise_setpoint): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + # hardcode follow time for now + follow_time = 1.8 + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, follow_time) duration = int((sec_since_boot() - t) * 1e9) self.send_mpc_solution(n_its, duration)