From c997eec8340667a863b53d950d885ee14f5d515d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 29 Jul 2020 14:36:48 -0500 Subject: [PATCH 1/7] add relative accel back with slightly new tuning --- selfdrive/controls/lib/dynamic_follow/__init__.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index d2e7563220678d..f39ac35880bc28 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -195,13 +195,13 @@ def _relative_accel_mod(self): a_ego = (self.df_data.v_rels[-1]['v_ego'] - self.df_data.v_rels[0]['v_ego']) / elapsed_time a_lead = (self.df_data.v_rels[-1]['v_lead'] - self.df_data.v_rels[0]['v_lead']) / elapsed_time - mods_x = [0, -.75, -1.5] - mods_y = [1.5, 1.25, 1] + mods_x = [-1.5, -.75, 0] + mods_y = [1, 1.5, 1.25] if a_lead < 0: # more weight to slight lead decel a_lead *= interp(a_lead, mods_x, mods_y) rel_x = [-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447, 0.8941, 1.7882, 2.6822] - mod_y = [0.3245 * 1.25, 0.277 * 1.2, 0.11075 * 1.15, 0.08106 * 1.075, 0.06325 * 1.05, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35] + mod_y = [0.3245 * 1.1, 0.277 * 1.08, 0.11075 * 1.06, 0.08106 * 1.045, 0.06325 * 1.035, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35] return interp(a_lead - a_ego, rel_x, mod_y) def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist): @@ -284,9 +284,9 @@ def _get_TR(self): y = [0.24, 0.16, 0.092, 0.0515, 0.0305, 0.022, 0.0, -0.0153, -0.042, -0.053, -0.059] # modification values TR_mods.append(interp(self.lead_data.a_lead, x, y)) - # deadzone = 7.5 * CV.MPH_TO_MS - # if self.lead_data.v_lead - deadzone > self.car_data.v_ego: - # TR_mods.append(self._relative_accel_mod()) + deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph + 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] From 3cf604bdb55055948d64ccb04433e10d1fe38ada Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 29 Jul 2020 14:39:04 -0500 Subject: [PATCH 2/7] change alert times --- selfdrive/controls/lib/events.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 6dfaa80eb11167..8ba598d3e0d93c 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -211,7 +211,7 @@ def wrong_car_mode_alert(CP, sm, metric): "Model longitudinal ", "Remain alert", AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 2.), + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 1.5), }, 'dfButtonAlert': { @@ -219,7 +219,7 @@ def wrong_car_mode_alert(CP, sm, metric): "Using profile: ", "", AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 2.), + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 1.5), }, 'lsButtonAlert': { @@ -227,7 +227,7 @@ def wrong_car_mode_alert(CP, sm, metric): "Lane Speed set to: ", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 2.), + Priority.LOW, VisualAlert.none, AudibleAlert.chimeWarning1, .4, 0., 1.5), }, 'dfButtonAlertSilent': { @@ -235,7 +235,7 @@ def wrong_car_mode_alert(CP, sm, metric): "Dynamic follow: ", "", AlertStatus.normal, AlertSize.small, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, 0., 2.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, 0., 1.), }, 'laneSpeedAlert': { @@ -259,7 +259,7 @@ def wrong_car_mode_alert(CP, sm, metric): "KEEPING ", "", AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.2, 0., 0.1), + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2, 0., .1), }, EventName.debugAlert: { @@ -275,7 +275,7 @@ def wrong_car_mode_alert(CP, sm, metric): "Be ready to take over at any time", "Always keep hands on wheel and eyes on road", AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 10.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 7.5), }, EventName.startupWhitePanda: { @@ -291,7 +291,7 @@ def wrong_car_mode_alert(CP, sm, metric): "WARNING: This branch is not tested", "Always keep hands on wheel and eyes on road", AlertStatus.userPrompt, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 7.5), }, EventName.startupNoControl: { From 675ad14934fa5130c686a50a5fc348f9697cbe13 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 29 Jul 2020 17:18:01 -0500 Subject: [PATCH 3/7] update alert time and update dyn camera offset time --- common/op_params.py | 2 +- selfdrive/controls/lib/events.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/common/op_params.py b/common/op_params.py index 9e2d95615100a2..c256a7c0b0490d 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -62,7 +62,7 @@ def __init__(self): 'log_auto_df': {'default': False, 'allowed_types': [bool], 'description': 'Logs dynamic follow data for auto-df'}, 'dynamic_camera_offset': {'default': True, 'allowed_types': [bool], 'description': 'Whether to automatically keep away from oncoming traffic.\n' 'Works from 35 to ~60 mph (requires radar)'}, - 'dynamic_camera_offset_time': {'default': 2.5, 'allowed_types': [int, float], 'description': 'How long to keep away from oncoming traffic in seconds after losing lead'}, + 'dynamic_camera_offset_time': {'default': 3.5, 'allowed_types': [int, float], 'description': 'How long to keep away from oncoming traffic in seconds after losing lead'}, 'support_white_panda': {'default': False, 'allowed_types': [bool], 'description': 'Enable this to allow engagement with the deprecated white panda.\n' 'localizer might not work correctly'}, 'prius_use_lqr': {'default': False, 'allowed_types': [bool], 'description': 'If you have a newer Prius with a good angle sensor, you can try enabling this to use LQR'}, diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 8ba598d3e0d93c..f4cfb5b2afe094 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -235,7 +235,7 @@ def wrong_car_mode_alert(CP, sm, metric): "Dynamic follow: ", "", AlertStatus.normal, AlertSize.small, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, 0., 1.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, 0., 1.5), }, 'laneSpeedAlert': { From 4bf937287a07fac4acff23a065da3cbcbcfa0de8 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 29 Jul 2020 17:23:11 -0500 Subject: [PATCH 4/7] rel accel mod: only add distance --- selfdrive/controls/lib/dynamic_follow/__init__.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index f39ac35880bc28..8bb265b8582f77 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -202,7 +202,10 @@ def _relative_accel_mod(self): rel_x = [-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447, 0.8941, 1.7882, 2.6822] mod_y = [0.3245 * 1.1, 0.277 * 1.08, 0.11075 * 1.06, 0.08106 * 1.045, 0.06325 * 1.035, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35] - return interp(a_lead - a_ego, rel_x, mod_y) + mod = interp(a_lead - a_ego, rel_x, mod_y) + if mod > 0: # temp: return only if adding distance + return mod + return None def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist): """ @@ -286,7 +289,9 @@ def _get_TR(self): deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph if self.lead_data.v_lead - deadzone > self.car_data.v_ego: - TR_mods.append(self._relative_accel_mod()) + rel_accel_mod = self._relative_accel_mod() + if rel_accel_mod is not None: + TR_mods.append(rel_accel_mod) x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance y = [1.0, 1.05] From 8d2b4c2677cfa997c70da88be2eada9acb84b42d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 2 Aug 2020 12:00:33 -0500 Subject: [PATCH 5/7] rename to more representative name --- selfdrive/controls/lib/dynamic_follow/__init__.py | 2 +- selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py | 2 +- selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 8bb265b8582f77..289d20df035e08 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -144,7 +144,7 @@ def _change_cost(self, libmpc): cost *= interp(cost_mod, cost_mod_speeds, cost_mods) if self.last_cost != cost: - libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + libmpc.change_costs(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) self.last_cost = cost def _store_df_data(self): diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index fd8947ff0131af..edc1fb740956d5 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -29,7 +29,7 @@ def _get_libmpc(mpc_id): void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); - void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); + void change_costs(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double TR); """) diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c index 02d8693e451f2f..16612103037c19 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -68,7 +68,7 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j } -void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ +void change_costs(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ int i; const int STEP_MULTIPLIER = 3; From 4cae7abd4b87ffc2d2dcb4f2dcf81114aad39984 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 2 Aug 2020 12:03:37 -0500 Subject: [PATCH 6/7] return 0 instead of None and just append it --- selfdrive/controls/lib/dynamic_follow/__init__.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 289d20df035e08..b9f303284ef29b 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -200,12 +200,12 @@ def _relative_accel_mod(self): if a_lead < 0: # more weight to slight lead decel a_lead *= interp(a_lead, mods_x, mods_y) + if a_lead - a_ego > 0: # return only if adding distance + return 0 + rel_x = [-2.6822, -1.7882, -0.8941, -0.447, -0.2235, 0.0, 0.2235, 0.447, 0.8941, 1.7882, 2.6822] mod_y = [0.3245 * 1.1, 0.277 * 1.08, 0.11075 * 1.06, 0.08106 * 1.045, 0.06325 * 1.035, 0.0, -0.09, -0.09375, -0.125, -0.3, -0.35] - mod = interp(a_lead - a_ego, rel_x, mod_y) - if mod > 0: # temp: return only if adding distance - return mod - return None + return interp(a_lead - a_ego, rel_x, mod_y) def global_profile_mod(self, profile_mod_x, profile_mod_pos, profile_mod_neg, x_vel, y_dist): """ @@ -289,9 +289,7 @@ def _get_TR(self): deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph if self.lead_data.v_lead - deadzone > self.car_data.v_ego: - rel_accel_mod = self._relative_accel_mod() - if rel_accel_mod is not None: - TR_mods.append(rel_accel_mod) + 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] From 39e470db070d0ab379406fc30ec55fa1022c331c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 2 Aug 2020 12:06:54 -0500 Subject: [PATCH 7/7] slightly higher TTC cost --- selfdrive/controls/lib/drive_helpers.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index ea7002475f0976..2e5295920ffcc0 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -17,7 +17,7 @@ class MPC_COST_LAT: class MPC_COST_LONG: - TTC = 5.0 + TTC = 6.5 DISTANCE = 0.1 ACCELERATION = 10.0 JERK = 20.0