mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-03-02 19:33:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

safer low speed (#21705)

Browse Source
This commit is contained in:
HaraldSchafer
2021-07-25 15:15:50 -07:00
committed by GitHub
parent c47b646b84
commit 4bd4ed4999
4 changed files with 23 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/controls/lib/lead_mpc.py
View File

@@ -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:

2
selfdrive/controls/lib/lead_mpc_lib/generator.cpp
View File

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

40
selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c
View File

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

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
f2bad42b2f22dc4f2843c5697f5444f0fb4af175
b13f5e632814c6ebb159a325331fd74da3a68066