diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index e3574012a17baa..8a47ec9e44a05d 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, DT_MDL +from common.realtime import sec_since_boot 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,6 +17,8 @@ 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 @@ -34,7 +36,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 * DT_MDL * self._k_i + self.i += error * self._rate * 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 @@ -48,7 +50,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 / DT_MDL) + self.i -= sign * max(self._to_clip) / (reset_time / self._rate) class DynamicFollow: @@ -66,10 +68,11 @@ 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 / DT_MDL) - self.model_input_len = round(45 / DT_MDL) + self.skip_every = round(0.25 / mpc_rate) + self.model_input_len = round(45 / mpc_rate) # Dynamic follow variables self.default_TR = 1.8 @@ -287,6 +290,10 @@ 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 @@ -299,43 +306,40 @@ 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 = [] - # # 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