Deal with long lag compensation (#20004)

* add hack for big lags

* wrong bracket

* new pathplanner values

selfdrive/controls/lib/pathplanner.py

@@ -191,6 +191,14 @@ class PathPlanner():
     next_tire_angle = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle)
     next_tire_angle_rate = self.mpc_solution.tire_angle_rate[0]
 
+    # TODO This gets around the fact that MPC can plan to turn and turn back in the
+    # time between now and delay, need better abstraction between planner and controls
+    plan_ahead_idx = sum(self.t_idxs < delay)
+    if next_tire_angle_rate > 0:
+      next_tire_angle = max(list(self.mpc_solution.tire_angle)[:plan_ahead_idx] + [next_tire_angle])
+    else:
+      next_tire_angle = min(list(self.mpc_solution.tire_angle)[:plan_ahead_idx] + [next_tire_angle])
+
     # reset to current steer angle if not active or overriding
     if active:
       tire_angle_desired = next_tire_angle

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-f69a7c09518ad0ee7a7ca88cd4a8974145f24a6d
+537244985e387f5d290d0deaaf900dc8231d13f8