diff --git a/.media/photos/df_profiles.jpg b/.media/photos/df_profiles.jpg
new file mode 100644
index 00000000000000..8286ffa53fc077
Binary files /dev/null and b/.media/photos/df_profiles.jpg differ
diff --git a/.media/photos/df_profiles.png b/.media/photos/df_profiles.png
deleted file mode 100644
index aaa0b4fb05c139..00000000000000
Binary files a/.media/photos/df_profiles.png and /dev/null differ
diff --git a/README.md b/README.md
index 63a8409bacc639..ef7ca6e3d8d882 100644
--- a/README.md
+++ b/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.
-
+
**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)
-----
diff --git a/selfdrive/controls/lib/dg_test.py b/selfdrive/controls/lib/dg_test.py
new file mode 100644
index 00000000000000..62289eaa3f728d
--- /dev/null
+++ b/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)
diff --git a/selfdrive/controls/lib/dynamic_gas.py b/selfdrive/controls/lib/dynamic_gas.py
index 099aba6ed5fb5d..3c1ca456fb3cb0 100644
--- a/selfdrive/controls/lib/dynamic_gas.py
+++ b/selfdrive/controls/lib/dynamic_gas.py
@@ -15,9 +15,7 @@ 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]
@@ -25,8 +23,6 @@ def update(self, v_ego, lead_data, mpc_TR, blinker_status):
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:
@@ -34,9 +30,11 @@ def update(self, v_ego, lead_data, mpc_TR, blinker_status):
# 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)