diff --git a/cereal b/cereal index b88523f05a..42f84fd85d 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit b88523f05ac958f87a8f6d57c3f4bb20da55f216 +Subproject commit 42f84fd85d06c0fc85371daa2b4820fca49d763e diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2ef9051122..e959b14ee3 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -122,8 +122,9 @@ class LongitudinalPlanner: self.mpc.update(sm['radarState'], v_cruise, x, v, a, j) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) + self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N] - self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution) + self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N] self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution) # TODO counter is only needed because radar is glitchy, remove once radar is gone diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 543274d841..d8cf9386d1 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -22,6 +22,7 @@ def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner): uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist() uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist() uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist() + uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist() pm.send('uiPlan', ui_send) def plannerd_thread(sm=None, pm=None): diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index b44c9c2f46..79769e14b6 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -dfa8e947c4ef76a9d89974a434e94a078e1ccc6a \ No newline at end of file +f9c7e05b836c4bff364978752e82d64b90f9d6e6