Different lag comp (#20024)

* more sensible

* different names for different segs

* new ref

* this is checked somewhere

selfdrive/controls/lib/lateral_planner.py

@@ -186,16 +186,14 @@ class LateralPlanner():
 
     # TODO this needs more thought, use .2s extra for now to estimate other delays
     delay = CP.steerActuatorDelay + .2
-    next_curvature = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature)
+    next_curvature = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.curvature)
+    psi = interp(delay, self.t_idxs[:MPC_N+1], self.mpc_solution.psi)
     next_curvature_rate = self.mpc_solution.curvature_rate[0]
-
+    next_curvature_from_psi = psi/(v_ego*delay)
-    # TODO This gets around the fact that MPC can plan to turn and turn back in the
+    if psi > self.mpc_solution.curvature[0] * delay * v_ego:
-    # time between now and delay, need better abstraction between planner and controls
+      next_curvature = max(next_curvature_from_psi, next_curvature)
-    plan_ahead_idx = sum(self.t_idxs < delay)
-    if next_curvature_rate > 0:
-      next_curvature = max(list(self.mpc_solution.curvature)[:plan_ahead_idx] + [next_curvature])
     else:
-      next_curvature = min(list(self.mpc_solution.curvature)[:plan_ahead_idx] + [next_curvature])
+      next_curvature = min(next_curvature_from_psi, next_curvature)
 
     # reset to current steer angle if not active or overriding
     if active:

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-c1740f64ad8a8e789023e062bc1ab125438e6a0a
+defb3dc7205a3d501829f4a780241dfb97132025

selfdrive/test/process_replay/test_processes.py

@@ -14,9 +14,9 @@ INJECT_MODEL = 0
 segments = [
   ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"),     # HYUNDAI.SONATA
   ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"),     # TOYOTA.PRIUS (INDI)
-  ("TOYOTA", "0982d79ebb0de295|2021-01-03--20-03-36--6"),      # TOYOTA.RAV4  (LQR)
+  ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"),      # TOYOTA.RAV4  (LQR)
   ("HONDA", "0982d79ebb0de295|2021-01-08--10-13-10--6"),       # HONDA.CIVIC (NIDEC)
-  ("HONDA", "a8e8bf6a3864361b|2021-01-04--03-01-18--2"),       # HONDA.ACCORD (BOSCH)
+  ("HONDA2", "a8e8bf6a3864361b|2021-01-04--03-01-18--2"),       # HONDA.ACCORD (BOSCH)
   ("CHRYSLER", "52d86230ee29aa84|2021-01-10--17-16-34--30"),   # CHRYSLER.PACIFICA
   ("SUBARU", "4d70bc5e608678be|2021-01-15--17-02-04--5"),      # SUBARU.IMPREZA
   ("GM", "ae3ed0eb20960a20|2021-01-15--15-04-06--8"),          # GM.VOLT