Add upper/lower bounds for longitudinal lag (#22235)

* Add upper/lower bounds for longitudinal lag

* add ref

* update ref

* bump cereal

* bump cereal

cereal

@@ -1 +1 @@
-Subproject commit 49a0ee9196ce013d422febdf61847d87f1f57627
+Subproject commit 5c3520d53fd86cb07ff5151e8951d9db3e2d750e

selfdrive/car/hyundai/interface.py

@@ -44,7 +44,7 @@ class CarInterface(CarInterfaceBase):
     ret.stoppingDecelRate = 2.0
     ret.startAccel = 0.0
 
-    ret.longitudinalActuatorDelay = 1.0 # s
+    ret.longitudinalActuatorDelayUpperBound = 1.0 # s
 
     if candidate == CAR.SANTA_FE:
       ret.lateralTuning.pid.kf = 0.00005

selfdrive/car/interfaces.py

@@ -86,7 +86,8 @@ class CarInterfaceBase():
     ret.longitudinalTuning.kpV = [1.]
     ret.longitudinalTuning.kiBP = [0.]
     ret.longitudinalTuning.kiV = [1.]
-    ret.longitudinalActuatorDelay = 0.15
+    ret.longitudinalActuatorDelayLowerBound = 0.15
+    ret.longitudinalActuatorDelayUpperBound = 0.15
     return ret
 
   # returns a car.CarState, pass in car.CarControl

selfdrive/controls/lib/longcontrol.py

@@ -70,9 +70,16 @@ class LongControl():
     # Interp control trajectory
     # TODO estimate car specific lag, use .15s for now
     if len(long_plan.speeds) == CONTROL_N:
-      v_target = interp(CP.longitudinalActuatorDelay, T_IDXS[:CONTROL_N], long_plan.speeds)
+      v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds)
+      a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
+
+      v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds)
+      a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
+
+      v_target = min(v_target_lower, v_target_upper)
+      a_target = min(a_target_lower, a_target_upper)
+
       v_target_future = long_plan.speeds[-1]
-      a_target = 2 * (v_target - long_plan.speeds[0])/CP.longitudinalActuatorDelay - long_plan.accels[0]
     else:
       v_target = 0.0
       v_target_future = 0.0

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-eb150cfb34f825d0db2dc30c1077357b0a66952e
+c967d67902bd36e82c8b9e70c5538475b448ac49