try upper/lower delay similar to longcontrol lag comp

commaai · Oct 17, 2023 · 8e85333 · 8e85333
1 parent 7cfc1c6
commit 8e85333
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 7 deletions.
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
@@ -33,7 +33,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
       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)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
@@ -614,10 +614,16 @@ def state_control(self, CS):
       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,
-                                                                                       lat_plan.psis,
-                                                                                       lat_plan.curvatures,
-                                                                                       lat_plan.curvatureRates)
+      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'])

diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
@@ -163,15 +163,15 @@ 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
     curvature_rates = [0.0]*CONTROL_N
   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