From b39f6efa3fc91b3df851a133109ec18f598dce68 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Mon, 6 Sep 2021 17:56:52 -0700
Subject: [PATCH] parameterize longitudinal actuator delay (#22153)

* paremetrize longitudinal actuator delay

* update ref
---
 cereal                                   | 2 +-
 selfdrive/car/interfaces.py              | 1 +
 selfdrive/controls/lib/longcontrol.py    | 5 ++---
 selfdrive/test/process_replay/ref_commit | 2 +-
 4 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/cereal b/cereal
index 0c929f74ce..89557c5a17 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 0c929f74ce2ca96ff95bd9c1b50b5840ccc4503d
+Subproject commit 89557c5a178c2105e15c2b705395b535c7dcf371
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 46799aa5ad..f74fe06e03 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -87,6 +87,7 @@ class CarInterfaceBase():
     ret.longitudinalTuning.kpV = [1.]
     ret.longitudinalTuning.kiBP = [0.]
     ret.longitudinalTuning.kiV = [1.]
+    ret.longitudinalActuatorDelay = 0.15
     return ret
 
   # returns a car.CarState, pass in car.CarControl
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 4e724afe7e..f8694e45e3 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -14,7 +14,6 @@ DECEL_THRESHOLD_TO_PID = 0.8
 DECEL_STOPPING_TARGET = 2.0  # apply at least this amount of brake to maintain the vehicle stationary
 
 RATE = 100.0
-DEFAULT_LONG_LAG = 0.15
 
 # As per ISO 15622:2018 for all speeds
 ACCEL_MIN_ISO = -3.5 # m/s^2
@@ -78,9 +77,9 @@ class LongControl():
     # Interp control trajectory
     # TODO estimate car specific lag, use .15s for now
     if len(long_plan.speeds) == CONTROL_N:
-      v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.speeds)
+      v_target = interp(CP.longitudinalActuatorDelay, T_IDXS[:CONTROL_N], long_plan.speeds)
       v_target_future = long_plan.speeds[-1]
-      a_target = 2 * (v_target - long_plan.speeds[0])/DEFAULT_LONG_LAG - long_plan.accels[0]
+      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
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 32affbb2c0..256ee05953 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-bc50c3f64b4677eee32bc49bdf69c3be265fd743
\ No newline at end of file
+d55568ec276ae8789faa1f69585f3c9ab7087205
\ No newline at end of file