try upper/lower delay similar to longcontrol lag comp

selfdrive/car/toyota/interface.py

@@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
 
       # LTA control can be more delayed and winds up more often
-      ret.steerActuatorDelay = 0.18
+      ret.steerActuatorDelay = 0.35
       ret.steerLimitTimer = 0.8
     else:
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)

selfdrive/controls/controlsd.py

@@ -614,10 +614,16 @@ class Controls:
       actuators.accel = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
 
       # Steering PID loop and lateral MPC
-      self.desired_curvature, self.desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
+      desired_curvature_lower, desired_curvature_rate_lower = get_lag_adjusted_curvature(0.18, CS.vEgo,
                                                                                          lat_plan.psis,
                                                                                          lat_plan.curvatures,
                                                                                          lat_plan.curvatureRates)
+      desired_curvature_upper, desired_curvature_rate_upper = get_lag_adjusted_curvature(0.35, CS.vEgo,
+                                                                                         lat_plan.psis,
+                                                                                         lat_plan.curvatures,
+                                                                                         lat_plan.curvatureRates)
+      self.desired_curvature = desired_curvature_lower if abs(desired_curvature_lower) < abs(desired_curvature_upper) else desired_curvature_upper
+      self.desired_curvature_rate = desired_curvature_rate_lower if abs(desired_curvature_rate_lower) < abs(desired_curvature_rate_upper) else desired_curvature_rate_upper
       actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                                              self.last_actuators, self.steer_limited, self.desired_curvature,
                                                                              self.desired_curvature_rate, self.sm['liveLocationKalman'])

selfdrive/controls/lib/drive_helpers.py

@@ -163,7 +163,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)
 
 
-def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
+def get_lag_adjusted_curvature(steer_actuator_delay, v_ego, psis, curvatures, curvature_rates):
   if len(psis) != CONTROL_N:
     psis = [0.0]*CONTROL_N
     curvatures = [0.0]*CONTROL_N
@@ -171,7 +171,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates):
   v_ego = max(MIN_SPEED, v_ego)
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
-  delay = CP.steerActuatorDelay + .2
+  delay = steer_actuator_delay + .2
 
   # MPC can plan to turn the wheel and turn back before t_delay. This means
   # in high delay cases some corrections never even get commanded. So just use