safer low speed (#21705)

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:

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);

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;
 
 }

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-f2bad42b2f22dc4f2843c5697f5444f0fb4af175
+b13f5e632814c6ebb159a325331fd74da3a68066