Update heading cost

commaai · May 6, 2022 · a987a0a · a987a0a
1 parent b008704
commit a987a0a
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
@@ -59,9 +59,11 @@ def update(self, sm):
     # Calculate final driving path and set MPC costs
     if self.use_lanelines:
       d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
+      heading_cost = MPC_COST_LAT.HEADING
     else:
       d_path_xyz = self.path_xyz
-    self.lat_mpc.set_weights(MPC_COST_LAT.PATH, 0.15 * MPC_COST_LAT.HEADING, self.steer_rate_cost)
+      heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
+    self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, self.steer_rate_cost)
 
     y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
     heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)