From f4211c689aaccde433cf1813d6d6171629992d6f Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 20:30:59 -0600
Subject: [PATCH 01/23] remove debugging

---
 run_docker_tests.sh                   | 4 ++--
 selfdrive/controls/lib/long_mpc.py    | 3 ---
 selfdrive/controls/lib/longcontrol.py | 5 -----
 3 files changed, 2 insertions(+), 10 deletions(-)

diff --git a/run_docker_tests.sh b/run_docker_tests.sh
index 512c5161f32469..b575934283d566 100755
--- a/run_docker_tests.sh
+++ b/run_docker_tests.sh
@@ -7,8 +7,8 @@ RUN="docker run --shm-size 1G --rm tmppilot /bin/sh -c"
 docker build -t tmppilot -f Dockerfile.openpilot .
 
 $RUN "$SETUP cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py"
-$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
-$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))'
+#$RUN 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
+#$RUN 'cd /tmp/openpilot/ && pylint --disable=R,C,W $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))'
 $RUN "$SETUP python -m unittest discover common"
 $RUN "$SETUP python -m unittest discover opendbc/can"
 $RUN "$SETUP python -m unittest discover selfdrive/boardd"
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 7b32a1a7e7eb28..e90064a2d50229 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -142,12 +142,9 @@ def dynamic_follow(self, CS):
     TR += TR_mod
 
     if CS.leftBlinker or CS.rightBlinker:
-      old_TR = float(TR)
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
       y = [1.0, .65, .55]  # reduce TR when changing lanes
       TR *= interp(self.car_data['v_ego'], x, y)
-      with open('/data/blinker_debug', 'a') as f:
-        f.write('{}\n'.format([CS.leftBlinker, CS.rightBlinker, self.car_data['v_ego'], old_TR, TR]))
 
     # TR *= self.get_traffic_level()  # modify TR based on last minute of traffic data  # todo: look at getting this to work, a model could be used
 
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index b5ce9e1d2d96e2..ed4ba308c7d51a 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -82,8 +82,6 @@ def dynamic_gas(self, CP):
     x, y = [], []
     if CP.enableGasInterceptor:  # if pedal, todo: make different profiles for different vehicles
       if self.candidate in [CAR.COROLLA]:
-        with open('/data/long_corolla', 'a') as f:
-          f.write('we have liftoff\n')
         x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]
         y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617]  # todo: this is the average of the above, only above the 8th index (about .75 reduction)
     elif self.candidate in self.toyota_candidates:
@@ -95,9 +93,6 @@ def dynamic_gas(self, CP):
 
     gas = interp(self.v_ego, x, y)
 
-    with open('/data/lead_data', 'a') as f:
-      f.write(str(self.lead_data) + '\n')
-
     if self.lead_data['status']:  # if lead
       if self.v_ego <= 8.9408:  # if under 20 mph
         # TR = 1.8  # desired TR, might need to switch this to hardcoded distance values

From f4be7ea4c72de73acd616a631e6a58d1dd72212a Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 20:50:45 -0600
Subject: [PATCH 02/23] update dynamic gas tuning

---
 selfdrive/controls/controlsd.py       | 18 +++++----
 selfdrive/controls/lib/longcontrol.py | 58 ++++++++++++++++-----------
 2 files changed, 44 insertions(+), 32 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 6379dd768908f6..622b72051b9920 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -239,7 +239,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_
 
 
 def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
-                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control):
+                  AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm):
   """Given the state, this function returns an actuators packet"""
 
   actuators = car.CarControl.Actuators.new_message()
@@ -286,7 +286,13 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
 
   # Gas/Brake PID loop
-  passable_loc = {'lead_one': passable_state_control['lead_one'], 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead}
+  passable_loc = {}
+  if not travis:
+    passable_loc['lead_one'] = sm['radarState'].leadOne
+    # passable_loc['mpc_TR'] = sm['radarState'].mpcTR
+    # passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']}
+    passable_loc['has_lead'] = plan.hasLead
+    passable_loc['gas_pressed'] = CS.gasPressed
   actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
                                               v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable_loc)
   # Steering PID loop and lateral MPC
@@ -479,7 +485,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
 
   if sm is None:
     sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
-                              'model', 'gpsLocation', 'radarState'], ignore_alive=['gpsLocation'])
+                              'model', 'gpsLocation', 'radarState', 'liveTracks'], ignore_alive=['gpsLocation'])
 
 
   if can_sock is None:
@@ -597,13 +603,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
       prof.checkpoint("State transition")
 
     # Compute actuators (runs PID loops and lateral MPC)
-    if not travis:
-      passable_state_control = {'lead_one': sm['radarState'].leadOne}
-    else:
-      passable_state_control = {'lead_one': None}
     actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \
       state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
-                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control)
+                    driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm)
 
     prof.checkpoint("State Control")
 
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index ed4ba308c7d51a..21b3d78c67c02f 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -2,7 +2,9 @@
 from common.numpy_fast import clip, interp
 from selfdrive.controls.lib.pid import PIController
 from common.travis_checker import travis
-from selfdrive.car.toyota.values import CAR
+from selfdrive.car.toyota.values import CAR as CAR_TOYOTA
+from selfdrive.config import Conversions as CV
+from common.op_params import opParams
 
 LongCtrlState = log.ControlsState.LongControlState
 
@@ -67,23 +69,27 @@ def __init__(self, CP, compute_gb, candidate):
                             convert=compute_gb)
     self.v_pid = 0.0
     self.last_output_gb = 0.0
-    self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False}
-    self.v_ego = 0.0
-    self.gas_pressed = False
+    self.op_params = opParams()
+    self.dynamic_lane_speed_active = self.op_params.get('dynamic_lane_speed', default=True)
+    self.min_dynamic_lane_speed = self.op_params.get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS
     self.candidate = candidate
-    self.toyota_candidates = [attr for attr in dir(CAR) if not attr.startswith("__")]
+    self.toyota_candidates = [attr for attr in dir(CAR_TOYOTA) if not attr.startswith("__")]
+
+    self.gas_pressed = False
+    self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False}
+    self.track_data = []
+    self.mpc_TR = 1.8
 
   def reset(self, v_pid):
     """Reset PID controller and change setpoint"""
     self.pid.reset()
     self.v_pid = v_pid
 
-  def dynamic_gas(self, CP):
+  def dynamic_gas(self, v_ego, CP):
     x, y = [], []
-    if CP.enableGasInterceptor:  # if pedal, todo: make different profiles for different vehicles
-      if self.candidate in [CAR.COROLLA]:
-        x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]
-        y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617]  # todo: this is the average of the above, only above the 8th index (about .75 reduction)
+    if CP.enableGasInterceptor and (self.candidate in self.toyota_candidates):  # todo: make different profiles for different makes
+      x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]  # vels
+      y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617]  # max gas
     elif self.candidate in self.toyota_candidates:
       x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]
       y = [0.35, 0.47, 0.43, 0.35, 0.3, 0.3, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
@@ -91,20 +97,17 @@ def dynamic_gas(self, CP):
     if not x:
       x, y = CP.gasMaxBP, CP.gasMaxV  # if unsupported car, use stock. todo: think about disallowing dynamic follow for unsupported cars
 
-    gas = interp(self.v_ego, x, y)
+    gas = interp(v_ego, x, y)
 
     if self.lead_data['status']:  # if lead
-      if self.v_ego <= 8.9408:  # if under 20 mph
-        # TR = 1.8  # desired TR, might need to switch this to hardcoded distance values
-        # current_TR = self.lead_data['x_lead'] / self.v_ego if self.v_ego > 0 else TR
-
+      if v_ego <= 8.9408:  # if under 20 mph
         x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481]  # relative velocity mod
         y = [gas * 0.9901, gas * 0.905, gas * 0.8045, gas * 0.625, gas * 0.431, gas * 0.2083, gas * .0667, 0]
         gas_mod = -interp(self.lead_data['v_rel'], x, y)
 
-        # x = [0.0, 0.22, 0.44518483, 0.675, 1.0, 1.76361684]  # lead accel mod  # todo: this
-        # y = [0.0, (gas * 0.08), (gas * 0.20), (gas * 0.4), (gas * 0.52), (gas * 0.6)]
-        # gas_mod += interp(a_lead, x, y)
+        x = [0.44704, 1.78816]  # lead accel mod
+        y = [0.0, gas_mod * .6]
+        gas_mod -= interp(self.lead_data['a_lead'], x, y)  # as lead accelerates, we can reduce the reduction of the above mod (the max this will ouput is the original gas value, it never increases it)
 
         # x = [TR * 0.5, TR, TR * 1.5]  # as lead gets further from car, lessen gas mod  # todo: this
         # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5]
@@ -113,11 +116,18 @@ def dynamic_gas(self, CP):
 
         x = [1.78816, 6.0, 8.9408]  # slowly ramp mods down as we approach 20 mph
         y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas]
-        gas = interp(self.v_ego, x, y)
+        gas = interp(v_ego, x, y)
       else:
-        x = [-0.89408, 0, 2.0]  # need to tune this
-        y = [-.17, -.08, .01]
-        gas += interp(self.lead_data['v_rel'], x, y)
+        current_TR = self.lead_data['x_lead'] / v_ego
+        x = [-1.78816, -0.89408, 0, 1.78816, 2.68224]  # relative velocity mod
+        y = [-gas * 0.35, -gas * 0.25, -gas * 0.075, gas * 0.175, gas * 0.225]
+        gas_mod = interp(self.lead_data['v_rel'], x, y)
+
+        x_tr = [self.mpc_TR - 0.2, self.mpc_TR, self.mpc_TR + 0.2, self.mpc_TR + 0.4]
+        y_tr = [-gas_mod * 0.22, 0.0, gas_mod * 0.15, gas_mod * 0.45]
+        gas_mod -= interp(current_TR, x_tr, y_tr)
+
+        gas += gas_mod
 
     return clip(gas, 0.0, 1.0)
 
@@ -127,15 +137,15 @@ def handle_passable(self, passable):
     self.lead_data['a_lead'] = passable['lead_one'].aLeadK
     self.lead_data['x_lead'] = passable['lead_one'].dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
+    # self.mpc_TR = passable['mpc_TR']
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
-    self.v_ego = v_ego
 
     # Actuation limits
     if not travis:
       self.handle_passable(passable)
-      gas_max = self.dynamic_gas(CP)
+      gas_max = self.dynamic_gas(v_ego, CP)
     else:
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)

From a001a0e083a5d66a334ee010e7a4513f7d4c6687 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 20:53:26 -0600
Subject: [PATCH 03/23] add capnp message

---
 cereal/log.capnp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/cereal/log.capnp b/cereal/log.capnp
index b059218feba89c..e4dd2d0022dbee 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -372,6 +372,7 @@ struct RadarState @0x9a185389d6fdd05f {
   leadOne @3 :LeadData;
   leadTwo @4 :LeadData;
   cumLagMs @5 :Float32;
+  mpcTR @13 :Float32;
 
   struct LeadData {
     dRel @0 :Float32;

From 13da4b17364a08260f00923954484256503e130a Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 21:32:39 -0600
Subject: [PATCH 04/23] send TR if not travis

---
 selfdrive/controls/lib/long_mpc.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index e90064a2d50229..1e178bd94825e5 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -31,6 +31,7 @@ def __init__(self, mpc_id):
     self.new_lead = False
     self.last_cloudlog_t = 0.0
 
+    self.pm = messaging.PubMaster(['radarState'])
     self.car_data = {'v_ego': 0.0, 'a_ego': 0.0}
     self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
     self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
@@ -79,6 +80,10 @@ def get_TR(self, CS):
 
     if not travis:
       self.change_cost(TR)
+      dat = messaging.new_message()
+      dat.init('radarState')
+      dat.radarState.mpcTR = TR
+      self.pm.send('radarState', dat)
     return TR
 
   def change_cost(self, TR):

From 19221ffbbc0517d948906137aa0278ea6f6018c6 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 21:34:36 -0600
Subject: [PATCH 05/23] dynamic gas tuning

---
 selfdrive/controls/lib/longcontrol.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 21b3d78c67c02f..e7634e93587a73 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -112,7 +112,7 @@ def dynamic_gas(self, v_ego, CP):
         # x = [TR * 0.5, TR, TR * 1.5]  # as lead gets further from car, lessen gas mod  # todo: this
         # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5]
         # gas_mod += (interp(current_TR, x, y))
-        new_gas = gas + gas_mod  # (interp(current_TR, x, y))
+        new_gas = gas + gas_mod
 
         x = [1.78816, 6.0, 8.9408]  # slowly ramp mods down as we approach 20 mph
         y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas]
@@ -123,9 +123,9 @@ def dynamic_gas(self, v_ego, CP):
         y = [-gas * 0.35, -gas * 0.25, -gas * 0.075, gas * 0.175, gas * 0.225]
         gas_mod = interp(self.lead_data['v_rel'], x, y)
 
-        x_tr = [self.mpc_TR - 0.2, self.mpc_TR, self.mpc_TR + 0.2, self.mpc_TR + 0.4]
-        y_tr = [-gas_mod * 0.22, 0.0, gas_mod * 0.15, gas_mod * 0.45]
-        gas_mod -= interp(current_TR, x_tr, y_tr)
+        x = [self.mpc_TR - 0.22, self.mpc_TR, self.mpc_TR + 0.2, self.mpc_TR + 0.4]
+        y = [-gas_mod * 0.36, 0.0, gas_mod * 0.15, gas_mod * 0.45]
+        gas_mod -= interp(current_TR, x, y)
 
         gas += gas_mod
 

From 4501f2252714ae5dcc7462fac83de83c544ae91d Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 21:35:24 -0600
Subject: [PATCH 06/23] hope this works, receive mpc_tr

---
 selfdrive/controls/controlsd.py       | 2 +-
 selfdrive/controls/lib/longcontrol.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 622b72051b9920..a876509b489a69 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -289,7 +289,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   passable_loc = {}
   if not travis:
     passable_loc['lead_one'] = sm['radarState'].leadOne
-    # passable_loc['mpc_TR'] = sm['radarState'].mpcTR
+    passable_loc['mpc_TR'] = sm['radarState'].mpcTR
     # passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']}
     passable_loc['has_lead'] = plan.hasLead
     passable_loc['gas_pressed'] = CS.gasPressed
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index e7634e93587a73..c2f6e3fc4ba055 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -137,7 +137,7 @@ def handle_passable(self, passable):
     self.lead_data['a_lead'] = passable['lead_one'].aLeadK
     self.lead_data['x_lead'] = passable['lead_one'].dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
-    # self.mpc_TR = passable['mpc_TR']
+    self.mpc_TR = passable['mpc_TR']
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""

From 77134f1c1b7ecc7734e1a2cb40e336ee1705c777 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 22:07:41 -0600
Subject: [PATCH 07/23] remove

---
 selfdrive/controls/lib/long_mpc.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 1e178bd94825e5..b66581cbaf292b 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -31,7 +31,6 @@ def __init__(self, mpc_id):
     self.new_lead = False
     self.last_cloudlog_t = 0.0
 
-    self.pm = messaging.PubMaster(['radarState'])
     self.car_data = {'v_ego': 0.0, 'a_ego': 0.0}
     self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
     self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
@@ -80,10 +79,10 @@ def get_TR(self, CS):
 
     if not travis:
       self.change_cost(TR)
-      dat = messaging.new_message()
-      dat.init('radarState')
-      dat.radarState.mpcTR = TR
-      self.pm.send('radarState', dat)
+      # dat = messaging.new_message()
+      # dat.init('radarState')
+      # dat.radarState.mpcTR = TR
+      # self.pm.send('radarState', dat)
     return TR
 
   def change_cost(self, TR):

From d68ca048c6a02fa8e4f50f69d92032c0112aad75 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Sun, 22 Dec 2019 22:08:58 -0600
Subject: [PATCH 08/23] remove mpc_tr for now

---
 selfdrive/controls/controlsd.py       | 2 +-
 selfdrive/controls/lib/longcontrol.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index a876509b489a69..622b72051b9920 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -289,7 +289,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   passable_loc = {}
   if not travis:
     passable_loc['lead_one'] = sm['radarState'].leadOne
-    passable_loc['mpc_TR'] = sm['radarState'].mpcTR
+    # passable_loc['mpc_TR'] = sm['radarState'].mpcTR
     # passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']}
     passable_loc['has_lead'] = plan.hasLead
     passable_loc['gas_pressed'] = CS.gasPressed
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index c2f6e3fc4ba055..e7634e93587a73 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -137,7 +137,7 @@ def handle_passable(self, passable):
     self.lead_data['a_lead'] = passable['lead_one'].aLeadK
     self.lead_data['x_lead'] = passable['lead_one'].dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
-    self.mpc_TR = passable['mpc_TR']
+    # self.mpc_TR = passable['mpc_TR']
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""

From cea5ddda8b04b84acf9098e8900251c920c26160 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 00:09:38 -0600
Subject: [PATCH 09/23] pass mpc_tr

---
 cereal/log.capnp                      | 6 +++++-
 cereal/service_list.yaml              | 1 +
 selfdrive/controls/controlsd.py       | 2 +-
 selfdrive/controls/lib/long_mpc.py    | 9 +++++----
 selfdrive/controls/lib/longcontrol.py | 2 +-
 selfdrive/controls/lib/planner.py     | 2 +-
 6 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/cereal/log.capnp b/cereal/log.capnp
index e4dd2d0022dbee..dc2080bc5874df 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -372,7 +372,6 @@ struct RadarState @0x9a185389d6fdd05f {
   leadOne @3 :LeadData;
   leadTwo @4 :LeadData;
   cumLagMs @5 :Float32;
-  mpcTR @13 :Float32;
 
   struct LeadData {
     dRel @0 :Float32;
@@ -1815,6 +1814,10 @@ struct KalmanOdometry {
   rotStd @3 :List(Float32); # std rad/s in device frame
 }
 
+struct SmiskolData {
+  mpcTR @0 :Float32;
+}
+
 struct Event {
   # in nanoseconds?
   logMonoTime @0 :UInt64;
@@ -1890,5 +1893,6 @@ struct Event {
     carEvents @68: List(Car.CarEvent);
     carParams @69: Car.CarParams;
     frontFrame @70: FrameData;
+    smiskolData @71 :SmiskolData;
   }
 }
diff --git a/cereal/service_list.yaml b/cereal/service_list.yaml
index 884bfe9a5bf991..b09eb2b9fa7754 100644
--- a/cereal/service_list.yaml
+++ b/cereal/service_list.yaml
@@ -75,6 +75,7 @@ thumbnail: [8069, true, 0.2, 1]
 carEvents: [8070, true, 1., 1]
 carParams: [8071, true, 0.02, 1]
 frontFrame: [8072, true, 10.]
+smiskolData: [8073, true, 20.]
 
 testModel: [8040, false, 0.]
 testLiveLocation: [8045, false, 0.]
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 622b72051b9920..a876509b489a69 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -289,7 +289,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   passable_loc = {}
   if not travis:
     passable_loc['lead_one'] = sm['radarState'].leadOne
-    # passable_loc['mpc_TR'] = sm['radarState'].mpcTR
+    passable_loc['mpc_TR'] = sm['radarState'].mpcTR
     # passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']}
     passable_loc['has_lead'] = plan.hasLead
     passable_loc['gas_pressed'] = CS.gasPressed
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index b66581cbaf292b..666771343da508 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -31,6 +31,7 @@ def __init__(self, mpc_id):
     self.new_lead = False
     self.last_cloudlog_t = 0.0
 
+    self.pm = messaging.PubMaster(['smiskolData'])
     self.car_data = {'v_ego': 0.0, 'a_ego': 0.0}
     self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
     self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
@@ -79,10 +80,10 @@ def get_TR(self, CS):
 
     if not travis:
       self.change_cost(TR)
-      # dat = messaging.new_message()
-      # dat.init('radarState')
-      # dat.radarState.mpcTR = TR
-      # self.pm.send('radarState', dat)
+      dat = messaging.new_message()
+      dat.init('smiskolData')
+      dat.smiskolData.mpcTR = TR
+      self.pm.send('smiskolData', dat)
     return TR
 
   def change_cost(self, TR):
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index e7634e93587a73..c2f6e3fc4ba055 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -137,7 +137,7 @@ def handle_passable(self, passable):
     self.lead_data['a_lead'] = passable['lead_one'].aLeadK
     self.lead_data['x_lead'] = passable['lead_one'].dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
-    # self.mpc_TR = passable['mpc_TR']
+    self.mpc_TR = passable['mpc_TR']
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index e2e07409a03b3c..85de56035d73e6 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -24,7 +24,7 @@
 # lookup tables VS speed to determine min and max accels in cruise
 # make sure these accelerations are smaller than mpc limits
 _A_CRUISE_MIN_V  = [-1.0, -.8, -.67, -.5, -.30]
-_A_CRUISE_MIN_BP = [   0., 5.,  10., 20.,  40.]
+_A_CRUISE_MIN_BP = [  0.,  5.,  10., 20.,  40.]
 
 # need fast accel at very low speed for stop and go
 # make sure these accelerations are smaller than mpc limits

From 6c360cd28db57ed40e671ca21f1b483f0bcdba42 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 00:14:13 -0600
Subject: [PATCH 10/23] slightly farther TRs

---
 selfdrive/controls/lib/long_mpc.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 666771343da508..e68d0c03f79088 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -120,10 +120,12 @@ def lead_accel_over_time(self):
     return a_lead  # if above doesn't execute, we'll return a_lead from radar
 
   def dynamic_follow(self, CS):
+    # x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469]
+    # y_mod = [1.55742, 1.5842153, 1.6392148499999997, 1.68, 1.7325, 1.83645, 1.881]
     # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
     # y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
-    x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469]
-    y_mod = [1.55742, 1.5842153, 1.6392148499999997, 1.68, 1.7325, 1.83645, 1.881]
+    x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
+    y_mod = [1.146, 1.162, 1.18, 1.205, 1.243, 1.3, 1.378, 1.424, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
 
     sng_TR = 1.8  # stop and go parameters
     sng_speed = 15.0 * CV.MPH_TO_MS

From 552b5a808fa54465bf5bdbf65670e4f17ff85350 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 00:15:30 -0600
Subject: [PATCH 11/23] dynamic follow tuning

---
 selfdrive/controls/lib/long_mpc.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index e68d0c03f79088..567d6193a88276 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -138,13 +138,13 @@ def dynamic_follow(self, CS):
       TR = interp(self.car_data['v_ego'], x, y)
 
     # Dynamic follow modifications (the secret sauce)
-    x = [-20, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0, 1.0107, 1.89, 2.6909]  # relative velocity values
-    y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.082, -0.18, -0.28]  # modification values
+    x = [-20.0, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0.0, 1.3411, 1.89, 2.6909]  # relative velocity values
+    y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.06, -0.144, -0.224]  # modification values
     TR_mod = interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y)
 
-    x = [-4.4704, -1.77, -0.3145, 0, 0.446, 1.3411]  # lead acceleration values
-    y = [0.237, 0.12, 0.027, 0, -0.105, -0.195]  # modification values
-    TR_mod += interp(self.lead_accel_over_time(), x, y)  # todo: test if these modifications are too much
+    x = [-4.4704, -1.77, -0.3145, 0.0, 0.1495, 0.5104, 0.7037, 0.9357]  # lead acceleration values
+    y = [0.237, 0.12, 0.027, 0, -0.006, -0.036, -0.042, -0.045]  # modification values
+    TR_mod += interp(self.lead_accel_over_time(), x, y)
 
     TR += TR_mod
 

From 521e352f9b096e6a926539eccbc24ffbddbedbd4 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 00:40:53 -0600
Subject: [PATCH 12/23] fixes multi publishing error

---
 selfdrive/controls/controlsd.py       |  6 +--
 selfdrive/controls/lib/long_mpc.py    | 13 +++---
 selfdrive/controls/lib/longcontrol.py | 62 ++++++++++++++++++++++++---
 selfdrive/controls/lib/planner.py     |  5 ++-
 4 files changed, 68 insertions(+), 18 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index a876509b489a69..b28578ac78f396 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -289,8 +289,8 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
   passable_loc = {}
   if not travis:
     passable_loc['lead_one'] = sm['radarState'].leadOne
-    passable_loc['mpc_TR'] = sm['radarState'].mpcTR
-    # passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']}
+    passable_loc['mpc_TR'] = sm['smiskolData'].mpcTR
+    passable_loc['live_tracks'] = {'tracks': sm['liveTracks'], 'updated': sm.updated['liveTracks']}
     passable_loc['has_lead'] = plan.hasLead
     passable_loc['gas_pressed'] = CS.gasPressed
   actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
@@ -485,7 +485,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
 
   if sm is None:
     sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
-                              'model', 'gpsLocation', 'radarState', 'liveTracks'], ignore_alive=['gpsLocation'])
+                              'model', 'gpsLocation', 'radarState', 'liveTracks', 'smiskolData'], ignore_alive=['gpsLocation'])
 
 
   if can_sock is None:
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 567d6193a88276..2f97e0b325c091 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -17,7 +17,7 @@
 
 
 class LongitudinalMpc():
-  def __init__(self, mpc_id):
+  def __init__(self, mpc_id, pm):
     self.mpc_id = mpc_id
     self.op_params = opParams()
 
@@ -31,7 +31,7 @@ def __init__(self, mpc_id):
     self.new_lead = False
     self.last_cloudlog_t = 0.0
 
-    self.pm = messaging.PubMaster(['smiskolData'])
+    self.pm = pm
     self.car_data = {'v_ego': 0.0, 'a_ego': 0.0}
     self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
     self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
@@ -80,10 +80,11 @@ def get_TR(self, CS):
 
     if not travis:
       self.change_cost(TR)
-      dat = messaging.new_message()
-      dat.init('smiskolData')
-      dat.smiskolData.mpcTR = TR
-      self.pm.send('smiskolData', dat)
+      if self.mpc_id == 1:
+        dat = messaging.new_message()
+        dat.init('smiskolData')
+        dat.smiskolData.mpcTR = TR
+        self.pm.send('smiskolData', dat)
     return TR
 
   def change_cost(self, TR):
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index c2f6e3fc4ba055..69cb6b8591f558 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -5,6 +5,7 @@
 from selfdrive.car.toyota.values import CAR as CAR_TOYOTA
 from selfdrive.config import Conversions as CV
 from common.op_params import opParams
+import numpy as np
 
 LongCtrlState = log.ControlsState.LongControlState
 
@@ -69,6 +70,7 @@ def __init__(self, CP, compute_gb, candidate):
                             convert=compute_gb)
     self.v_pid = 0.0
     self.last_output_gb = 0.0
+
     self.op_params = opParams()
     self.dynamic_lane_speed_active = self.op_params.get('dynamic_lane_speed', default=True)
     self.min_dynamic_lane_speed = self.op_params.get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS
@@ -79,13 +81,14 @@ def __init__(self, CP, compute_gb, candidate):
     self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False}
     self.track_data = []
     self.mpc_TR = 1.8
+    self.v_ego = 0.0
 
   def reset(self, v_pid):
     """Reset PID controller and change setpoint"""
     self.pid.reset()
     self.v_pid = v_pid
 
-  def dynamic_gas(self, v_ego, CP):
+  def dynamic_gas(self, CP):
     x, y = [], []
     if CP.enableGasInterceptor and (self.candidate in self.toyota_candidates):  # todo: make different profiles for different makes
       x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]  # vels
@@ -97,10 +100,10 @@ def dynamic_gas(self, v_ego, CP):
     if not x:
       x, y = CP.gasMaxBP, CP.gasMaxV  # if unsupported car, use stock. todo: think about disallowing dynamic follow for unsupported cars
 
-    gas = interp(v_ego, x, y)
+    gas = interp(self.v_ego, x, y)
 
     if self.lead_data['status']:  # if lead
-      if v_ego <= 8.9408:  # if under 20 mph
+      if self.v_ego <= 8.9408:  # if under 20 mph
         x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481]  # relative velocity mod
         y = [gas * 0.9901, gas * 0.905, gas * 0.8045, gas * 0.625, gas * 0.431, gas * 0.2083, gas * .0667, 0]
         gas_mod = -interp(self.lead_data['v_rel'], x, y)
@@ -116,9 +119,9 @@ def dynamic_gas(self, v_ego, CP):
 
         x = [1.78816, 6.0, 8.9408]  # slowly ramp mods down as we approach 20 mph
         y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas]
-        gas = interp(v_ego, x, y)
+        gas = interp(self.v_ego, x, y)
       else:
-        current_TR = self.lead_data['x_lead'] / v_ego
+        current_TR = self.lead_data['x_lead'] / self.v_ego
         x = [-1.78816, -0.89408, 0, 1.78816, 2.68224]  # relative velocity mod
         y = [-gas * 0.35, -gas * 0.25, -gas * 0.075, gas * 0.175, gas * 0.225]
         gas_mod = interp(self.lead_data['v_rel'], x, y)
@@ -131,6 +134,12 @@ def dynamic_gas(self, v_ego, CP):
 
     return clip(gas, 0.0, 1.0)
 
+  def handle_live_tracks(self, live_tracks):
+    if live_tracks['updated']:
+      self.track_data = []
+      for track in live_tracks['tracks']:
+        self.track_data.append({'v_lead': self.v_ego + track.vRel, 'y_rel': track.yRel})
+
   def handle_passable(self, passable):
     self.gas_pressed = passable['gas_pressed']
     self.lead_data['v_rel'] = passable['lead_one'].vRel
@@ -138,14 +147,53 @@ def handle_passable(self, passable):
     self.lead_data['x_lead'] = passable['lead_one'].dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
     self.mpc_TR = passable['mpc_TR']
+    self.handle_live_tracks(passable['live_tracks'])
+
+  def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
+    v_cruise *= CV.KPH_TO_MS  # convert to m/s
+    min_tracks = 3
+    vels = [i * CV.MPH_TO_MS for i in [5, 40, 70]]
+    margins = [0.4, 0.55, 0.6]
+    track_speed_margin = interp(self.v_ego, vels, margins)
+    MPC_TIME_STEP = 1 / 20.
+    similar_track_tolerance = 0.022
+    if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:
+      tracks = []
+      for trk_vel in self.track_data:
+        valid = all([True if abs(x - trk_vel) >= similar_track_tolerance else False for x in tracks])  # radar sometimes reports multiple points for one vehicle, especially semis
+        # todo: factor in yRel, so multiple vehicles that happen to be at the exact same speed aren't filtered out!
+        if valid:
+          tracks.append(trk_vel)
+      tracks = [trk_vel for trk_vel in tracks if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]  # .125, 0.025, 0.02500009536743164, 0.02500009536743164
+      if len(tracks) >= min_tracks:
+        average_track_speed = np.mean(tracks)
+        if average_track_speed < v_target and average_track_speed < v_target_future:
+          # so basically, if there's at least 3 tracks, the speeds of the tracks must be within n% of set speed, if our speed is at least set_speed mph,
+          # if the average speeds of tracks is less than v_target and v_target_future, then get a weight for how many tracks exist, with more tracks, the more we
+          # favor the average track speed, then weighted average it with our set_speed, if these conditions aren't met, then we just return original values
+          # this should work...?
+          x = [3, 6, 19]
+          y = [0.275, .375, 0.5]
+          track_speed_weight = interp(len(tracks), x, y)
+          if self.lead_data['status']:  # if lead, give more weight to surrounding tracks (todo: this if check might need to be flipped, so if not lead...)
+            track_speed_weight = clip(1.05 * track_speed_weight, min(y), max(y))
+          v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight)
+          if v_target_slow < v_target and v_target_slow < v_target_future:  # just a sanity check, don't want to run into any leads if we somehow predict faster velocity
+            a_target_slow = MPC_TIME_STEP * ((v_target_slow - v_target) / 1.0)  # long_mpc runs at 20 hz, so interpolate assuming a_target is 1 second into future? or since long_control is 100hz, should we interpolate using that?
+            a_target = a_target_slow
+            v_target = v_target_slow
+            v_target_future = v_target_slow
+
+    return v_target, v_target_future, a_target
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable):
     """Update longitudinal control. This updates the state machine and runs a PID loop"""
-
+    self.v_ego = v_ego
     # Actuation limits
     if not travis:
       self.handle_passable(passable)
-      gas_max = self.dynamic_gas(v_ego, CP)
+      gas_max = self.dynamic_gas(CP)
+      # v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
     else:
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 85de56035d73e6..9e99ce77e934ff 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -65,8 +65,9 @@ class Planner():
   def __init__(self, CP):
     self.CP = CP
 
-    self.mpc1 = LongitudinalMpc(1)
-    self.mpc2 = LongitudinalMpc(2)
+    pm = messaging.PubMaster(['smiskolData'])
+    self.mpc1 = LongitudinalMpc(1, pm)
+    self.mpc2 = LongitudinalMpc(2, pm)
 
     self.v_acc_start = 0.0
     self.a_acc_start = 0.0

From 16e7a8552acefbaa62a7ccb25a513fa78feb5f86 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 01:28:33 -0600
Subject: [PATCH 13/23] set pm instead

---
 selfdrive/controls/lib/long_mpc.py    | 7 +++++--
 selfdrive/controls/lib/longcontrol.py | 2 +-
 selfdrive/controls/lib/planner.py     | 6 ++++--
 3 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 2f97e0b325c091..58bf6f9c7ebbf1 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -17,7 +17,7 @@
 
 
 class LongitudinalMpc():
-  def __init__(self, mpc_id, pm):
+  def __init__(self, mpc_id):
     self.mpc_id = mpc_id
     self.op_params = opParams()
 
@@ -31,13 +31,16 @@ def __init__(self, mpc_id, pm):
     self.new_lead = False
     self.last_cloudlog_t = 0.0
 
-    self.pm = pm
+    self.pm = None
     self.car_data = {'v_ego': 0.0, 'a_ego': 0.0}
     self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False}
     self.df_data = {"v_leads": [], "v_egos": []}  # dynamic follow data
     self.last_cost = 0.0
     self.customTR = self.op_params.get('following_distance', None)
 
+  def set_pm(self, pm):
+    self.pm = pm
+
   def send_mpc_solution(self, pm, qp_iterations, calculation_time):
     qp_iterations = max(0, qp_iterations)
     dat = messaging.new_message()
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 69cb6b8591f558..222e6fa74feef4 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -193,7 +193,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     if not travis:
       self.handle_passable(passable)
       gas_max = self.dynamic_gas(CP)
-      # v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
+      v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
     else:
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 9e99ce77e934ff..94c34238034c17 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -66,8 +66,10 @@ def __init__(self, CP):
     self.CP = CP
 
     pm = messaging.PubMaster(['smiskolData'])
-    self.mpc1 = LongitudinalMpc(1, pm)
-    self.mpc2 = LongitudinalMpc(2, pm)
+    self.mpc1 = LongitudinalMpc(1)
+    self.mpc2 = LongitudinalMpc(2)
+    if not travis:
+      self.mpc1.set_pm(pm)
 
     self.v_acc_start = 0.0
     self.a_acc_start = 0.0

From 9bd8ecb2c56aca5dbf013e1d9c805180389baaf0 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 01:44:12 -0600
Subject: [PATCH 14/23] debug

---
 selfdrive/controls/lib/longcontrol.py | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 222e6fa74feef4..1501de3c2e0e19 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -156,15 +156,16 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
     margins = [0.4, 0.55, 0.6]
     track_speed_margin = interp(self.v_ego, vels, margins)
     MPC_TIME_STEP = 1 / 20.
-    similar_track_tolerance = 0.022
+    track_tolerance_v = 0.0268
+    track_tolerance_y = 1.8288
     if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:
       tracks = []
-      for trk_vel in self.track_data:
-        valid = all([True if abs(x - trk_vel) >= similar_track_tolerance else False for x in tracks])  # radar sometimes reports multiple points for one vehicle, especially semis
-        # todo: factor in yRel, so multiple vehicles that happen to be at the exact same speed aren't filtered out!
-        if valid:
-          tracks.append(trk_vel)
-      tracks = [trk_vel for trk_vel in tracks if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]  # .125, 0.025, 0.02500009536743164, 0.02500009536743164
+      for track in self.track_data:
+        valid = all([True if abs(trk['v_lead'] - track['v_lead']) >= track_tolerance_v else False for trk in tracks])  # radar sometimes reports multiple points for one vehicle, especially semis
+        valid_y = all([True if abs(trk['y_rel'] - track['y_rel']) >= track_tolerance_y else False for trk in tracks])
+        if valid or valid_y:
+          tracks.append(track)
+      tracks = [trk['v_lead'] for trk in tracks if (self.v_ego * track_speed_margin) <= trk['v_lead'] <= v_cruise]  # .125, 0.025, 0.02500009536743164, 0.02500009536743164
       if len(tracks) >= min_tracks:
         average_track_speed = np.mean(tracks)
         if average_track_speed < v_target and average_track_speed < v_target_future:
@@ -193,7 +194,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     if not travis:
       self.handle_passable(passable)
       gas_max = self.dynamic_gas(CP)
-      v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
+      # v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
     else:
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)

From 9c3159e5ec2557b3a1b36138fc83476a60f96496 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 01:59:53 -0600
Subject: [PATCH 15/23] enable dynamic lane speed, but averaging with v_ego
 instead of v_cruise. need to test both

---
 selfdrive/controls/lib/longcontrol.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 1501de3c2e0e19..3565f3e5b53495 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -174,11 +174,11 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
           # favor the average track speed, then weighted average it with our set_speed, if these conditions aren't met, then we just return original values
           # this should work...?
           x = [3, 6, 19]
-          y = [0.275, .375, 0.5]
+          y = [0.3, .4, 0.6]
           track_speed_weight = interp(len(tracks), x, y)
           if self.lead_data['status']:  # if lead, give more weight to surrounding tracks (todo: this if check might need to be flipped, so if not lead...)
             track_speed_weight = clip(1.05 * track_speed_weight, min(y), max(y))
-          v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight)
+          v_target_slow = (self.v_ego * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight)
           if v_target_slow < v_target and v_target_slow < v_target_future:  # just a sanity check, don't want to run into any leads if we somehow predict faster velocity
             a_target_slow = MPC_TIME_STEP * ((v_target_slow - v_target) / 1.0)  # long_mpc runs at 20 hz, so interpolate assuming a_target is 1 second into future? or since long_control is 100hz, should we interpolate using that?
             a_target = a_target_slow
@@ -194,7 +194,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     if not travis:
       self.handle_passable(passable)
       gas_max = self.dynamic_gas(CP)
-      # v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
+      v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
     else:
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)

From 5e1b179ae048fbb4e0f6a9c336f2afc5f9d61d1a Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 20:00:35 -0600
Subject: [PATCH 16/23] dynamic lane speed tuning

---
 selfdrive/controls/lib/longcontrol.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 3565f3e5b53495..6f43ff9940d772 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -174,11 +174,11 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
           # favor the average track speed, then weighted average it with our set_speed, if these conditions aren't met, then we just return original values
           # this should work...?
           x = [3, 6, 19]
-          y = [0.3, .4, 0.6]
+          y = [0.275, .375, 0.5]
           track_speed_weight = interp(len(tracks), x, y)
           if self.lead_data['status']:  # if lead, give more weight to surrounding tracks (todo: this if check might need to be flipped, so if not lead...)
             track_speed_weight = clip(1.05 * track_speed_weight, min(y), max(y))
-          v_target_slow = (self.v_ego * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight)
+          v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight)
           if v_target_slow < v_target and v_target_slow < v_target_future:  # just a sanity check, don't want to run into any leads if we somehow predict faster velocity
             a_target_slow = MPC_TIME_STEP * ((v_target_slow - v_target) / 1.0)  # long_mpc runs at 20 hz, so interpolate assuming a_target is 1 second into future? or since long_control is 100hz, should we interpolate using that?
             a_target = a_target_slow

From 48a25bbd90b620255c0a5b581d137a678598657d Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Mon, 23 Dec 2019 22:57:43 -0600
Subject: [PATCH 17/23] add option to disable steering ratio learner and just
 use static value from interface.py

---
 common/op_params.py                   |  3 ++-
 selfdrive/controls/lib/pathplanner.py | 11 ++++++++++-
 2 files changed, 12 insertions(+), 2 deletions(-)

diff --git a/common/op_params.py b/common/op_params.py
index 4f2dab81db155b..e56107cb7ce01f 100644
--- a/common/op_params.py
+++ b/common/op_params.py
@@ -37,7 +37,8 @@ def __init__(self):
                            'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR'},
                            'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. '
                                                                                                              'If False, lane change will occur IMMEDIATELY after signaling')},
-                           'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}}
+                           'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'},
+                           'use_static_steering_ratio': {'default': False, 'allowed_types': [bool], 'description': 'Whether you want openpilot to use the automatically learned steering ratio. If True, it will use the value in interface.py instead'}}
 
     self.params = {}
     self.params_file = "/data/op_params.json"
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 74f68454fcc4a0..aa50c990d85f0c 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -10,6 +10,7 @@
 from cereal import log
 from common.op_params import opParams
 from selfdrive.controls.lane_hugging import LaneHugging
+from common.travis_checker import travis
 
 LaneChangeState = log.PathPlan.LaneChangeState
 LaneChangeDirection = log.PathPlan.LaneChangeDirection
@@ -49,6 +50,7 @@ def __init__(self, CP):
 
     self.last_cloudlog_t = 0
     self.steer_rate_cost = CP.steerRateCost
+    self.steer_ratio = CP.steerRatio
 
     self.setup_mpc()
     self.solution_invalid_cnt = 0
@@ -59,6 +61,7 @@ def __init__(self, CP):
     self.op_params = opParams()
     self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True)
     self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0)
+    self.use_static_steering_ratio = self.op_params.get('use_static_steering_ratio', default=False)
     self.lane_hugging = LaneHugging()
 
   def setup_mpc(self):
@@ -77,6 +80,11 @@ def setup_mpc(self):
     self.angle_steers_des_prev = 0.0
     self.angle_steers_des_time = 0.0
 
+  def getSteerRatio(self, sR):
+    if not travis and not self.use_static_steering_ratio:
+      return self.steer_ratio
+    return sR
+
   def update(self, sm, pm, CP, VM):
     v_ego = sm['carState'].vEgo
     angle_steers = sm['carState'].steeringAngle
@@ -166,7 +174,8 @@ def update(self, sm, pm, CP, VM):
 
     # account for actuation delay
     angle_offset = self.lane_hugging.modify_offset(float(sm['liveParameters'].angleOffset), lane_change_direction, self.lane_change_state)
-    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay)
+    sR = self.getSteerRatio(VM.sR)
+    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, sR, CP.steerActuatorDelay)
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,

From b148a9361138448c231dbe9845dd5db427c68c24 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Tue, 24 Dec 2019 15:07:47 -0600
Subject: [PATCH 18/23] dont take y into consideration

---
 selfdrive/controls/lib/longcontrol.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 6f43ff9940d772..b73aa531e55d29 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -162,8 +162,8 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
       tracks = []
       for track in self.track_data:
         valid = all([True if abs(trk['v_lead'] - track['v_lead']) >= track_tolerance_v else False for trk in tracks])  # radar sometimes reports multiple points for one vehicle, especially semis
-        valid_y = all([True if abs(trk['y_rel'] - track['y_rel']) >= track_tolerance_y else False for trk in tracks])
-        if valid or valid_y:
+        # valid_y = all([True if abs(trk['y_rel'] - track['y_rel']) >= track_tolerance_y else False for trk in tracks])
+        if valid:  # or valid_y:
           tracks.append(track)
       tracks = [trk['v_lead'] for trk in tracks if (self.v_ego * track_speed_margin) <= trk['v_lead'] <= v_cruise]  # .125, 0.025, 0.02500009536743164, 0.02500009536743164
       if len(tracks) >= min_tracks:

From bad9ed9f5d312533c9c4f9bb4acebddc683b49ac Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Tue, 24 Dec 2019 15:09:01 -0600
Subject: [PATCH 19/23] smaller tolerance

---
 selfdrive/controls/lib/longcontrol.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index b73aa531e55d29..5b03b5aae1cce1 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -156,7 +156,7 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
     margins = [0.4, 0.55, 0.6]
     track_speed_margin = interp(self.v_ego, vels, margins)
     MPC_TIME_STEP = 1 / 20.
-    track_tolerance_v = 0.0268
+    track_tolerance_v = 0.022352
     track_tolerance_y = 1.8288
     if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:
       tracks = []

From aec3b9718a5dfc24a05d4d5cb73af2d41d836b0c Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Tue, 24 Dec 2019 15:09:57 -0600
Subject: [PATCH 20/23] tune corolla sR

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 94b73917f55d93..88524862bac4cd 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -121,7 +121,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       stop_and_go = False
       ret.safetyParam = 100
       ret.wheelbase = 2.70
-      ret.steerRatio = 18.27
+      ret.steerRatio = 17.8
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]

From 3e2a714488ed129bd2d58ac2a31d06428c508e99 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 25 Dec 2019 00:05:07 -0600
Subject: [PATCH 21/23] that tune was horrible

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 88524862bac4cd..55d3bf4eb7804f 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -128,7 +128,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       ret.lateralTuning.pid.kf = 0.00003 * 0.639  # full torque for 20 deg at 80mph means 0.00007818594
       if ret.enableGasInterceptor:
         ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42]
-        # ret.longitudinalTuning.kiV = [0.135, 0.09]
+        ret.longitudinalTuning.kiV = [0.135, 0.09]
 
     elif candidate == CAR.LEXUS_RXH:
       stop_and_go = True

From ea132b95af0508092d09a3f0fab54e8a57887b69 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Wed, 25 Dec 2019 20:15:45 -0600
Subject: [PATCH 22/23] remove custom pedal tuning

---
 selfdrive/car/toyota/interface.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 55d3bf4eb7804f..73f3857d133c31 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -126,9 +126,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
       ret.lateralTuning.pid.kf = 0.00003 * 0.639  # full torque for 20 deg at 80mph means 0.00007818594
-      if ret.enableGasInterceptor:
-        ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42]
-        ret.longitudinalTuning.kiV = [0.135, 0.09]
+#       if ret.enableGasInterceptor:
+#         ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42]
+#         ret.longitudinalTuning.kiV = [0.135, 0.09]
 
     elif candidate == CAR.LEXUS_RXH:
       stop_and_go = True

From 3fb6f89aa97e969cbd1c03b4d470d64a8ad3a767 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Wed, 25 Dec 2019 20:18:12 -0600
Subject: [PATCH 23/23] debug overshoot

---
 selfdrive/controls/lib/long_mpc.py | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 58bf6f9c7ebbf1..71a68a3fe6898f 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -124,12 +124,12 @@ def lead_accel_over_time(self):
     return a_lead  # if above doesn't execute, we'll return a_lead from radar
 
   def dynamic_follow(self, CS):
-    # x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469]
-    # y_mod = [1.55742, 1.5842153, 1.6392148499999997, 1.68, 1.7325, 1.83645, 1.881]
+    x_vel = [0.0, 5.222, 11.164, 14.937, 20.973, 33.975, 42.469]
+    y_mod = [1.55742, 1.5842153, 1.6392148499999997, 1.68, 1.7325, 1.83645, 1.881]
     # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
     # y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
-    x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
-    y_mod = [1.146, 1.162, 1.18, 1.205, 1.243, 1.3, 1.378, 1.424, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
+    # x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336]  # velocities
+    # y_mod = [1.146, 1.162, 1.18, 1.205, 1.243, 1.3, 1.378, 1.424, 1.543, 1.62, 1.664, 1.736, 1.853]  # TRs
 
     sng_TR = 1.8  # stop and go parameters
     sng_speed = 15.0 * CV.MPH_TO_MS
@@ -189,6 +189,8 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
 
     # Setup current mpc state
     self.cur_state[0].x_ego = 0.0
+    with open("/data/set_speed", "a") as f:
+      f.write("{}".format([v_ego, CS.cruiseState.speed]))
 
     if lead is not None and lead.status:
       x_lead = lead.dRel