add jerk to longitudinal plan (#21598)

* add jerk output, so its like lateral

* typo

* add to packet

* update cereal and ref

selfdrive/controls/lib/lead_mpc.py

@@ -25,6 +25,10 @@ class LeadMpc():
     self.duration = 0
     self.status = False
 
+    self.v_solution = np.zeros(CONTROL_N)
+    self.a_solution = np.zeros(CONTROL_N)
+    self.j_solution = np.zeros(CONTROL_N)
+
   def reset_mpc(self):
     ffi, self.libmpc = libmpc_py.get_libmpc(self.lead_id)
     self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
@@ -85,6 +89,7 @@ class LeadMpc():
     self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
     self.v_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.v_ego)
     self.a_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.a_ego)
+    self.j_solution = interp(T_IDXS[:CONTROL_N], MPC_T[:-1], self.mpc_solution.j_ego)
     self.duration = int((sec_since_boot() - t) * 1e9)
 
     # Reset if NaN or goes through lead car

selfdrive/controls/lib/long_mpc.py

@@ -29,6 +29,10 @@ class LongitudinalMpc():
     self.cur_state[0].v_ego = 0
     self.cur_state[0].a_ego = 0
 
+    self.v_solution = [0.0 for i in range(len(T_IDXS))]
+    self.a_solution = [0.0 for i in range(len(T_IDXS))]
+    self.j_solution = [0.0 for i in range(len(T_IDXS)-1)]
+
   def set_accel_limits(self, min_a, max_a):
     self.min_a = min_a
     self.max_a = max_a
@@ -54,6 +58,7 @@ class LongitudinalMpc():
 
     self.v_solution = list(self.mpc_solution.v_ego)
     self.a_solution = list(self.mpc_solution.a_ego)
+    self.j_solution = list(self.mpc_solution.j_ego)
 
     # Reset if NaN or goes through lead car
     nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)

selfdrive/controls/lib/longitudinal_planner.py

@@ -109,6 +109,7 @@ class Planner():
         self.longitudinalPlanSource = key
         self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
         self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]
+        self.j_desired_trajectory = self.mpcs[key].j_solution[:CONTROL_N]
         next_a = self.mpcs[key].a_solution[5]
 
     # determine fcw
@@ -140,6 +141,7 @@ class Planner():
 
     longitudinalPlan.speeds = [float(x) for x in self.v_desired_trajectory]
     longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory]
+    longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory]
 
     longitudinalPlan.hasLead = self.mpcs['lead0'].status
     longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-97382f81d5d59213c25b5a3b553b3da2076742a2
+f2bad42b2f22dc4f2843c5697f5444f0fb4af175