From 9da113316adc9537f2dce5f3d1d71c13b42cc584 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 12 Oct 2021 01:37:44 -0500 Subject: [PATCH 1/3] firstly, this should have been second --- selfdrive/controls/lib/dynamic_follow/__init__.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 8a47ec9e44a05d..611a61fcfbee19 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -290,10 +290,6 @@ def _get_TR(self): v_rel_dist_factor = self.dmc_v_rel.update(self.lead_data.v_lead - self.car_data.v_ego) a_lead_dist_factor = self.dmc_a_rel.update(self.lead_data.a_lead - self.car_data.a_ego) - TR = interp(self.car_data.v_ego, x_vel, y_dist) - TR *= v_rel_dist_factor - TR *= a_lead_dist_factor - if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again self.sng = False @@ -306,6 +302,9 @@ def _get_TR(self): y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)] TR = interp(self.car_data.v_ego, x, y) + TR *= v_rel_dist_factor + TR *= a_lead_dist_factor + return float(clip(TR, self.min_TR, 2.7)) TR_mods = [] From 40fae184d7eb6f8204ded2f359530d8cb911f0ce Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 12 Oct 2021 01:40:13 -0500 Subject: [PATCH 2/3] comment --- .../controls/lib/dynamic_follow/__init__.py | 64 +++++++++---------- 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 611a61fcfbee19..70d3694ea8f2ec 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -307,38 +307,38 @@ def _get_TR(self): return float(clip(TR, self.min_TR, 2.7)) - TR_mods = [] - # Dynamic follow modifications (the secret sauce) - x = [-26, -15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68, 4.4704] # relative velocity values - y = [1.76, 1.504, 1.34, 1.29, 1.25, 1.22, 1.19, 1.13, 1.053, 1.017, 1.0, 0.985, 0.958, 0.87, 0.81, 0.685] # multiplier values - y = np.array(y) - 1 # converts back to original abs mod - y *= 1.1 # multiplier for how much to mod - y = y / TR + 1 # converts back to multipliers - TR_mods.append(interp(self.lead_data.v_lead - self.car_data.v_ego, x, y)) - - x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values - y = [1.16, 1.1067, 1.0613, 1.0343, 1.0203, 1.0147, 1.0, 0.9898, 0.972, 0.9647, 0.9607] # multiplier values - converted_with_TR = 1.5 # todo: do without numpy and simplify by converting with TR of 1, so only subtract - absolute_y_TR_mod = np.array(y) * converted_with_TR - converted_with_TR # converts back to original abs mod - absolute_y_TR_mod *= 1.2 # multiplier for how much to mod - y = absolute_y_TR_mod / TR + 1 # converts back to multipliers with accel mod of 1.4 taking current TR into account - TR_mods.append(interp(self.get_rel_accel(), x, y)) # todo: make this over more than 1 sec - - # deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this - # if self.lead_data.v_lead - deadzone > self.car_data.v_ego: - # TR_mods.append(self._relative_accel_mod()) - - # x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance - # y = [1.0, 1.05] - - TR *= mean(TR_mods) # with mods as multipliers, profile mods shouldn't be needed - - # if (self.car_data.left_blinker or self.car_data.right_blinker) and df_profile != self.df_profiles.traffic: - # x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph - # y = [1.0, .75, .65] - # TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes - - return float(clip(TR, self.min_TR, 2.7)) + # TR_mods = [] + # # Dynamic follow modifications (the secret sauce) + # x = [-26, -15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68, 4.4704] # relative velocity values + # y = [1.76, 1.504, 1.34, 1.29, 1.25, 1.22, 1.19, 1.13, 1.053, 1.017, 1.0, 0.985, 0.958, 0.87, 0.81, 0.685] # multiplier values + # y = np.array(y) - 1 # converts back to original abs mod + # y *= 1.1 # multiplier for how much to mod + # y = y / TR + 1 # converts back to multipliers + # TR_mods.append(interp(self.lead_data.v_lead - self.car_data.v_ego, x, y)) + # + # x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values + # y = [1.16, 1.1067, 1.0613, 1.0343, 1.0203, 1.0147, 1.0, 0.9898, 0.972, 0.9647, 0.9607] # multiplier values + # converted_with_TR = 1.5 # todo: do without numpy and simplify by converting with TR of 1, so only subtract + # absolute_y_TR_mod = np.array(y) * converted_with_TR - converted_with_TR # converts back to original abs mod + # absolute_y_TR_mod *= 1.2 # multiplier for how much to mod + # y = absolute_y_TR_mod / TR + 1 # converts back to multipliers with accel mod of 1.4 taking current TR into account + # TR_mods.append(interp(self.get_rel_accel(), x, y)) # todo: make this over more than 1 sec + # + # # deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this + # # if self.lead_data.v_lead - deadzone > self.car_data.v_ego: + # # TR_mods.append(self._relative_accel_mod()) + # + # # x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance + # # y = [1.0, 1.05] + # + # TR *= mean(TR_mods) # with mods as multipliers, profile mods shouldn't be needed + # + # # if (self.car_data.left_blinker or self.car_data.right_blinker) and df_profile != self.df_profiles.traffic: + # # x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + # # y = [1.0, .75, .65] + # # TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes + # + # return float(clip(TR, self.min_TR, 2.7)) def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False): self.lead_data.v_lead = v_lead From a4d80d78ed917d4d9532d4359b475d875d5223b6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 12 Oct 2021 01:42:35 -0500 Subject: [PATCH 3/3] use DT_MDL --- selfdrive/controls/lib/dynamic_follow/__init__.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 70d3694ea8f2ec..e3574012a17baa 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -1,7 +1,7 @@ import math import numpy as np import cereal.messaging as messaging -from common.realtime import sec_since_boot +from common.realtime import sec_since_boot, DT_MDL from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG from common.op_params import opParams from common.numpy_fast import interp, clip, mean @@ -17,8 +17,6 @@ class DistanceModController: def __init__(self, k_i, k_d, x_clip, mods): - self._rate = 1 / 20. - self._k_i = k_i self._k_d = k_d self._to_clip = x_clip # reaches this with v_rel=3.5 mph for 4 seconds @@ -36,7 +34,7 @@ def update(self, error): if (d := self._k_d * (error - self.last_error)) < 0: # only add if it will add distance self.i += d - self.i += error * self._rate * self._k_i + self.i += error * DT_MDL * self._k_i self.i = clip(self.i, self._to_clip[0], self._to_clip[-1]) # clip to reasonable range self._slow_reset() # slowly reset from max to 0 @@ -50,7 +48,7 @@ def _slow_reset(self): if abs(self.i) > 0.01: # oscillation starts around 0.006 reset_time = 15 # in x seconds i goes from max to 0 sign = 1 if self.i > 0 else -1 - self.i -= sign * max(self._to_clip) / (reset_time / self._rate) + self.i -= sign * max(self._to_clip) / (reset_time / DT_MDL) class DynamicFollow: @@ -68,11 +66,10 @@ def __init__(self, mpc_id): self.pm = None # Model variables - mpc_rate = 1 / 20. self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625]} self.predict_rate = 1 / 4. - self.skip_every = round(0.25 / mpc_rate) - self.model_input_len = round(45 / mpc_rate) + self.skip_every = round(0.25 / DT_MDL) + self.model_input_len = round(45 / DT_MDL) # Dynamic follow variables self.default_TR = 1.8