use long plan in lat plan

selfdrive/controls/lib/lateral_planner.py

@@ -52,10 +52,9 @@ class LateralPlanner:
   def update(self, sm, v_plan):
     # clip speed , lateral planning is not possible at 0 speed
     self.v_ego = max(MIN_SPEED, sm['carState'].vEgo)
-    v_plan = self.v_ego * np.ones(len(T_IDXS))
+    v_plan = np.clip(v_plan, MIN_SPEED, np.inf)
     self.v_plan = v_plan
-    plan_odo = np.concatenate(([0], np.cumsum(((v_plan[0:-1] + v_plan[1:])/2) * np.diff(T_IDXS))))
-    plan_odo = np.clip(plan_odo, MIN_SPEED, np.inf)[:LAT_MPC_N + 1]
+    plan_odo = np.concatenate(([0], np.cumsum(((v_plan[0:-1] + v_plan[1:])/2) * np.diff(T_IDXS))))[:LAT_MPC_N + 1]
     measured_curvature = sm['controlsState'].curvature
 
     # Parse model predictions

selfdrive/controls/plannerd.py

@@ -30,10 +30,10 @@ def plannerd_thread(sm=None, pm=None):
     sm.update()
 
     if sm.updated['modelV2']:
-      lateral_planner.update(sm)
-      lateral_planner.publish(sm, pm)
       longitudinal_planner.update(sm)
       longitudinal_planner.publish(sm, pm)
+      lateral_planner.update(sm, longitudinal_planner.mpc.v_solution)
+      lateral_planner.publish(sm, pm)
 
 
 def main(sm=None, pm=None):