diff --git a/selfdrive/controls/lib/lead_mpc.py b/selfdrive/controls/lib/lead_mpc.py index da5bbc25f5..ee8ea98e7b 100644 --- a/selfdrive/controls/lib/lead_mpc.py +++ b/selfdrive/controls/lib/lead_mpc.py @@ -95,7 +95,7 @@ class LeadMpc(): # Reset if NaN or goes through lead car crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego)) nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego) - backwards = min(self.mpc_solution[0].v_ego) < -0.01 + backwards = min(self.mpc_solution[0].v_ego) < -0.15 if ((backwards or crashing) and self.prev_lead_status) or nans: if t > self.last_cloudlog_t + 5.0: diff --git a/selfdrive/controls/lib/lead_mpc_lib/generator.cpp b/selfdrive/controls/lib/lead_mpc_lib/generator.cpp index f633db71ee..9941846a5f 100644 --- a/selfdrive/controls/lib/lead_mpc_lib/generator.cpp +++ b/selfdrive/controls/lib/lead_mpc_lib/generator.cpp @@ -69,7 +69,7 @@ int main( ) ocp.minimizeLSQ(Q, h); ocp.minimizeLSQEndTerm(QN, hN); - ocp.subjectTo( 0.0 <= v_ego); + ocp.subjectTo( -0.1 <= v_ego); ocp.setNOD(2); OCPexport mpc(ocp); diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c index 8cfc06f3b8..6dd373b8d5 100644 --- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c @@ -4227,64 +4227,64 @@ acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0]; acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1]; acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2]; tmp = acadoVariables.x[4] + acadoWorkspace.d[1]; -acadoWorkspace.lbA[0] = - tmp; +acadoWorkspace.lbA[0] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[0] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[7] + acadoWorkspace.d[4]; -acadoWorkspace.lbA[1] = - tmp; +acadoWorkspace.lbA[1] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[1] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[10] + acadoWorkspace.d[7]; -acadoWorkspace.lbA[2] = - tmp; +acadoWorkspace.lbA[2] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[2] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[13] + acadoWorkspace.d[10]; -acadoWorkspace.lbA[3] = - tmp; +acadoWorkspace.lbA[3] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[3] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[16] + acadoWorkspace.d[13]; -acadoWorkspace.lbA[4] = - tmp; +acadoWorkspace.lbA[4] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[4] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[19] + acadoWorkspace.d[16]; -acadoWorkspace.lbA[5] = - tmp; +acadoWorkspace.lbA[5] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[5] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[22] + acadoWorkspace.d[19]; -acadoWorkspace.lbA[6] = - tmp; +acadoWorkspace.lbA[6] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[6] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[25] + acadoWorkspace.d[22]; -acadoWorkspace.lbA[7] = - tmp; +acadoWorkspace.lbA[7] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[7] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[28] + acadoWorkspace.d[25]; -acadoWorkspace.lbA[8] = - tmp; +acadoWorkspace.lbA[8] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[8] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[31] + acadoWorkspace.d[28]; -acadoWorkspace.lbA[9] = - tmp; +acadoWorkspace.lbA[9] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[9] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[34] + acadoWorkspace.d[31]; -acadoWorkspace.lbA[10] = - tmp; +acadoWorkspace.lbA[10] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[10] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[37] + acadoWorkspace.d[34]; -acadoWorkspace.lbA[11] = - tmp; +acadoWorkspace.lbA[11] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[11] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[40] + acadoWorkspace.d[37]; -acadoWorkspace.lbA[12] = - tmp; +acadoWorkspace.lbA[12] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[12] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[43] + acadoWorkspace.d[40]; -acadoWorkspace.lbA[13] = - tmp; +acadoWorkspace.lbA[13] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[13] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[46] + acadoWorkspace.d[43]; -acadoWorkspace.lbA[14] = - tmp; +acadoWorkspace.lbA[14] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[14] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[49] + acadoWorkspace.d[46]; -acadoWorkspace.lbA[15] = - tmp; +acadoWorkspace.lbA[15] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[15] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[52] + acadoWorkspace.d[49]; -acadoWorkspace.lbA[16] = - tmp; +acadoWorkspace.lbA[16] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[16] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[55] + acadoWorkspace.d[52]; -acadoWorkspace.lbA[17] = - tmp; +acadoWorkspace.lbA[17] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[17] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[58] + acadoWorkspace.d[55]; -acadoWorkspace.lbA[18] = - tmp; +acadoWorkspace.lbA[18] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[18] = (real_t)1.0000000000000000e+12 - tmp; tmp = acadoVariables.x[61] + acadoWorkspace.d[58]; -acadoWorkspace.lbA[19] = - tmp; +acadoWorkspace.lbA[19] = (real_t)-1.0000000000000001e-01 - tmp; acadoWorkspace.ubA[19] = (real_t)1.0000000000000000e+12 - tmp; } diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2002a59f22..735ca6c7af 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f2bad42b2f22dc4f2843c5697f5444f0fb4af175 \ No newline at end of file +b13f5e632814c6ebb159a325331fd74da3a68066 \ No newline at end of file