From ce1ec4e21841ab669a7e2808f93415367cb23254 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 02:51:21 -0600
Subject: [PATCH 01/39] add dynamic gas for non-pedal cars, up acceleration
 limits

---
 README.md                             | 35 +++------------------------
 selfdrive/controls/controlsd.py       | 10 ++++----
 selfdrive/controls/lib/longcontrol.py | 16 ++++++------
 selfdrive/controls/lib/planner.py     |  2 +-
 4 files changed, 18 insertions(+), 45 deletions(-)

diff --git a/README.md b/README.md
index c48d163620bcfa..5cccdf2089cae3 100644
--- a/README.md
+++ b/README.md
@@ -1,35 +1,6 @@
-Shane's Stock Additions (0.7)
+Dynamic Lane Speed (0.7)
 =====
 
-This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla.
+This is an experimental branch exploring a possibility of an adaptive/dynamic set speed that will reduce your speed when cars in other lanes are going significantly slower than you, if you don't currently have a lead to slow you down
 
-
-Highlight Features
-====
-
-1. **(NOT YET ADDED) Dynamic gas**: This aims to provide a smoother driving experience in stop and go traffic by modifying the maximum gas that can be applied based on your current velocity and the relative velocity of the lead car. It'll also of course increase the maximum gas when the lead is accelerating to help you get up to speed quicker than stock. And smoother; this eliminates the jerking you get from stock openpilot with comma pedal. Better tuning will be next.
-2. **Dynamic follow**: This is my dynamic follow from 0.5, where it changes your TR (following distance) dynamically based on multiple vehicle factors, as well as data from the lead vehicle. [Here's an old write up from a while ago explaining how it works exactly. Some of it might be out of date, but how it functions is the same.](https://github.com/ShaneSmiskol/openpilot/blob/dynamic-follow/README.md)
-3. **(NOT YET ADDED) Two PID loops to control gas and brakes independently (new!)**: If you have a Toyota Corolla with a comma pedal, you'll love this addition. Two longitudinal PID loops are set up in `longcontrol.py` so that one is running with comma pedal tuning to control the gas, and the other is running stock non-pedal tuning for better braking control. In the car, this feels miles better than stock openpilot, and nearly as good as your stock Toyota cruise control before you pulled out your DSU! It won't accelerate up to stopped cars and brake at the last moment anymore.
-3. **Custom wheel offset to reduce lane hugging**: Stock openpilot doesn't seem to be able to identify your car's true angle offset. With the `LaneHugging` module you can specify a custom angle offset to be added to your desired steering angle. Simply find the angle your wheel is at when you're driving on a straight highway. By default, this is disabled, to enable you can:
-    - Use the `opEdit` class in the root directory of openpilot. To use it, simply open an `ssh` shell and enter the commands below:
-        ```python
-        cd /data/openpilot
-        python op_edit.py
-        ```
-        You'll be greeted with a list of your parameters you can explore, enter the number corresponding to `lane_hug_direction`. Your options are to enter `'left'` or `'right'` for whichever direction your car has a tendency to hug toward. `None` will disable the feature.
-        Finally you'll need to enter your absolute angle offset (negative will be converted to positive) with the `opParams` parameter: `lane_hug_angle_offset`.
-4. **Custom following distance**: Using the `following_distance` parameter in `opParams`, you can specify a custom TR value to always be used. Afraid of technology and want to give yourself the highest following distance out there? Try out 2.7s! Are you daredevil and don't care about pissing off the car you're tailgating ahead? Try 0.9s!
-    - Again, you can use `opEdit` to change this:
-        ```python
-        cd /data/openpilot
-        python op_edit.py
-        ```
-        Then enter the number for the `following_distance` parameter and set to a float or integer between `0.9` and `2.7`. `None` will use dynamic follow!
-5. **Customize this branch (opEdit Parameter class)**: This is a handy tool to change your `opParams` parameters without diving into any json files or code. You can specify parameters to be used in any fork's operation that supports `opParams`. First, ssh in to your EON and make sure you're in `/data/openpilot`, then start `opEdit`:
-    ```python
-    cd /data/openpilot
-    python op_edit.py
-    ```
-   A list of parameters that you can change are located [here](https://github.com/ShaneSmiskol/openpilot/blob/stock_additions-07/common/op_params.py#L29).
-
-    Parameters are stored at `/data/op_params.json`
\ No newline at end of file
+> What if we got all the radar points (up to 16 for Toyota I believe), only cared about the ones that are within a margin of our speed and if the average is lower than us by another margin, reduce speed to the average of the cars around us? Or maybe the average of the... average and our initial set speed
\ No newline at end of file
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 5423ec500d90d8..2d76ca59e631d1 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, radar_state):
   """Given the state, this function returns an actuators packet"""
 
   actuators = car.CarControl.Actuators.new_message()
@@ -286,7 +286,7 @@ 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 = {'radar_state': radar_state, 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead}
   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
@@ -594,12 +594,12 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
 
     # Compute actuators (runs PID loops and lateral MPC)
     if not travis:
-      passable_state_control = {'lead_one': sm['radarState'].leadOne}
+      radar_state = sm['radarState']
     else:
-      passable_state_control = {'lead_one': None}
+      radar_state = 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, radar_state)
 
     prof.checkpoint("State Control")
 
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 88b647e1e3968b..b5dbf9b30087f7 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -76,13 +76,15 @@ def reset(self, v_pid):
     self.v_pid = v_pid
 
   def dynamic_gas(self, CP):
-    if True:  # CP.enableGasInterceptor:  # if user has pedal (probably Toyota) #todo: make different profiles for different toyotas, and check if toyota or not
+    if CP.enableGasInterceptor:  # if user has pedal (probably Toyota) #todo: make different profiles for different toyotas, and check if toyota or not
       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.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7]
       # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475]  # todo: elvaluate if this is better
       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)
-    # else:
-    #   x, y = CP.gasMaxBP, CP.gasMaxV  # if user doesn't have pedal, use stock params
+    else:
+      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]
+      # x, y = CP.gasMaxBP, CP.gasMaxV  # if user doesn't have pedal, use stock params
 
     gas = interp(self.v_ego, x, y)
 
@@ -119,9 +121,9 @@ def dynamic_gas(self, CP):
 
   def handle_passable(self, passable):
     self.gas_pressed = passable['gas_pressed']
-    self.lead_data['v_rel'] = passable['lead_one'].vRel
-    self.lead_data['a_lead'] = passable['lead_one'].aLeadK
-    self.lead_data['x_lead'] = passable['lead_one'].dRel
+    self.lead_data['v_rel'] = passable['radar_state'].leadOne.vRel
+    self.lead_data['a_lead'] = passable['radar_state'].leadOne.aLeadK
+    self.lead_data['x_lead'] = passable['radar_state'].leadOne.dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
 
   def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable):
@@ -129,7 +131,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     self.v_ego = v_ego
 
     # Actuation limits
-    if not travis and CP.enableGasInterceptor:
+    if not travis:
       self.handle_passable(passable)  # so travis doesn't call vRel... on None
       gas_max = self.dynamic_gas(CP)
     else:
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index 6e3b45a813d2bb..dcc3b02ee29a9d 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -28,7 +28,7 @@
 
 # need fast accel at very low speed for stop and go
 # make sure these accelerations are smaller than mpc limits
-_A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4]
+_A_CRUISE_MAX_V = [2.0, 1.9, 0.8, .4]
 _A_CRUISE_MAX_BP = [0.,  6.4, 22.5, 40.]
 
 # Lookup table for turns

From c98ff59cd1a10a7d21f5b33bc2a9ea79c3d358a3 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 03:00:17 -0600
Subject: [PATCH 02/39] enable dynamic follow

---
 selfdrive/controls/lib/long_mpc.py    | 2 +-
 selfdrive/controls/lib/longcontrol.py | 9 +++++++++
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 9f0fdd0a615abb..70efbf2f18e0d5 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -212,7 +212,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
 
     # Calculate mpc
     t = sec_since_boot()
-    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, 1.8)
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR())
     duration = int((sec_since_boot() - t) * 1e9)
 
     if not travis:
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index b5dbf9b30087f7..2e53c36abd1eef 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -126,6 +126,12 @@ def handle_passable(self, passable):
     self.lead_data['x_lead'] = passable['radar_state'].leadOne.dRel
     self.lead_data['status'] = passable['has_lead']  # this fixes radarstate always reporting a lead, thanks to arne
 
+  def dynamic_lane_speed(self, v_target, v_target_future):
+    pass
+
+
+    return 0, 0
+
   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
@@ -138,6 +144,9 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
 
+    if not travis:
+      v_target, v_target_future = self.dynamic_lane_speed(v_target, v_target_future)
+
     # Update state machine
     output_gb = self.last_output_gb
     self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,

From 3e68542f12b692dea3e90d8b3fc92759371089ba Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 03:47:53 -0600
Subject: [PATCH 03/39] add dynamic lane speed

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

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 2d76ca59e631d1..207f8380a8fe9f 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -75,8 +75,6 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa
   can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
   CS = CI.update(CC, can_strs)
 
-  sm.update(0)
-
   events = list(CS.events)
   add_lane_change_event(events, sm['pathPlan'])
   enabled = isEnabled(state)
@@ -239,7 +237,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, radar_state):
+                  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 +284,14 @@ 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 = {'radar_state': radar_state, 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead}
+  if not travis:
+    if sm.updated['liveTracks']:
+      live_tracks = sm['liveTracks']
+    else:
+      live_tracks = None
+    passable_loc = {'lead_one': sm['radarState'].leadOne, 'live_tracks': live_tracks, 'has_lead': plan.hasLead, 'gas_pressed': CS.gasPressed}
+  else:
+    passable_loc = None
   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 +484,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:
@@ -551,6 +556,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
   prof = Profiler(False)  # off by default
 
   while True:
+    sm.update(0)
     start_time = sec_since_boot()
     prof.checkpoint("Ratekeeper", ignore=True)
 
@@ -593,13 +599,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:
-      radar_state = sm['radarState']
-    else:
-      radar_state = 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, radar_state)
+                    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 2e53c36abd1eef..f44c3274bc1f60 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -2,6 +2,8 @@
 from common.numpy_fast import clip, interp
 from selfdrive.controls.lib.pid import PIController
 from common.travis_checker import travis
+from selfdrive.config import Conversions as CV
+import numpy as np
 
 LongCtrlState = log.ControlsState.LongControlState
 
@@ -69,6 +71,7 @@ def __init__(self, CP, compute_gb):
     self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False}
     self.v_ego = 0.0
     self.gas_pressed = False
+    self.track_data = []
 
   def reset(self, v_pid):
     """Reset PID controller and change setpoint"""
@@ -119,22 +122,46 @@ def dynamic_gas(self, CP):
 
     return clip(gas, 0.0, 1.0)
 
+  def handle_live_tracks(self, tracks):
+    if tracks is not None:  # if updated
+      self.track_data = []
+      for track in tracks:
+        self.track_data.append(self.v_ego + track.vRel)
+
   def handle_passable(self, passable):
-    self.gas_pressed = passable['gas_pressed']
-    self.lead_data['v_rel'] = passable['radar_state'].leadOne.vRel
-    self.lead_data['a_lead'] = passable['radar_state'].leadOne.aLeadK
-    self.lead_data['x_lead'] = passable['radar_state'].leadOne.dRel
+    self.lead_data['v_rel'] = passable['lead_one'].vRel
+    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
-
-  def dynamic_lane_speed(self, v_target, v_target_future):
-    pass
-
-
-    return 0, 0
+    self.gas_pressed = passable['gas_pressed']
+    self.handle_live_tracks(passable['live_tracks'])
+
+  def dynamic_lane_speed(self, v_target, v_target_future, v_cruise):
+    min_tracks = 3
+    track_speed_margin = .55  # 55 percent
+    min_speed = 30
+    self.track_data = [i for i in self.track_data if (v_cruise * track_speed_margin) < i]
+    if len(self.track_data) >= min_tracks and self.v_ego > (min_speed * CV.MPH_TO_MS):
+      average_track_speed = np.mean(self.track_data)
+      if average_track_speed < v_target and average_track_speed < v_target_future:
+        # so basically, if there's no lead, there's at least 3 tracks, the speeds of the tracks must be within 50% of set speed, if our speed is at least 30 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, 8, 16]
+        y = [0.3, .55, 0.7]
+        track_speed_weight = interp(len(self.track_data), x, y)
+        v_target_slow = (v_cruise * (1 - track_speed_weight)) + (average_track_speed * track_speed_weight)
+        if v_target_slow < v_target:  # just a sanity check
+          v_target = v_target_slow
+          v_target_future = v_target_slow
+
+    return v_target, v_target_future
 
   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
+    v_cruise *= CV.KPH_TO_MS  # convert to m/s
 
     # Actuation limits
     if not travis:
@@ -144,8 +171,8 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
 
-    if not travis:
-      v_target, v_target_future = self.dynamic_lane_speed(v_target, v_target_future)
+    if not travis and not self.lead_data['status']:  # want to make sure we don't mess with anything when there's a lead
+      v_target, v_target_future = self.dynamic_lane_speed(v_target, v_target_future, v_cruise)
 
     # Update state machine
     output_gb = self.last_output_gb

From 49a6236c1d24c2a3059a555c9dbcce5075af5d13 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 03:51:06 -0600
Subject: [PATCH 04/39] debugging

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

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index f44c3274bc1f60..23c856ad7d9d1d 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -137,6 +137,8 @@ def handle_passable(self, passable):
     self.handle_live_tracks(passable['live_tracks'])
 
   def dynamic_lane_speed(self, v_target, v_target_future, v_cruise):
+    with open('/data/live_tracks_test', 'a') as f:
+      f.write('{}\n'.format(self.track_data))
     min_tracks = 3
     track_speed_margin = .55  # 55 percent
     min_speed = 30

From 9839fed88d1c55f712701a9b8efa9bf310a6edbe Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 04:01:11 -0600
Subject: [PATCH 05/39] todo

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 70efbf2f18e0d5..2f64a15f323377 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -144,8 +144,8 @@ def dynamic_follow(self):
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
       y = [1.0, .7, .65]  # 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([self.CS.leftBlinker, self.CS.rightBlinker, self.car_data['v_ego'], old_TR, TR]))
+      # with open('/data/blinker_debug', 'a') as f:
+      #   f.write('{}\n'.format([self.CS.leftBlinker, self.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
 
@@ -215,9 +215,9 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
     n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR())
     duration = int((sec_since_boot() - t) * 1e9)
 
-    if not travis:
-      with open('/data/mpc_debug.{}'.format(self.mpc_id), 'a') as f:
-        f.write('{}\n'.format(self.lead_data))
+    # if not travis:
+    #   with open('/data/mpc_debug.{}'.format(self.mpc_id), 'a') as f:
+    #     f.write('{}\n'.format(self.lead_data))
 
     if LOG_MPC:
       self.send_mpc_solution(pm, n_its, duration)
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 23c856ad7d9d1d..7f74cd274e2718 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -142,7 +142,7 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise):
     min_tracks = 3
     track_speed_margin = .55  # 55 percent
     min_speed = 30
-    self.track_data = [i for i in self.track_data if (v_cruise * track_speed_margin) < i]
+    self.track_data = [i for i in self.track_data if (v_cruise * track_speed_margin) < i]  # TODO: test self.v_ego instead of v_cruise
     if len(self.track_data) >= min_tracks and self.v_ego > (min_speed * CV.MPH_TO_MS):
       average_track_speed = np.mean(self.track_data)
       if average_track_speed < v_target and average_track_speed < v_target_future:

From b741e4f89a4e17128e55ff082c2e9f1fcf684795 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 04:01:38 -0600
Subject: [PATCH 06/39] ehh why not

---
 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 7f74cd274e2718..b654c0525e48cc 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -142,7 +142,7 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise):
     min_tracks = 3
     track_speed_margin = .55  # 55 percent
     min_speed = 30
-    self.track_data = [i for i in self.track_data if (v_cruise * track_speed_margin) < i]  # TODO: test self.v_ego instead of v_cruise
+    self.track_data = [i for i in self.track_data if (self.v_ego * track_speed_margin) < i]
     if len(self.track_data) >= min_tracks and self.v_ego > (min_speed * CV.MPH_TO_MS):
       average_track_speed = np.mean(self.track_data)
       if average_track_speed < v_target and average_track_speed < v_target_future:

From 2aee53eebb6e0e76ad6d30cd13113cb6be17ba5c Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 04:02:34 -0600
Subject: [PATCH 07/39] test 15 mph

---
 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 b654c0525e48cc..7f7933a89be719 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -141,7 +141,7 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise):
       f.write('{}\n'.format(self.track_data))
     min_tracks = 3
     track_speed_margin = .55  # 55 percent
-    min_speed = 30
+    min_speed = 15
     self.track_data = [i for i in self.track_data if (self.v_ego * track_speed_margin) < i]
     if len(self.track_data) >= min_tracks and self.v_ego > (min_speed * CV.MPH_TO_MS):
       average_track_speed = np.mean(self.track_data)

From 0aedd1d6167e769560ae929fce5bcaf40ed503b1 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 14:58:54 -0600
Subject: [PATCH 08/39] maybe this will work?

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 2f64a15f323377..d68ece8b0f3813 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -78,7 +78,8 @@ def get_TR(self):
       self.store_lead_data()
       TR = self.dynamic_follow()
 
-    self.change_cost(TR)
+    if not travis:
+      self.change_cost(TR)
     return TR
 
   def change_cost(self, TR):

From d0028323f3a16e322101edafae339e514920008a Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 15:18:57 -0600
Subject: [PATCH 09/39] disable steer offset when lane change is occurring in
 direction of hug

---
 selfdrive/controls/lane_hugging.py       | 13 ++++++++++---
 selfdrive/controls/lib/latcontrol_lqr.py |  2 +-
 selfdrive/controls/lib/latcontrol_pid.py |  2 +-
 3 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/selfdrive/controls/lane_hugging.py b/selfdrive/controls/lane_hugging.py
index 5860a567205be4..f2d459c8095bfd 100644
--- a/selfdrive/controls/lane_hugging.py
+++ b/selfdrive/controls/lane_hugging.py
@@ -1,5 +1,8 @@
 from common.op_params import opParams
+from cereal import log
 
+LaneChangeState = log.PathPlan.LaneChangeState
+LaneChangeDirection = log.PathPlan.LaneChangeDirection
 
 class LaneHugging:
   def __init__(self):
@@ -9,11 +12,15 @@ def __init__(self):
       self.direction = self.direction.lower()
     self.angle_offset = abs(self.op_params.get('lane_hug_angle_offset', 0.0))
 
-  def offset_mod(self, angle_steers_des):
+  def offset_mod(self, path_plan):
     # negative angles: right
     # positive angles: left
-    if self.direction == 'left':
+    angle_steers_des = path_plan.angleSteers
+    lane_change_state = path_plan.laneChangeState
+    lane_change_direction = path_plan.laneChangeDirection
+    starting = LaneChangeState.laneChangeStarting
+    if self.direction == 'left' and ((lane_change_state == starting and not lane_change_direction.left) or lane_change_state != starting):
       angle_steers_des -= self.angle_offset
-    elif self.direction == 'right':
+    elif self.direction == 'right' and ((lane_change_state == starting and not lane_change_direction.right) or lane_change_state != starting):
       angle_steers_des += self.angle_offset
     return angle_steers_des
diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py
index 6070c1213ebc80..ce3f00f6ac5496 100644
--- a/selfdrive/controls/lib/latcontrol_lqr.py
+++ b/selfdrive/controls/lib/latcontrol_lqr.py
@@ -53,7 +53,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste
 
     # Subtract offset. Zero angle should correspond to zero torque
     # self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
-    self.angle_steers_des = self.lane_hugging.offset_mod(path_plan.angleSteers - path_plan.angleOffset)
+    self.angle_steers_des = self.lane_hugging.offset_mod(path_plan) - path_plan.angleOffset
     angle_steers -= path_plan.angleOffset
 
     # Update Kalman filter
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index 03b8695e7a5330..ee4ac69a50ab39 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -27,7 +27,7 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste
       self.pid.reset()
     else:
       # self.angle_steers_des = path_plan.angleSteers  # get from MPC/PathPlanner
-      self.angle_steers_des = self.lane_hugging.offset_mod(path_plan.angleSteers)
+      self.angle_steers_des = self.lane_hugging.offset_mod(path_plan)
 
       steers_max = get_steer_max(CP, v_ego)
       self.pid.pos_limit = steers_max

From 282d94229c7a6166e167c53032ec4c8a62c3a6d1 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 15:19:09 -0600
Subject: [PATCH 10/39] is this required?

---
 selfdrive/test/process_replay/test_processes.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py
index 748f4e5f9417a2..8af87cf0bb7602 100755
--- a/selfdrive/test/process_replay/test_processes.py
+++ b/selfdrive/test/process_replay/test_processes.py
@@ -17,7 +17,7 @@
   "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE
   "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA
 ]
-# segments.remove("cce908f7eb8db67d|2019-08-02--15-09-51--3")
+segments.remove("cce908f7eb8db67d|2019-08-02--15-09-51--3")
 
 def get_segment(segment_name):
   route_name, segment_num = segment_name.rsplit("--", 1)

From 047ad3a6327bc22c8a85cef2b3d9c4561343f8f5 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 15:20:33 -0600
Subject: [PATCH 11/39] remove lint tests

---
 run_docker_tests.sh                | 4 ++--
 selfdrive/controls/lib/long_mpc.py | 2 +-
 2 files changed, 3 insertions(+), 3 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 d68ece8b0f3813..74c279c2acd678 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -141,7 +141,7 @@ def dynamic_follow(self):
     TR += TR_mod
 
     if self.CS.leftBlinker or self.CS.rightBlinker:
-      old_TR = float(TR)
+      # old_TR = float(TR)
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
       y = [1.0, .7, .65]  # reduce TR when changing lanes
       TR *= interp(self.car_data['v_ego'], x, y)

From 26087beb5f7f24fec12b962def936e658f05e7cb Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 18:41:36 -0600
Subject: [PATCH 12/39] fix for detecting lane change

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

diff --git a/selfdrive/controls/lane_hugging.py b/selfdrive/controls/lane_hugging.py
index f2d459c8095bfd..5ef29497121255 100644
--- a/selfdrive/controls/lane_hugging.py
+++ b/selfdrive/controls/lane_hugging.py
@@ -17,10 +17,10 @@ def offset_mod(self, path_plan):
     # positive angles: left
     angle_steers_des = path_plan.angleSteers
     lane_change_state = path_plan.laneChangeState
-    lane_change_direction = path_plan.laneChangeDirection
+    direction = path_plan.laneChangeDirection
     starting = LaneChangeState.laneChangeStarting
-    if self.direction == 'left' and ((lane_change_state == starting and not lane_change_direction.left) or lane_change_state != starting):
+    if self.direction == 'left' and ((lane_change_state == starting and direction != LaneChangeDirection.left) or lane_change_state != starting):
       angle_steers_des -= self.angle_offset
-    elif self.direction == 'right' and ((lane_change_state == starting and not lane_change_direction.right) or lane_change_state != starting):
+    elif self.direction == 'right' and ((lane_change_state == starting and direction != LaneChangeDirection.right) or lane_change_state != starting):
       angle_steers_des += self.angle_offset
     return angle_steers_des

From ffb9adbbe445083e25ad721b5f17596bd63795c5 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:20:02 -0600
Subject: [PATCH 13/39] dynamic lane speed tuning, alter a_target as well.
 allow it to work when we have a lead, as it can pick up lead ahead of lead
 and slow down for that lead maybe? more weight on surrounding cars

---
 selfdrive/controls/lib/longcontrol.py | 27 ++++++++++++++++-----------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 7f7933a89be719..733843c4e78142 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -136,13 +136,13 @@ def handle_passable(self, passable):
     self.gas_pressed = passable['gas_pressed']
     self.handle_live_tracks(passable['live_tracks'])
 
-  def dynamic_lane_speed(self, v_target, v_target_future, v_cruise):
-    with open('/data/live_tracks_test', 'a') as f:
-      f.write('{}\n'.format(self.track_data))
+  def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
+    # with open('/data/live_tracks_test', 'a') as f:  # looks like it's getting live_tracks nicely
+    #   f.write('{}\n'.format(self.track_data))
     min_tracks = 3
-    track_speed_margin = .55  # 55 percent
+    track_speed_margin = .5  # 50 percent
     min_speed = 15
-    self.track_data = [i for i in self.track_data if (self.v_ego * track_speed_margin) < i]
+    self.track_data = [trk_vel for trk_vel in self.track_data if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]
     if len(self.track_data) >= min_tracks and self.v_ego > (min_speed * CV.MPH_TO_MS):
       average_track_speed = np.mean(self.track_data)
       if average_track_speed < v_target and average_track_speed < v_target_future:
@@ -150,15 +150,20 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise):
         # 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, 8, 16]
-        y = [0.3, .55, 0.7]
+        x = [3, 6, 16]
+        y = [0.4, .6, 0.7]
         track_speed_weight = interp(len(self.track_data), 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.2 * 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:  # just a sanity check
+        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 = (1 / 20.) * ((v_target_slow - v_target) / 1.0)  # mpc time_step is 0.2 s, so interpolate assuming a_target is 1 second into future?
+          a_target = a_target_slow  # todo: should we mess with a_target, or leave it alone??
           v_target = v_target_slow
           v_target_future = v_target_slow
 
-    return v_target, v_target_future
+
+    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"""
@@ -173,8 +178,8 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
       gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV)
     brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV)
 
-    if not travis and not self.lead_data['status']:  # want to make sure we don't mess with anything when there's a lead
-      v_target, v_target_future = self.dynamic_lane_speed(v_target, v_target_future, v_cruise)
+    if not travis:
+      v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
 
     # Update state machine
     output_gb = self.last_output_gb

From 78ed689a720e9bc8b7681791b0b31176e55f0441 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:26:05 -0600
Subject: [PATCH 14/39] make minimum dynamic lane speed controller speed
 adjustable

---
 common/op_params.py                   | 3 ++-
 selfdrive/controls/lib/longcontrol.py | 7 ++++---
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/common/op_params.py b/common/op_params.py
index 121b4e9187db31..8c656ed9f9fe71 100644
--- a/common/op_params.py
+++ b/common/op_params.py
@@ -38,7 +38,8 @@ def __init__(self):
                            '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)'},
-                           'min_model_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the model will be allowed to slow down for curves (in MPH)'}}
+                           'min_model_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the model will be allowed to slow down for curves (in MPH)'},
+                           'min_dynamic_lane_speed': {'default': 20, 'allowed_types': [float, int], 'description': 'The minimum speed the dynamic lane speed controller will adjust your speed based on surrounding cars from the radar (in MPH)'}}
 
     self.params = {}
     self.params_file = "/data/op_params.json"
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 733843c4e78142..418a3d86067a29 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -4,6 +4,7 @@
 from common.travis_checker import travis
 from selfdrive.config import Conversions as CV
 import numpy as np
+from common.op_params import opParams
 
 LongCtrlState = log.ControlsState.LongControlState
 
@@ -72,6 +73,7 @@ def __init__(self, CP, compute_gb):
     self.v_ego = 0.0
     self.gas_pressed = False
     self.track_data = []
+    self.min_dynamic_lane_speed = opParams().get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS
 
   def reset(self, v_pid):
     """Reset PID controller and change setpoint"""
@@ -141,12 +143,11 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
     #   f.write('{}\n'.format(self.track_data))
     min_tracks = 3
     track_speed_margin = .5  # 50 percent
-    min_speed = 15
     self.track_data = [trk_vel for trk_vel in self.track_data if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]
-    if len(self.track_data) >= min_tracks and self.v_ego > (min_speed * CV.MPH_TO_MS):
+    if len(self.track_data) >= min_tracks and self.v_ego > self.min_dynamic_lane_speed:
       average_track_speed = np.mean(self.track_data)
       if average_track_speed < v_target and average_track_speed < v_target_future:
-        # so basically, if there's no lead, there's at least 3 tracks, the speeds of the tracks must be within 50% of set speed, if our speed is at least 30 mph,
+        # so basically, if there's at least 3 tracks, the speeds of the tracks must be within 50% 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...?

From c6b7d49713c07c0ea3c97fa1904c7b307d01e1c2 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:29:53 -0600
Subject: [PATCH 15/39] refactor

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

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 418a3d86067a29..fbed320ce5122b 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -143,6 +143,7 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
     #   f.write('{}\n'.format(self.track_data))
     min_tracks = 3
     track_speed_margin = .5  # 50 percent
+    MPC_TIME_STEP = 1 / 20.
     self.track_data = [trk_vel for trk_vel in self.track_data if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]
     if len(self.track_data) >= min_tracks and self.v_ego > self.min_dynamic_lane_speed:
       average_track_speed = np.mean(self.track_data)
@@ -158,7 +159,7 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
           track_speed_weight = clip(1.2 * 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 = (1 / 20.) * ((v_target_slow - v_target) / 1.0)  # mpc time_step is 0.2 s, so interpolate assuming a_target is 1 second into future?
+          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  # todo: should we mess with a_target, or leave it alone??
           v_target = v_target_slow
           v_target_future = v_target_slow

From 2c699f7a4c0c69d5be3c0920bdc671cfd0d36d78 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:37:43 -0600
Subject: [PATCH 16/39] lead data is working as expected

---
 selfdrive/controls/lib/longcontrol.py | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index fbed320ce5122b..e5428c5ac67fcf 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -93,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
@@ -139,8 +136,6 @@ def handle_passable(self, passable):
     self.handle_live_tracks(passable['live_tracks'])
 
   def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
-    # with open('/data/live_tracks_test', 'a') as f:  # looks like it's getting live_tracks nicely
-    #   f.write('{}\n'.format(self.track_data))
     min_tracks = 3
     track_speed_margin = .5  # 50 percent
     MPC_TIME_STEP = 1 / 20.

From ebe00841335784345c1244f3894ebc5974b02c38 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:38:29 -0600
Subject: [PATCH 17/39] slight tuning

---
 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 e5428c5ac67fcf..5d09c8b2c564dd 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -151,7 +151,7 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
         y = [0.4, .6, 0.7]
         track_speed_weight = interp(len(self.track_data), 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.2 * track_speed_weight, min(y), max(y))
+          track_speed_weight = clip(1.1 * 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?

From 762318e90fe465d65bb3c8dbdf386a99b20d495c Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:41:59 -0600
Subject: [PATCH 18/39] wrong side of the list for the deceleration detection,
 we want past v_ego, not immediately last

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 74c279c2acd678..fa629e5c7ec496 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -11,6 +11,7 @@
 from common.op_params import opParams
 from common.numpy_fast import interp, clip
 from common.travis_checker import travis
+from selfdrive.config import Conversions as CV
 
 LOG_MPC = os.environ.get('LOG_MPC', False)
 
@@ -18,7 +19,6 @@
 class LongitudinalMpc():
   def __init__(self, mpc_id):
     self.mpc_id = mpc_id
-    self.MPH_TO_MS = 0.44704
     self.op_params = opParams()
 
     self.setup_mpc()
@@ -119,10 +119,10 @@ def dynamic_follow(self):
     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
 
-    sng_TR = 1.7  # stop and go parameters
-    sng_speed = 15.0 * self.MPH_TO_MS
+    sng_TR = 1.8  # stop and go parameters
+    sng_speed = 15.0 * CV.MPH_TO_MS
 
-    if self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][-1]['v_ego'] >= self.car_data['v_ego']:  # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use 1.8s and slowly decrease
+    if self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][0]['v_ego'] >= self.car_data['v_ego']:  # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use 1.8s and slowly decrease
       TR = interp(self.car_data['v_ego'], x_vel, y_mod)
     else:  # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating
       x = [sng_speed / 3.0, sng_speed]  # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating

From 1e6c989e83eac55d592b02382450b03281de85a5 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:49:53 -0600
Subject: [PATCH 19/39] slightly higher TR below 50 mph

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index fa629e5c7ec496..339ae8e824ca5e 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -117,7 +117,7 @@ def lead_accel_over_time(self):
 
   def dynamic_follow(self):
     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
+    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 e7672648db74288dd611b0a8913cf9510877ef8c Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 19:57:53 -0600
Subject: [PATCH 20/39] reduce relative velocity tuning with positive vRel

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 339ae8e824ca5e..e62441aa1ab50d 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -130,8 +130,8 @@ def dynamic_follow(self):
       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

From 03c08c258534d58c474828973464ae7c184eba3b Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 20:13:16 -0600
Subject: [PATCH 21/39] reduce positive lead acceleration tuning

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index e62441aa1ab50d..22d1999c599a1b 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -134,8 +134,8 @@ def dynamic_follow(self):
     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
+    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)  # todo: test if these modifications are too much
 
     TR += TR_mod

From 503a88b35848591664b411e6b23a8919058b3380 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 20:22:01 -0600
Subject: [PATCH 22/39] better handling of passable if travis, or if
 live_tracks not updated. hopefully this also fixed long_mpc not receiving
 blinker updates

---
 selfdrive/controls/controlsd.py       | 12 +++++-------
 selfdrive/controls/lib/long_mpc.py    | 18 ++++++++----------
 selfdrive/controls/lib/longcontrol.py | 10 ++++------
 3 files changed, 17 insertions(+), 23 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 207f8380a8fe9f..56486a7c3cae8c 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -284,14 +284,12 @@ 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 = {}
   if not travis:
-    if sm.updated['liveTracks']:
-      live_tracks = sm['liveTracks']
-    else:
-      live_tracks = None
-    passable_loc = {'lead_one': sm['radarState'].leadOne, 'live_tracks': live_tracks, 'has_lead': plan.hasLead, 'gas_pressed': CS.gasPressed}
-  else:
-    passable_loc = None
+    passable_loc['lead_one'] = sm['radarState'].leadOne
+    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
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 22d1999c599a1b..9ddb23977efafa 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.CS = 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
@@ -69,14 +68,14 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v
     self.cur_state[0].a_ego = a
 
-  def get_TR(self):
+  def get_TR(self, CS):
     if not self.lead_data['status'] or travis:
       TR = 1.8
     elif self.customTR is not None:
       TR = clip(self.customTR, 0.9, 2.7)
     else:
       self.store_lead_data()
-      TR = self.dynamic_follow()
+      TR = self.dynamic_follow(CS)
 
     if not travis:
       self.change_cost(TR)
@@ -115,7 +114,7 @@ def lead_accel_over_time(self):
           a_lead = calculated_accel
     return a_lead  # if above doesn't execute, we'll return a_lead from radar
 
-  def dynamic_follow(self):
+  def dynamic_follow(self, CS):
     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
 
@@ -140,13 +139,13 @@ def dynamic_follow(self):
 
     TR += TR_mod
 
-    if self.CS.leftBlinker or self.CS.rightBlinker:
-      # old_TR = float(TR)
+    if CS.leftBlinker or CS.rightBlinker:
+      old_TR = float(TR)
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
       y = [1.0, .7, .65]  # 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([self.CS.leftBlinker, self.CS.rightBlinker, self.car_data['v_ego'], old_TR, TR]))
+      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
 
@@ -176,7 +175,6 @@ def process_lead(self, v_lead, a_lead, x_lead, status):
 
   def update(self, pm, CS, lead, v_cruise_setpoint):
     v_ego = CS.vEgo
-    self.CS = CS
     self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo}
 
     # Setup current mpc state
@@ -213,7 +211,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint):
 
     # Calculate mpc
     t = sec_since_boot()
-    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR())
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, self.get_TR(CS))
     duration = int((sec_since_boot() - t) * 1e9)
 
     # if not travis:
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 5d09c8b2c564dd..b866a15d2862bf 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -121,10 +121,10 @@ def dynamic_gas(self, CP):
 
     return clip(gas, 0.0, 1.0)
 
-  def handle_live_tracks(self, tracks):
-    if tracks is not None:  # if updated
+  def handle_live_tracks(self, live_tracks):
+    if live_tracks['updated']:
       self.track_data = []
-      for track in tracks:
+      for track in live_tracks['tracks']:
         self.track_data.append(self.v_ego + track.vRel)
 
   def handle_passable(self, passable):
@@ -171,13 +171,11 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
     if not travis:
       self.handle_passable(passable)  # so travis doesn't call vRel... on None
       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)
 
-    if not travis:
-      v_target, v_target_future, a_target = self.dynamic_lane_speed(v_target, v_target_future, v_cruise, a_target)
-
     # Update state machine
     output_gb = self.last_output_gb
     self.long_control_state = long_control_state_trans(active, self.long_control_state, v_ego,

From 8ec62126acee9ae12c4551900a98c169a0b83c6b Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 20:32:22 -0600
Subject: [PATCH 23/39] allow dynamic lane speed to be toggleable

---
 common/op_params.py                   |  9 +++---
 selfdrive/controls/lib/longcontrol.py | 43 ++++++++++++++-------------
 selfdrive/controls/lib/planner.py     |  4 +--
 3 files changed, 30 insertions(+), 26 deletions(-)

diff --git a/common/op_params.py b/common/op_params.py
index 8c656ed9f9fe71..d78448bacfede4 100644
--- a/common/op_params.py
+++ b/common/op_params.py
@@ -38,14 +38,15 @@ def __init__(self):
                            '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)'},
-                           'min_model_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the model will be allowed to slow down for curves (in MPH)'},
-                           'min_dynamic_lane_speed': {'default': 20, 'allowed_types': [float, int], 'description': 'The minimum speed the dynamic lane speed controller will adjust your speed based on surrounding cars from the radar (in MPH)'}}
+                           'min_model_slowdown_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the driving model will be allowed to slow you down for curves (in MPH)'},
+                           'dynamic_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether to automatically adjust your speed based on vehicles in surrounding lanes'},
+                           'min_dynamic_lane_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the dynamic lane speed controller will be allowed to adjust your speed based on surrounding cars from the radar (in MPH)'}}
 
     self.params = {}
     self.params_file = "/data/op_params.json"
     self.kegman_file = "/data/kegman.json"
     self.last_read_time = time.time()
-    self.read_timeout = 1.0  # max frequency to read with self.get(...) (sec)
+    self.read_frequency = 10.0  # max frequency to read with self.get(...) (sec)
     self.force_update = False  # replaces values with default params if True, not just add add missing key/value pairs
     self.run_init()  # restores, reads, and updates params
 
@@ -104,7 +105,7 @@ def put(self, key, value):
     write_params(self.params, self.params_file)
 
   def get(self, key=None, default=None):  # can specify a default value if key doesn't exist
-    if (time.time() - self.last_read_time) >= self.read_timeout and not travis:  # make sure we aren't reading file too often
+    if (time.time() - self.last_read_time) >= self.read_frequency and not travis:  # make sure we aren't reading file too often
       self.params, read_status = read_params(self.params_file, self.format_default_params())
       self.last_read_time = time.time()
     if key is None:  # get all
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index b866a15d2862bf..f1c5a86df16154 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -73,7 +73,9 @@ def __init__(self, CP, compute_gb):
     self.v_ego = 0.0
     self.gas_pressed = False
     self.track_data = []
-    self.min_dynamic_lane_speed = opParams().get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS
+    self.op_params = opParams()
+    self.min_dynamic_lane_speed = self.op_params.get('min_dynamic_lane_speed', default=20.) * CV.MPH_TO_MS
+    self.dynamic_lane_speed_active = self.op_params.get('dynamic_lane_speed', default=True)
 
   def reset(self, v_pid):
     """Reset PID controller and change setpoint"""
@@ -139,25 +141,26 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
     min_tracks = 3
     track_speed_margin = .5  # 50 percent
     MPC_TIME_STEP = 1 / 20.
-    self.track_data = [trk_vel for trk_vel in self.track_data if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]
-    if len(self.track_data) >= min_tracks and self.v_ego > self.min_dynamic_lane_speed:
-      average_track_speed = np.mean(self.track_data)
-      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 50% 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, 16]
-        y = [0.4, .6, 0.7]
-        track_speed_weight = interp(len(self.track_data), 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.1 * 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  # todo: should we mess with a_target, or leave it alone??
-          v_target = v_target_slow
-          v_target_future = v_target_slow
+    if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:
+      self.track_data = [trk_vel for trk_vel in self.track_data if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]
+      if len(self.track_data) >= min_tracks:
+        average_track_speed = np.mean(self.track_data)
+        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 50% 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, 16]
+          y = [0.4, .6, 0.7]
+          track_speed_weight = interp(len(self.track_data), 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.1 * 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  # todo: should we mess with a_target, or leave it alone??
+            v_target = v_target_slow
+            v_target_future = v_target_slow
 
 
     return v_target, v_target_future, a_target
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index dcc3b02ee29a9d..a1c3d976f2aa50 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -81,7 +81,7 @@ def __init__(self, CP):
     self.path_x = np.arange(192)
 
     self.params = Params()
-    self.min_model_speed = opParams().get('min_model_speed', default=20.0)
+    self.min_model_slowdown_speed = opParams().get('min_model_slowdown_speed', default=20.0)
 
   def choose_solution(self, v_cruise_setpoint, enabled):
     if enabled:
@@ -139,7 +139,7 @@ def update(self, sm, pm, CP, VM, PP):
       a_y_max = 2.975 - v_ego * 0.0375  # ~1.85 @ 75mph, ~2.6 @ 25mph
       v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
       model_speed = np.min(v_curvature)
-      model_speed = max(self.min_model_speed * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
+      model_speed = max(self.min_model_slowdown_speed * CV.MPH_TO_MS, model_speed)  # Don't slow down below 20mph
     else:
       model_speed = MAX_SPEED
 

From 9b0087b4e9bcfb050d5cfcd6e3a3fb4a998f13cd Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 20:35:12 -0600
Subject: [PATCH 24/39] add travis check for accel limits

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

diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index a1c3d976f2aa50..79ba7f1c24992c 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -14,6 +14,7 @@
 from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
 from common.op_params import opParams
+from common.travis_checker import travis
 
 MAX_SPEED = 255.0
 
@@ -28,7 +29,10 @@
 
 # need fast accel at very low speed for stop and go
 # make sure these accelerations are smaller than mpc limits
-_A_CRUISE_MAX_V = [2.0, 1.9, 0.8, .4]
+if not travis:
+  _A_CRUISE_MAX_V = [2.0, 1.9, 0.8, .4]
+else:
+  _A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4]
 _A_CRUISE_MAX_BP = [0.,  6.4, 22.5, 40.]
 
 # Lookup table for turns

From 6e0a3baf12c7215dddb2c11f10f4709e940c66e6 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 20:36:46 -0600
Subject: [PATCH 25/39] debug

---
 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 f1c5a86df16154..f550a3c7ebf263 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -138,6 +138,7 @@ def handle_passable(self, passable):
     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
     track_speed_margin = .5  # 50 percent
     MPC_TIME_STEP = 1 / 20.
@@ -168,7 +169,6 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, 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
-    v_cruise *= CV.KPH_TO_MS  # convert to m/s
 
     # Actuation limits
     if not travis:

From b937b5cbf2e66f9e435584058b0f5b7dbbcbbb62 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 20:39:39 -0600
Subject: [PATCH 26/39] tuning

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index 9ddb23977efafa..ea5960d88a2dba 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -90,8 +90,8 @@ def change_cost(self, TR):
       self.last_cost = cost
 
   def store_lead_data(self):
-    v_lead_retention = 2.0  # seconds
-    v_ego_retention = 1.5
+    v_lead_retention = 2.25  # seconds
+    v_ego_retention = 2.5
 
     if self.lead_data['status']:
       self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if
@@ -110,7 +110,7 @@ def lead_accel_over_time(self):
       if elapsed > min_consider_time:  # if greater than min time (not 0)
         v_diff = self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead']
         calculated_accel = v_diff / elapsed
-        if abs(calculated_accel) > abs(a_lead):  # if a_lead is greater than calculated accel (over last 1.5s, use that)
+        if abs(calculated_accel) > abs(a_lead):  # if a_lead is greater than calculated accel (over last 1.5s) use that
           a_lead = calculated_accel
     return a_lead  # if above doesn't execute, we'll return a_lead from radar
 
@@ -135,7 +135,7 @@ def dynamic_follow(self, CS):
 
     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)  # todo: test if these modifications are too much
+    TR_mod += interp(self.lead_accel_over_time(), x, y)
 
     TR += TR_mod
 

From 569ed2a6ec74d874722ee2838d4ced31f8d43917 Mon Sep 17 00:00:00 2001
From: Shane <shane@smiskol.com>
Date: Wed, 18 Dec 2019 20:40:52 -0600
Subject: [PATCH 27/39] change sleep time

---
 op_edit.py | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/op_edit.py b/op_edit.py
index 584a541cd1ce98..5921287a9444cc 100644
--- a/op_edit.py
+++ b/op_edit.py
@@ -7,6 +7,7 @@ class opEdit:  # use by running `python /data/openpilot/op_edit.py`
   def __init__(self):
     self.op_params = opParams()
     self.params = None
+    self.sleep_time = 1.0
     print('Welcome to the opParams command line editor!')
     print('Here are your parameters:\n')
     self.run_loop()
@@ -46,11 +47,11 @@ def parse_choice(self, choice):
       return 'error', choice
     else:
       print('\nNot an integer!\n', flush=True)
-      time.sleep(1.5)
+      time.sleep(self.sleep_time)
       return 'retry', choice
     if choice not in range(1, len(self.params) + 3):  # three for add/delete parameter
       print('Not in range!\n', flush=True)
-      time.sleep(1.5)
+      time.sleep(self.sleep_time)
       return 'continue', choice
 
     if choice == len(self.params) + 1:  # add new parameter
@@ -87,7 +88,7 @@ def change_parameter(self, choice):
 
     if extra_info and not any([isinstance(new_value, typ) for typ in param_allowed_types]):
       print('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(new_value)).split("'")[1]))
-      time.sleep(1.5)
+      time.sleep(self.sleep_time)
       return
 
     print('\nOld value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1]))
@@ -99,7 +100,7 @@ def change_parameter(self, choice):
       print('\nSaved!\n')
     else:
       print('\nNot saved!\n', flush=True)
-    time.sleep(1.5)
+    time.sleep(self.sleep_time)
 
   def parse_input(self, dat):
     try:
@@ -136,7 +137,7 @@ def delete_parameter(self):
       print('\nDeleted!\n')
     else:
       print('\nNot saved!\n', flush=True)
-    time.sleep(1.5)
+    time.sleep(self.sleep_time)
 
   def add_parameter(self):
     print('Type the name of your new parameter:')
@@ -169,7 +170,7 @@ def add_parameter(self):
       print('\nSaved!\n')
     else:
       print('\nNot saved!\n', flush=True)
-    time.sleep(1.5)
+    time.sleep(self.sleep_time)
 
 
 opEdit()

From 37f0d0ef74f8bd34bece687fc4e76d7da03e8637 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 00:38:17 -0600
Subject: [PATCH 28/39] tuning for dynamic lane speed controller

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

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index f550a3c7ebf263..4bab5162cccd04 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -140,26 +140,26 @@ def handle_passable(self, passable):
   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
-    track_speed_margin = .5  # 50 percent
+    track_speed_margin = .7  # 70 percent
     MPC_TIME_STEP = 1 / 20.
     if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:
       self.track_data = [trk_vel for trk_vel in self.track_data if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]
       if len(self.track_data) >= min_tracks:
         average_track_speed = np.mean(self.track_data)
         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 50% of set speed, if our speed is at least set_speed mph,
+          # 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, 16]
-          y = [0.4, .6, 0.7]
+          y = [0.325, .525, 0.625]
           track_speed_weight = interp(len(self.track_data), 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.1 * track_speed_weight, min(y), max(y))
+            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  # todo: should we mess with a_target, or leave it alone??
+            a_target = a_target_slow
             v_target = v_target_slow
             v_target_future = v_target_slow
 

From 4f8b244216fe0884cd9f9dc03de296220980c80a Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 00:40:41 -0600
Subject: [PATCH 29/39] let us use gas pedal without disengaging

---
 selfdrive/car/toyota/interface.py     | 4 ++--
 selfdrive/controls/lib/longcontrol.py | 1 -
 2 files changed, 2 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 6704d0261eedb4..1dda997bc8965d 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -413,11 +413,11 @@ def update(self, c, can_strings):
       events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
 
     # disable on pedals rising edge or when brake is pressed and speed isn't zero
-    if (ret.gasPressed and not self.gas_pressed_prev) or \
+    if (ret.gasPressed and not self.gas_pressed_prev and travis) or \
        (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
       events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
 
-    if ret.gasPressed:
+    if ret.gasPressed and travis:
       events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
 
     ret.events = events
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 4bab5162cccd04..9554ef4c7c1a50 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -163,7 +163,6 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
             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):

From 299db1cbc48a7d8046b5d23d93ca64b7dc4d6643 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 00:42:09 -0600
Subject: [PATCH 30/39] reset longcontrol when gas pressed

---
 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 9554ef4c7c1a50..05d692327c7011 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -186,7 +186,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_
 
     v_ego_pid = max(v_ego, MIN_CAN_SPEED)  # Without this we get jumps, CAN bus reports 0 when speed < 0.3
 
-    if self.long_control_state == LongCtrlState.off:
+    if self.long_control_state == LongCtrlState.off or (self.gas_pressed and not travis):
       self.v_pid = v_ego_pid
       self.pid.reset()
       output_gb = 0.

From 24b66ee5d8fd6567bbb2c57c6742df012b02cea4 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 01:02:43 -0600
Subject: [PATCH 31/39] add tuning for corolla pedal, might need to test to see
 if this is a good tune anymore

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

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 1dda997bc8965d..83c6d9d531ecd9 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -126,6 +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   # full torque for 20 deg at 80mph means 0.00007818594
+      if ret.enableGasInterceptor:
+        ret.longitudinalTuning.kpVPedal = [1.0, 0.66, 0.42]
+        ret.longitudinalTuning.kiVPedal = [0.135, 0.09]
 
     elif candidate == CAR.LEXUS_RXH:
       stop_and_go = True

From 5c41aa632f627e00323e42ec9b8a6dd324b344ce Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 09:23:41 -0600
Subject: [PATCH 32/39] Update interface.py

---
 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 83c6d9d531ecd9..c01dcdc9827664 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   # full torque for 20 deg at 80mph means 0.00007818594
-      if ret.enableGasInterceptor:
-        ret.longitudinalTuning.kpVPedal = [1.0, 0.66, 0.42]
-        ret.longitudinalTuning.kiVPedal = [0.135, 0.09]
+
+
+
 
     elif candidate == CAR.LEXUS_RXH:
       stop_and_go = True

From 5f7a5b39dd16e19d9fe11b8abf731f43efd05d1e Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 10:12:29 -0600
Subject: [PATCH 33/39] Corolla tuning

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

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index c01dcdc9827664..2252300e61a369 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -125,9 +125,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       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]]
-      ret.lateralTuning.pid.kf = 0.00003   # full torque for 20 deg at 80mph means 0.00007818594
-
-
+      ret.lateralTuning.pid.kf = 0.00003 * 0.92  # 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]
 
 
     elif candidate == CAR.LEXUS_RXH:

From 5984d9dea487611858655652d3e1d447d2d4d558 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 12:00:21 -0600
Subject: [PATCH 34/39] Debug

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

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 2252300e61a369..fdae42d7a57416 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -125,8 +125,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       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]]
-      ret.lateralTuning.pid.kf = 0.00003 * 0.92  # full torque for 20 deg at 80mph means 0.00007818594
+      ret.lateralTuning.pid.kf = 0.00003 * 0.8  # full torque for 20 deg at 80mph means 0.00007818594
       if ret.enableGasInterceptor:
+        with open('/data/gasint', 'a') as f:
+          f.write('True\n')
         ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42]
         ret.longitudinalTuning.kiV = [0.135, 0.09]
 

From 0bbba3d70552553dde37eb681ee5aa6eea52bca5 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 18:04:36 -0600
Subject: [PATCH 35/39] tuning

---
 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 05d692327c7011..3164b2716ebf0e 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -152,7 +152,7 @@ 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, 16]
-          y = [0.325, .525, 0.625]
+          y = [0.25, .35, 0.45]
           track_speed_weight = interp(len(self.track_data), 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))

From be58c12edf4530bc8d36397a92dc4e25360920df Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 19 Dec 2019 21:33:09 -0600
Subject: [PATCH 36/39] don't reset tracks before next update, duh! fix one car
 reporting more than 1 point, eg. semi trucks

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

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 3164b2716ebf0e..e04914c97bc63a 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -142,18 +142,25 @@ def dynamic_lane_speed(self, v_target, v_target_future, v_cruise, a_target):
     min_tracks = 3
     track_speed_margin = .7  # 70 percent
     MPC_TIME_STEP = 1 / 20.
+    similar_track_tolerance = 0.025
     if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:
-      self.track_data = [trk_vel for trk_vel in self.track_data if (self.v_ego * track_speed_margin) <= trk_vel <= v_cruise]
-      if len(self.track_data) >= min_tracks:
-        average_track_speed = np.mean(self.track_data)
+      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, 16]
-          y = [0.25, .35, 0.45]
-          track_speed_weight = interp(len(self.track_data), x, y)
+          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)

From 3e9e484a424c6d77eea3c7db737bfad845a4bf3a Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Fri, 20 Dec 2019 12:07:15 -0600
Subject: [PATCH 37/39] dynamic speed margins based on v_ego

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

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index e04914c97bc63a..701fdc564a3a37 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -140,7 +140,9 @@ def handle_passable(self, passable):
   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
-    track_speed_margin = .7  # 70 percent
+    vels = [i * CV.MPH_TO_MS for i in [5, 20, 60]]
+    margins = [0.5, 0.6, 0.65]
+    track_speed_margin = interp(self.v_ego, vels, margins)
     MPC_TIME_STEP = 1 / 20.
     similar_track_tolerance = 0.025
     if self.dynamic_lane_speed_active and self.v_ego > self.min_dynamic_lane_speed:

From 8903c1304493a3014fc2a2fb4f6958224ace6b26 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 21 Dec 2019 00:32:39 -0600
Subject: [PATCH 38/39] this works

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

diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
index ea5960d88a2dba..5ab8a05199a992 100644
--- a/selfdrive/controls/lib/long_mpc.py
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -144,8 +144,6 @@ def dynamic_follow(self, CS):
       x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
       y = [1.0, .7, .65]  # 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
 

From 38daf0f070b3a07fc04d0c4fbc48693dea381f7b Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Sat, 21 Dec 2019 00:33:08 -0600
Subject: [PATCH 39/39] remove temp

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

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index fdae42d7a57416..2f89e7685a1c3b 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -127,8 +127,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
       ret.lateralTuning.pid.kf = 0.00003 * 0.8  # full torque for 20 deg at 80mph means 0.00007818594
       if ret.enableGasInterceptor:
-        with open('/data/gasint', 'a') as f:
-          f.write('True\n')
+        # with open('/data/gasint', 'a') as f:
+        #   f.write('True\n')
         ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42]
         ret.longitudinalTuning.kiV = [0.135, 0.09]