diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index ea87adae55..88c4d694aa 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -78,8 +78,6 @@ class Planner(): 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() @@ -90,7 +88,7 @@ class Planner(): def choose_solution(self, v_cruise_setpoint, enabled): if enabled: - solutions = {'model': self.v_model, 'cruise': self.v_cruise} + solutions = {'cruise': self.v_cruise} if self.mpc1.prev_lead_status: solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: @@ -109,9 +107,6 @@ class Planner(): elif slowest == 'cruise': self.v_acc = self.v_cruise self.a_acc = self.a_cruise - elif slowest == 'model': - self.v_acc = self.v_model - self.a_acc = self.a_model self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint]) @@ -133,24 +128,6 @@ class Planner(): 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): - 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 = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph - 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 = MAX_SPEED - # Calculate speed for normal cruise control if enabled and not self.first_loop: accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)] @@ -168,12 +145,6 @@ class Planner(): 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: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 47366239cf..c285bfb49c 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5abe4f99eec3633bc30fda47c272e0f19a840a29 \ No newline at end of file +0533f640ab27f7b5af57aa4ebf4a29200550b3e8 \ No newline at end of file