Dynamic gas improvements (#86)

* add stock dg

* updates

* updates to dynamic gas. now factors in lead distance. less reduction for lead accel

* tmp

* no dg for cars without pedal unless there's a custom tune

* update README.md

* update README.md

* update README.md

* add new profiles

* add new profiles

* test

* Update README.md

* test

* test

README.md

@@ -1,4 +1,4 @@
-Shane's Stock Additions 0.7.4
+Stock Additions 0.7.4
 =====
 
 This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla w/ comma pedal.
@@ -27,24 +27,31 @@ Dynamic follow (3 profiles)
 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) The goal is to essentially make the driving experience more smooth and increase safety, braking and accelerating sooner.
 
 Now you can choose a profile based on traffic and your driving preference. There are three profiles currently:
-  * `traffic` - Meant to keep you a bit closer in traffic, hopefully reducing cut-ins. May not be the safest when approaching a vastly slower vehicle.
+  * `traffic` - Meant to keep you a bit closer in traffic, hopefully reducing cut-ins. Use with caution, as you do with any fork adding custom functionality.
   * `relaxed` - This is the current and now default dynamic follow profile just with a cool name. Also slight closer than previously at high speeds.
   * `roadtrip` - This profile is for road trips mainly where you're on two lane highways and don't want to be following particularly closely; at night for example.
-<img src=".media/photos/df_profiles.png?raw=true" width="800">
+<img src=".media/photos/df_profiles.jpg?raw=true" height="350">
 
 **How to choose a profile:** The easiest way is to use the new on-screen profile changing button! Once you're on a drive, you can simply tap the button on the bottom right of the screen to cycle between the profiles.
 
 Dynamic gas
 -----
-Currently supported vehicles (w/ comma pedal only):
+Dynamic gas aims to provide a smoother driving experience in stop and go traffic (under 20 mph) by reducing the maximum gas that can be applied based on your current velocity, the relative velocity of the lead, the acceleration of the lead, and the distance of the lead. This usually results in quicker and smoother acceleration from a standstill without the jerking you get in stock openpilot with comma pedal (ex. taking off from a traffic light). It tries to coast if the lead is just inching up, it doesn’t use maximum gas as soon as the lead inches forward. When you are above 20 mph, relative velocity and the current following distance in seconds is taken into consideration.
+
+All cars that have a comma pedal are supported! However to get the smoothest acceleration, I've custom tuned gas curve profiles for the following cars:
+
+pedal cars:
   * 2017 Toyota Corolla (non-TSS2)
   * Toyota RAV4 (non-TSS2)
   * 2017 Honda Civic
   * 2019 Honda Pilot
 
-*TODO: Need to factor in distance, as it will not accelerate to get closer to the stopped lead if you engage at ~0mph far back from the lead.*
+non-pedal cars:
+  * None yet
+
+If you have a car without a pedal, or you do have one but I haven't created a profile for you yet, please let me know and we can develop one for your car to test.
 
-This aims to provide a smoother driving experience in stop and go traffic (under 20 mph) 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. It tries to coast if the lead is only moving slowly, it doesn't use maximum gas as soon as the lead inches forward :). When you are above 20 mph, relative velocity and the following distance is taken into consideration.
+~~*TODO: Need to factor in distance, as it will not accelerate to get closer to the stopped lead if you engage at ~0mph far back from the lead.*~~ Update: Added lead distance into dynamic gas, now just need to tune and test.
 
 Customize this branch (opEdit Parameter class)
 -----

selfdrive/controls/lib/dg_test.py

@@ -0,0 +1,81 @@
+# from selfdrive.car.toyota.values import CAR as CAR_TOYOTA
+# from selfdrive.car.honda.values import CAR as CAR_HONDA
+from common.numpy_fast import clip, interp
+from selfdrive.config import Conversions as CV
+CAR_TOYOTA = ['Corolla']
+CAR_HONDA = []
+
+class CarParams:
+  enableGasInterceptor = True
+  gasMaxBP = [0., 9., 35]
+  gasMaxV = [0.2, 0.5, 0.7]
+
+class DynamicGas:
+  def __init__(self, CP, candidate):
+    self.toyota_candidates = [attr for attr in dir(CAR_TOYOTA) if not attr.startswith("__")]
+    self.honda_candidates = [attr for attr in dir(CAR_HONDA) if not attr.startswith("__")]
+
+    self.candidate = candidate
+    self.CP = CP
+
+  def update(self, v_ego, lead_data, mpc_TR, blinker_status):
+    x, y = self.CP.gasMaxBP, self.CP.gasMaxV
+    if self.CP.enableGasInterceptor:
+      if self.candidate == "Corolla":
+        x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2447, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.0581, 32.7101, 35.7633]
+        y = [0.215, 0.219, 0.23, 0.247, 0.271, 0.292, 0.335, 0.361, 0.38, 0.388, 0.396, 0.41, 0.421, 0.46, 0.513, 0.566, 0.624]
+
+    if not x:  # if unsupported car, don't use dynamic gas
+      # x, y = CP.gasMaxBP, CP.gasMaxV  # if unsupported car, use stock.
+      return interp(v_ego, self.CP.gasMaxBP, self.CP.gasMaxV)
+    gas = interp(v_ego, x, y)
+    print('Original gas: {}'.format(round(gas, 4)))
+
+    if lead_data['status']:  # if lead
+      if v_ego <= 8.9408:  # if under 20 mph
+        x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481]  # relative velocity mod
+        y = [gas * 0.9901, gas * 0.905, gas * 0.8045, gas * 0.625, gas * 0.431, gas * 0.2083, gas * .0667, 0]
+        gas_mod = -interp(lead_data['v_rel'], x, y)
+
+        x = [0.44704, 1.1176, 1.34112]  # lead accel mod
+        y = [1.0, 0.85, 0.75]  # maximum we can reduce gas_mod is 40 percent (never increases mod)
+        gas_mod *= interp(lead_data['a_lead'], x, y)
+
+        x = [0.6096, 3.048, 8, 10, 12]  # as lead gets further from car, lessen gas mod
+        y = [1.0, 0.9, 0.65, 0.3, 0]
+        gas_mod *= interp(lead_data['x_lead'], x, y)
+
+        print('Gas mod: {}'.format(round(gas_mod, 4)))
+        new_gas = gas + gas_mod
+        print('New gas: {}'.format(round(new_gas, 4)))
+
+        x = [1.78816, 6.0, 8.9408]  # slowly ramp mods down as we approach 20 mph
+        y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas]
+        gas = interp(v_ego, x, y)
+      else:
+        x = [-3.12928, -1.78816, -0.89408, 0, 0.33528, 1.78816, 2.68224]  # relative velocity mod
+        y = [-gas * 0.2625, -gas * 0.18, -gas * 0.12, 0.0, gas * 0.075, gas * 0.135, gas * 0.19]
+        gas_mod = interp(lead_data['v_rel'], x, y)
+
+        current_TR = lead_data['x_lead'] / v_ego
+        x = [mpc_TR - 0.22, mpc_TR, mpc_TR + 0.2, mpc_TR + 0.4]
+        y = [-gas_mod * 0.15, 0.0, gas_mod * 0.25, gas_mod * 0.4]
+        gas_mod -= interp(current_TR, x, y)
+
+        gas += gas_mod
+
+        if blinker_status:
+          x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
+          y = [1.0, 1.275, 1.45]
+          gas *= interp(v_ego, x, y)
+
+    return clip(gas, 0.0, 1.0)
+
+
+CP = CarParams()
+dg = DynamicGas(CP, 'Corolla')
+
+lead_data = {'status': True, 'a_lead': 1.1, 'v_rel': -10, 'x_lead': 20}
+v_ego = 2.5 * CV.MPH_TO_MS
+mpc_TR = 1.9
+o = dg.update(v_ego, lead_data, mpc_TR, False)

selfdrive/controls/lib/dynamic_gas.py

@@ -15,28 +15,26 @@ def update(self, v_ego, lead_data, mpc_TR, blinker_status):
     x, y = [], []  # gasMaxBP, gasMaxV
     if self.CP.enableGasInterceptor:
       if self.candidate == CAR_TOYOTA.COROLLA:
-        # x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326]  # stock tune
-        # 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]
-        # x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2446, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.058, 32.7102, 35.7633]  # todo: test higher only at low speeds
+        # x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2446, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.058, 32.7102, 35.7633]  # todo: average this and below
         # y = [0.221, 0.225, 0.236, 0.253, 0.275, 0.296, 0.338, 0.363, 0.38, 0.389, 0.4, 0.41, 0.421, 0.458, 0.51, 0.561, 0.617]
         x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2447, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.0581, 32.7101, 35.7633]
         y = [0.215, 0.219, 0.23, 0.247, 0.271, 0.292, 0.335, 0.361, 0.38, 0.388, 0.396, 0.41, 0.421, 0.46, 0.513, 0.566, 0.624]
       elif self.candidate == CAR_TOYOTA.RAV4:
         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.234, 0.237, 0.246, 0.26, 0.279, 0.297, 0.332, 0.354, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617]
       elif self.candidate in [CAR_HONDA.PILOT_2019, CAR_HONDA.CIVIC]:
-        # x = [0.0, 1.4082, 2.8031, 4.2266, 5.3827, 6.1656, 7.2478, 8.2831, 10.2447, 12.964, 15.423, 18.119, 20.117, 24.4661, 29.0581, 32.7101, 35.7633]
-        # y = [0.253, 0.256, 0.265, 0.278, 0.296, 0.313, 0.346, 0.367, 0.38, 0.389, 0.4, 0.41, 0.421, 0.458, 0.51, 0.561, 0.617]
         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.234, 0.237, 0.246, 0.26, 0.279, 0.297, 0.332, 0.354, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617]
     # else:
     #   if self.candidate in [CAR_TOYOTA.CAMRY, CAR_TOYOTA.CAMRYH]:
     #     x = [0.]
     #     y = [0.5]
 
-    if not x:  # if unsupported car, don't use dynamic gas
-      # x, y = CP.gasMaxBP, CP.gasMaxV  # if unsupported car, use stock.
-      return interp(v_ego, self.CP.gasMaxBP, self.CP.gasMaxV)
+    if not x:
+      if self.CP.enableGasInterceptor:
+        x, y = self.CP.gasMaxBP, self.CP.gasMaxV  # if no custom gas profile, use stock gas values
+      else:  # else if no gas profile and user doesn't have pedal, no dynamic gas
+        return interp(v_ego, self.CP.gasMaxBP, self.CP.gasMaxV)
 
     gas = interp(v_ego, x, y)
 
@@ -46,13 +44,14 @@ def update(self, v_ego, lead_data, mpc_TR, blinker_status):
         y = [gas * 0.9901, gas * 0.905, gas * 0.8045, gas * 0.625, gas * 0.431, gas * 0.2083, gas * .0667, 0]
         gas_mod = -interp(lead_data['v_rel'], x, y)
 
-        x = [0.44704, 1.78816]  # lead accel mod
-        y = [0.0, gas_mod * .4]  # maximum we can reduce gas_mod is 40 percent of it
-        gas_mod -= interp(lead_data['a_lead'], x, y)  # reduce the reduction of the above mod (the max this will ouput is the original gas value, it never increases it)
+        x = [0.44704, 1.1176, 1.34112]  # lead accel mod
+        y = [1.0, 0.85, 0.75]  # maximum we can reduce gas_mod is 40 percent (never increases mod)
+        gas_mod *= interp(lead_data['a_lead'], x, y)
+
+        x = [0.6096, 3.048, 8, 10, 12]  # as lead gets further from car, lessen gas mod
+        y = [1.0, 0.9, 0.65, 0.3, 0]
+        gas_mod *= interp(lead_data['x_lead'], x, y)
 
-        # x = [TR * 0.5, TR, TR * 1.5]  # as lead gets further from car, lessen gas mod  # todo: this
-        # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5]
-        # gas_mod += (interp(current_TR, x, y))
         new_gas = gas + gas_mod
 
         x = [1.78816, 6.0, 8.9408]  # slowly ramp mods down as we approach 20 mph
@@ -72,7 +71,7 @@ def update(self, v_ego, lead_data, mpc_TR, blinker_status):
 
         if blinker_status:
           x = [8.9408, 22.352, 31.2928]  # 20, 50, 70 mph
-          y = [1.0, 1.275, 1.45]
+          y = [1.0, 1.3, 1.475]
           gas *= interp(v_ego, x, y)
 
     return clip(gas, 0.0, 1.0)