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 diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index d2e7563220678d..b9f303284ef29b 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): @@ -195,13 +195,16 @@ 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) + 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.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 +287,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] diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 6dfaa80eb11167..f4cfb5b2afe094 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.5), }, '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: { 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;