From be4c7bacb4666928a0732149a6948de6a4e49f03 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 9 Nov 2020 19:14:54 -0600 Subject: [PATCH] Slowdown for curves (#274) * Update accel limits * not as much deceleration * Display slowdown img when either model-related solution are slowing down * tone down deceleration * fix missing var --- SA_RELEASES.md | 1 + cereal/log.capnp | 1 + selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/planner.py | 35 ++++++++++++++++++++++++++++--- 4 files changed, 35 insertions(+), 4 deletions(-) diff --git a/SA_RELEASES.md b/SA_RELEASES.md index 2dc1378ec20d8e..e051f2e35b1929 100644 --- a/SA_RELEASES.md +++ b/SA_RELEASES.md @@ -5,6 +5,7 @@ Stock Additions v0.6 - 2020-11-09 (0.7.10) * Raise max limit of global_df_mod to 1.5x * Continued white panda support (untested, grey gives no warning) * Reduced DM uncertain alert at night with EON + * Add slow down for curves from openpilot 0.6.3 Stock Additions v0.5.4 - 2020-10-15 (0.7.7) === diff --git a/cereal/log.capnp b/cereal/log.capnp index 8fce0af4309e2e..d1c8c0f9aa3a3d 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -840,6 +840,7 @@ struct Plan { mpc3 @3; model @4; dynamicSpeed @5; + curveSlowdown @6; } } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d9f6a2cb7c11a2..72ba910c1a1ad8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -572,7 +572,7 @@ def publish_logs(self, CS, start_time, actuators, v_acc, a_acc, lac_log): controlsState.jerkFactor = float(self.sm['plan'].jerkFactor) controlsState.gpsPlannerActive = self.sm['plan'].gpsPlannerActive controlsState.vCurvature = self.sm['plan'].vCurvature - controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource == LongitudinalPlanSource.model + controlsState.decelForModel = self.sm['plan'].longitudinalPlanSource in [LongitudinalPlanSource.model, LongitudinalPlanSource.curveSlowdown] controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.mapValid = self.sm['plan'].mapValid diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index ac68a71a88d382..ea135474b967f3 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -26,12 +26,12 @@ # need fast accel at very low speed for stop and go # make sure these accelerations are smaller than mpc limits -_A_CRUISE_MAX_V = [1.55, 1.4, 0.7, .4] +_A_CRUISE_MAX_V = [1.6, 1.4, 0.7, .4] _A_CRUISE_MAX_V_FOLLOWING = [1.7, 1.65, 0.7, .5] _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] # Lookup table for turns -_A_TOTAL_MAX_V = [2.2, 4.35] +_A_TOTAL_MAX_V = [2.2, 4.15] _A_TOTAL_MAX_BP = [20., 40.] @@ -74,6 +74,8 @@ def __init__(self, CP): self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 + self.v_model = 0.0 + self.a_model = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() @@ -85,7 +87,7 @@ def __init__(self, CP): def choose_solution(self, v_cruise_setpoint, enabled, model_enabled): possible_futures = [self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint] if enabled: - solutions = {'cruise': self.v_cruise} + solutions = {'cruise': self.v_cruise, 'curveSlowdown': self.v_model} if self.mpc1.prev_lead_status: solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: @@ -110,6 +112,9 @@ def choose_solution(self, v_cruise_setpoint, enabled, model_enabled): elif slowest == 'model': self.v_acc = self.mpc_model.v_mpc self.a_acc = self.mpc_model.a_mpc + elif slowest == 'curveSlowdown': + self.v_acc = self.v_model + self.a_acc = self.a_model self.v_acc_future = min(possible_futures) @@ -131,6 +136,24 @@ def update(self, sm, pm, CP, VM, PP): enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping) following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0 + if len(sm['model'].path.poly): # old slowdown for curves code + path = list(sm['model'].path.poly) + + # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function + # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b + # k = y'' / (1 + y'^2)^1.5 + # TODO: compute max speed without using a list of points and without numpy + y_p = 3 * path[0] * self.path_x ** 2 + 2 * path[1] * self.path_x + path[2] + y_pp = 6 * path[0] * self.path_x + 2 * path[1] + curv = y_pp / (1. + y_p ** 2) ** 1.5 + + a_y_max = 3.1 - v_ego * 0.032 + v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) + model_speed = np.min(v_curvature) + model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph + else: + model_speed = 255. # (MAX_SPEED) + # Calculate speed for normal cruise control if enabled and not self.first_loop and not sm['carState'].gasPressed: accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] @@ -148,6 +171,12 @@ def update(self, sm, pm, CP, VM, PP): jerk_limits[1], jerk_limits[0], LON_MPC_STEP) + self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start, + model_speed, + 2 * accel_limits[1], accel_limits[0], + 2 * jerk_limits[1], jerk_limits[0], + LON_MPC_STEP) + # cruise speed can't be negative even is user is distracted self.v_cruise = max(self.v_cruise, 0.) else: